4
$\begingroup$

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?

$\endgroup$
4
$\begingroup$

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

$\endgroup$
0
$\begingroup$

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

$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.