# Kalman filter update returns an invalid covariance matrix?

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?

I think your computations are wrong. When I compute the Kalman gain matrix $K_k$ I get: $$K_k = \pmatrix{0.9950311 \\ 0.4993789}.$$
$$Q = \left( \begin{array}{cc} t^3/3 & t^2/2 \\ t^2/2 & t \end{array} \right) \sigma_a^2$$