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:

enter image description here 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}$


$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?

  • $\begingroup$ You probably implemented it incorrectly. What does your covariance matrix look like at the end? $\endgroup$
    – fibonatic
    Aug 6, 2019 at 11:38
  • $\begingroup$ 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 $\endgroup$
    – jd9610
    Aug 6, 2019 at 16:32
  • $\begingroup$ @jd9610, Any chance you share set of data? We'll be able to give you a full solution on your exact data. $\endgroup$
    – Royi
    Aug 6, 2019 at 16:34
  • $\begingroup$ Sure, I'd be happy to. Will the simulink file be good enough? I've uploaded it here. $\endgroup$
    – jd9610
    Aug 6, 2019 at 16:44
  • $\begingroup$ You probably have a mismatch in the time step size. How is the solver configured in your simulink model settings? $\endgroup$
    – fibonatic
    Aug 7, 2019 at 3:53


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.