In the previous articles, we looked at objects falling under gravity. In those articles we asked the Kalman filter to try and estimate the position, velocity and acceleration from the readings. However, we know in advance what gravity is doing. So, why don't we instead tell the Kalman filter that the object is accelerating? That way, we can get an even more accurate filter.
We can actually use the exact same code to generate the data that we've been using so far.
Remember that the full state propagation equation is given by
\[ \hat{x}_k = \Phi_k \hat{x}_{k-1} + G_k u_{k-1} + K_k [z_k - H \Phi_k \hat{x}_{k-1} - H G_k u_{k-1}] \]So far, we've been ignoring \(G\), but this is our control term now. So, let's construct the state-space equation for falling with a control term.
\[ \begin{bmatrix} \dot{x} \\ \ddot{x} \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \end{bmatrix} + \begin{bmatrix} 0 \\ -1 \end{bmatrix} g \]We've already calculated for this system before that
\[\Phi_k = \begin{bmatrix} 1 & t \\ 0 & 1 \end{bmatrix} \]Now, it's trivial to see that \(u_{k-1}=g\) and that
\[ G = \begin{bmatrix} 0 \\ -1 \end{bmatrix} \]But, we need \(G_k\), which is given by
\[ G_k = \int^t_0 \Phi(\tau) G d \tau = \int^t_0 \begin{bmatrix} 1 & \tau \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ -1 \end{bmatrix} d \tau = \begin{bmatrix} -0.5 t^2 \\ -t \end{bmatrix} \]If you think about that term, you might have been able to derive it from intuition alone - it is, after all, just SUVAT written out in a funny way.
So, solving the equations is now really quite easy. For convenience, we define
\[ \tilde{x} = x^*_k - \hat{x}_{k-1} - \hat{\dot{x}}_{k-1} t + 0.5 g t^2 \]And so we have the solutions
\[\hat{x}_k = \hat{x}_{k-1} + \hat{\dot{x}}_{k-1} t - 0.5g t^2 + K_1 \tilde{x} \] \[ \hat{\dot{x}}_k = \hat{\dot{x}}_{k-1} - g t + K_2 \tilde{x} \]And this produces the following results - which look great! Don't forget to look at the companion repository for the full code!