0
$\begingroup$

I'm working on a simple Kalman filter that estimates the position, velocity, and acceleration of a point mass using position measurements. If you know the position of something, you should also know it's first and second derivatives, but I'm not sure how to get this state information to propagate backwards through the governing equations to the higher order terms. You could just numerically differentiate the measurements and treat that like separate measurements before it goes into the filter, but that doesn't feel like the right way for some reason.

Here's the model of the system I'm using. It's just one-dimensional motion of a point mass:

$\hat x =\begin{bmatrix}x\\v\\a\end{bmatrix}$ $F =\begin{bmatrix}0&1&0\\0&0&1\\0&0&0\end{bmatrix}$

$\hat x_{k+1}=(F\hat x_k)dt + \hat x_k$

$P_{k+1}=(FP_kF^T + \dot Q)dt + P_k$

This is the model of the sensor I'm using:

$H = \begin{bmatrix} 1&0&0\end{bmatrix}$

$z=Hx, \hat z = H \hat x$

These is my initial filter state and the update equations I'm using:

$\hat x_0 =\begin{bmatrix}0\\0\\0\end{bmatrix}$ $P_0 =\begin{bmatrix}0&0&0\\0&0&0\\0&0&0\end{bmatrix}$

$K'=P_kH^T(HP_kH^T+R)^{-1}$

$P_k'=P_k - K'HP_k$

$\hat x_k'= \hat x_k+K'(z_k-H \hat x_k)$

I'm using $\dot Q = \begin{bmatrix}0&0&0\\0&0&0\\0&0&1\end{bmatrix}$ to model the system so that uncertainty enters through the acceleration, then propagates to the rest of the state.

When I simulate these equations in Python with a constant position input, the position in the state settles on the right value, but the velocity and acceleration never change and their uncertainties just continually increase. Numerically differentiating the measurement and treating the results like separate measurements seems to work but it doesn't really feel right to treat one measurement like three. Is there a more elegant way to fix this? Thanks.

$\endgroup$
4
  • $\begingroup$ This would probably do better on the math site or the signal processing site. $\endgroup$
    – DanielSank
    Sep 14 '16 at 18:54
  • $\begingroup$ Your F matrix doesn't look right $\endgroup$
    – docscience
    Sep 14 '16 at 20:00
  • $\begingroup$ It completely decouples acceleration $\endgroup$
    – docscience
    Sep 14 '16 at 20:01
  • $\begingroup$ It's just x_dot = v and v_dot = a. There's nothing inside the model that tells me what a_dot should be. $\endgroup$ Sep 14 '16 at 20:03

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.