How do I choose the parameters of a Kalman filter?

Question

Suppose I want to track the position of a car in 2D. What I get as sensor data is my current position. So my state is $$\mathbf{x} = \begin{pmatrix}x\\y\\\dot{x}\\\dot{y}\end{pmatrix}$$

where $x \in \mathbb{R}$ is the position in m away from some predefined point, $\dot{x} \in \mathbb{R}$ is the velocity in m/s at starting time and $\ddot{x} \in \mathbb{R}$ is the acceleration in $m/s^2$. The measurements are

$$\mathbf{z} = \begin{pmatrix}x^{(M)}\\y^{(M)}\end{pmatrix}$$

What I get to choose is my acceleration at each time step $i$ (time steps have the length $t$):

$$u = \begin{pmatrix}\ddot{x}^{(u)}\\\ddot{y}^{(u)}\end{pmatrix}$$

As the Kalman filter is a linear filter, my state model is:

$$\mathbf{x}^{(P)} = A x + Bu$$

The measurement is dependent on the state, with some noise $v$:

$$\mathbf{z} = H \mathbf{x} + v$$

with $A \in \mathbb{R}^{4 \times 4}$, $H \in \mathbb{R}^{2 \times 4}$. As one can decompose the acceleration / speed in the directions and the equation for the new position is

$$\begin{align}x_{new}(t) &= x + \dot{x} t + 0.5 \ddot{x} t^2\\ y_{new}(t) &= y + \dot{y} t + 0.5 \ddot{y} t^2\\ \dot{x}_{new}(t) &= \dot{x} + \ddot{x} t\\ \dot{y}_{new}(t) &= \dot{y} + \ddot{y} t\end{align}$$

So given our state model, we get:

$$\mathbf{x}^{(P)} = \underbrace{\begin{pmatrix}1& 0 & t & 0\\ 0& 1 & 0 & t\\ 0& 0 & 1 & 0\\ 0& 0 & 0 & 1\end{pmatrix}}_{A_i} \mathbf{x} + \underbrace{\begin{pmatrix}0.5t^2 & 0\\ 0 & 0.5t^2\\ t & 0\\ 0 & t\end{pmatrix}}_{B_i} \cdot u_i$$

1. Is this so far a reasonable scenario / approach to the Kalman filter?

2. How do I choose the initial uncertainty covariance matrix $P_0 \in \mathbb{R}^{4 \times 4}$ / the initial state $\mathbf{x}$? I've heard that one mainly makes the matrix values "big" - whatever that means. For example, should it be a diagonal matrix $$P_0 = \begin{pmatrix}a_1 & 0 & 0 & 0\\ 0 & a_2 & 0 & 0\\ 0 & 0 & a_3 & 0\\ 0 & 0 & 0 & a_4\end{pmatrix}$$ for some $a \in \mathbb{R}^+$? For example, $a_1 = a_2 = 20000000$ as the earths diameter is about $40000\textrm{ km}$ and $a_3=a_4=90$ as going more than $324\textrm{ km/h}$ is never going to happen for a car?

   For the initial state parameter, I would wait two time steps: $$\mathbf{x}_0 = \begin{pmatrix}x^{(M)}_{-1}\\ y^{(M)}_{-1}\\ x^{(M)}_{-1} - x^{(M)}_{-2}\\ y^{(M)}_{-1} - y^{(M)}_{-2}\end{pmatrix}$$

Prediction step

The state prediction works as above: $$\mathbf{x}^{(P)}_{i+1} = A_i \mathbf{x}_{i} + B_i u_i$$

Covariance prediction:

$$P_{i+1}^{(P)} = A P_i A^T + Q \quad \text{with}\quad Q \in \mathbb{R}^{4 \times 4}. \tag{P}$$

3. Where do I get the process error covariance $Q$ from? Which properties does it have to have? I guess positive definite? What does this matrix mean?

Innovation step

Innovation, which compares the measurement with the prediction:

$$\tilde{y}_{i+1} = z_{i+1} - H \mathbf{x}^{(P)}_{i+1}$$

4. (solved): Where do I get the observation matrix $H \in \mathbb{R}^{2 \times 4}$ from? What does it mean?

EDIT:

I got it. In my example $$H = \begin{pmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{pmatrix},$$ as it encodes the relationship between the state and the measurement.

Innovation Covariance:

$$S_{i+1} = H P_{i+1}^{(P)} H^T + R$$

For the measurement error covariance $R \in \mathbb{R}^{2 \times 2}$ I have to know something about the way my sensors work. I guess this will usually be a diagonal matrix, as the sensors will be independent(?).

Kalman Gain:

$$K_{i+1} = P_{i+1}^{(P)} H^T S^{-1}_{i+1}$$

Now, finally the state and covariance update:

$$x_{i+1} = \mathbf{x}^{(P)}_{i+1} + K_{i+1} \tilde{y}$$ $$P_{i+1} = (I - K_{i+1} H) P_{i+1}^{(P)}$$

Sources:

• http://greg.czerniak.info/guides/kalman1/ (I found this while writing this question - it already answered a couple of my question, which I hopefully removed.)
• Various lectures at KIT

Answer 1

Is this so far a reasonable scenario / approach to the Kalman filter?

Answer 1: Yes, your model looks reasonable. You're treating the acceleration as constant, however. If it will change in your experiment, you should include that in your system error matrix $Q$.

How do I choose the initial uncertainty covariance matrix $P_0 \in \mathbb{R}^{4 \times 4}$ / the initial state $\mathbf{x}$?

Answer 2: $P_0$ is your initial state covariance. That expresses how much you know about the initial estimate of the state $\mathbf{x}_0$. If you have no idea, it's usual to set $$ P_0 = \sigma^2I_4 $$ where $\sigma^2$ is large. See, for example, this answer which sets $\sigma^2 = 1000$.

Where do I get the process error covariance $Q$ from? Which properties does it have to have? I guess positive definite? What does this matrix mean?

Answer 3: This matrix expresses your system error. The entries in this matrix stand for the covariance of the corresponding values in your system model. If you assume, for example, the acceleration as constant like you did, but it is going to change in the real world, then you might want to include the corresponding covariance here.

Aside from that, it's always a good idea to experiment with your $Q$ and $P$ matrices a lot. Making $Q$ larger will rely more on your live data, making $P$ larger will rely more on your prediction. Rarely a model is perfect without tuning.