# Position Kalman Filter fails to track a constant-acceleration path

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 probably implemented it incorrectly. What does your covariance matrix look like at the end? Aug 6, 2019 at 11:38
• I'm using Simulink for the implementation so I hope the block diagram will suffice: i.sstatic.net/LfhBU.png. 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: i.sstatic.net/7vvp8.png Aug 6, 2019 at 16:32
• @jd9610, Any chance you share set of data? We'll be able to give you a full solution on your exact data.
– Royi
Aug 6, 2019 at 16:34
• Sure, I'd be happy to. Will the simulink file be good enough? I've uploaded it here. Aug 6, 2019 at 16:44
• You probably have a mismatch in the time step size. How is the solver configured in your simulink model settings? Aug 7, 2019 at 3:53