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$$
Is this so far a reasonable scenario / approach to the Kalman filter?
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}$$
- 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}$$
- (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