I am trying to work through a simple introduction to the Kalman Filter but I am hitting a brick wall. I want to track the position and velocity of a target but only measure (noisily) the position. My understanding is that this is the classic application of a KF but I can't work out the details.
I am using the notation of Welch and Bishop. Note that the results below are generated using a simple implementation I wrote myself so the errors may be a buggy implementation or incorrect usage (or both!?). This post is a little long but I wanted to show all the steps I have done to highlight exactly where I am going wrong.
Here is what I am doing. The state update equation
$$ x_k = A x_{k-1} $$
has
$$ A = \left( \begin{array}{cc} 1 & t \\ 0 & 1 \\ \end{array} \right)$$
where $t$ is the time step length.
The noise covariance matrix, Q, is given by
$$ Q = \left( \begin{array}{cc} t^4/4 & t^3/2 \\ t^3/2 & t \end{array} \right) \sigma_a^2$$
which comes from assuming a random constant acceleration is applied in the drift step.
The measurement model
$$ z = Hx $$ is specified by
$$ H = \left( \begin{array}{cc} 1 & 0 \end{array} \right) $$
Now, to cut a long story short, whenever I try and run this I end up with a state covariance matrix, $P_k$, that is not symmetric!?
Here are some firm numbers to give an example. If we initialise to
$$ x = \left( \begin{array}{c} 0 \\ 0 \end{array} \right) $$
$$ P = \left( \begin{array}{cc} 100 & 0 \\ 0 & 100 \end{array} \right) $$
i.e. stationary target at the origin, but with large uncertainty over position and velocity. Assume step length $t=1$ and $\sigma_a^2=1$ so that
$$ Q = \left( \begin{array}{cc} 0.25 & 0.5 \\ 0.5 & 1 \end{array} \right) $$
Let's say we make a measurement after the first time step of $z=1$ with error $\sigma_z^2 = 1$ so that the measurement covariance matrix is simply
$$ R = (1) $$
Working through the steps, the state prediction is
$$ x_k = A x_{k-1} $$
$$ x_k = \left( \begin{array}{c} 0 \\ 0 \end{array} \right) $$
and the state covariance prediction is
$$ P_k = \left( \begin{array}{cc} 200.25 & 100.5 \\ 100.5 & 101 \end{array} \right) $$
The Kalman gain, K, is given by
$$ K_k = P_k H^T (H P_k H^T + R)^{-1} $$
which I compute for this example as
$$ K_k = \left( \begin{array}{c} 0.990099 \\ 0 \end{array} \right) $$
this leads to a corrected state estimate of
$$ \hat{x}_k = x_k + K_k(z_k – Hx_k) $$
$$ \hat{x}_k = \left( \begin{array}{c} 0.990099 \\ 0 \end{array} \right) $$
Makes sense, we are almost completely believing the measurement since we started with a high degree of ignorance.
Okay, so far so good. But here is where is goes pear shaped. The corrected state covariance
$$ \hat{P}_k = (I – K_kH)P_k $$
$$ \hat{P}_k = (I – \left( \begin{array}{cc} 0.990099 & 0 \\ 0 & 0 \end{array} \right) ) P_k$$
$$ \hat{P}_k = \left( \begin{array}{cc} 0.00990099 & 0 \\ 0 & 1 \end{array} \right)\left( \begin{array}{cc} 200.25 & 100.5 \\ 100.5 & 101 \end{array} \right) $$
$$ \hat{P}_k = \left( \begin{array}{cc} 1.982673 & 0.9950495 \\ 100.5 & 101 \end{array} \right) $$
which is not symmetric and hence not a valid covariance matrix. What am I doing wrong here?
Best Answer
I think your computations are wrong. When I compute the Kalman gain matrix $K_k$ I get: $$ K_k = \pmatrix{0.9950311 \\ 0.4993789}.$$