Kalman filter update returns an invalid covariance matrix?

Question

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?

Answer 1

I think your computations are wrong. When I compute the Kalman gain matrix $K_k$ I get: $$ K_k = \pmatrix{0.9950311 \\ 0.4993789}.$$

Answer 2

Besides the possibility that your implementation may simply be wrong, as alluded to in the answer of F. Tussel, it is not uncommon to obtain invalid covariance matrices despite an analytically correct implementation. This can happen (and actually happens quite often) due to numerical inaccuracies. One way to prevent this is to use what's called the Joseph stabilized version of the covariance update equation*: $$ \hat{P}_k=(I−K_k H)P_k=(I-K_k H)P_k(I-K_k H)^T + K_k R_k K_k^T. $$ This update equation guarantees the SPD-ness of the resulting covariance matrix. For details, see, e.g., Dan Simon, Optimal State Estimation, p.129. (I highly recommend that book for anyone working with (linear or nonlinear) Kalman filters or smoothers. The theory is thoroughly presented, derivations are complete and understandable, and a huge number of topics of practical relevance is discussed, which is often glossed over in many other books on the subject.)

Answer 3

This video gives a good overview of a Kalman filter implementation for a similar scenario. The filter models position and velocity along two axes and uses a noise covariance matrix similar to yours.

While this noise covariance model should work, I had problems using it (sometimes I encountered nonsymmetric matrices). I found another tutorial that generates a similar noise covariance matrix that worked better for me:

$$Q = \left( \begin{array}{cc} t^3/3 & t^2/2 \\ t^2/2 & t \end{array} \right) \sigma_a^2$$

A explanation of this matrix is on page 17. The full derivation requires integration of a matrix exponential. I don't understand the derivation, but using this noise covariance matrix solved some of my problems.

edit: this question has more resources and a link to a textbook with very clear/thorough explanation of Q in the Kalman filter