I'm trying to build a discrete Kalman Filter that fuses accelerometer (acceleration) and GPS (position, velocity) measurements. However, I'm finding that my filter can't properly track a constant-acceleration path:
As seen in the top right graph, the difference between the filtered and actual x-positions has a ramping bias, and the variance of the residual noise also increases over time.
My implementation uses the following standard signal model:
$A = \begin{pmatrix} 1 & \Delta t \\ 0 & 1 \end{pmatrix}, \quad B = \begin{pmatrix} \frac{1}{2}\Delta t^{2}\\ \Delta t \end{pmatrix}, \quad C = \begin{pmatrix} 1 & 0\\ 0 & 1 \end{pmatrix} \quad D = \begin{pmatrix} 0\\0 \end{pmatrix}$
$x_{n+1}=Ax_{n}+Bu_{n}$
$y_{n} = Cx_{n}+Du_{n}$
Where $x = (r_x,v_x)^{T}$, the measurement $y=x$ uses the GPS position/velocity readings, $\Delta t$ is the sample time and the accelerometer reading is used as the control input $u$. My $Q$ and $R$ matrices are both constant, reflecting each sensor's minimum rated accuracy.
Is this poor tracking behavior to be expected, or is there something wrong with my implementation? I also understand that several other alternative signal models can be used for this problem - for example, using the acceleration as a observable state instead of a control input. I've tried implementing this model too, but the filtered result is even noisier than what's shown above. Is one of these models more suitable over the other, and why?
You can see the entries of the optimal covariance matrix in the bottom right - they're symmetric but not quite diagonal. The matrix norm (sqrt of the sum of squared entries) quickly decays to a small but nonzero value: https://i.stack.imgur.com/7vvp8.png
– jd9610 Aug 06 '19 at 16:32