8
$\begingroup$

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}$$

  1. 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}$$

  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:

$\endgroup$
3
  • $\begingroup$ martin-thoma.com/kalman-filter is an article I wrote based on this question and the answers. $\endgroup$ Jun 21, 2016 at 17:53
  • $\begingroup$ Please see our two papers in the December 2016 issue of the journal SADHANA attached herewith. Prof.M.R.Ananthasayanam. $\endgroup$ Jul 23, 2017 at 15:33
  • $\begingroup$ @ANANTHASAYANAM : I have converted your answer to a comment. It'd be good if you have an answer in those papers to provide links to them. $\endgroup$
    – Peter K.
    Jul 23, 2017 at 17:38

1 Answer 1

5
$\begingroup$

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.

$\endgroup$
6
  • $\begingroup$ I thought I could adjust the $u_i$ in each time step and thus adjust the acceleration in each step? $\endgroup$ Jun 20, 2016 at 11:36
  • 1
    $\begingroup$ Answer 2: I thought $R$ would already encode my measurement error and $P$ would encode the current uncertainty about the state? $\endgroup$ Jun 20, 2016 at 11:39
  • $\begingroup$ @MartinThoma 1: I thought you said you only had distance measurements? How are you getting the acceleration if you can't measure it? 2: You are correct. $P_0$ is your initial state covariance and $P_k$ is the current state covariance. In general, $P_k$ is independent of any measurements. As per your equation ($P$) [which I just tagged], unless $A$ or $Q$ are time-varying, $P$ will evolve independently of anything else happening in the system. $\endgroup$
    – Peter K.
    Jun 20, 2016 at 14:28
  • 2
    $\begingroup$ @Emiswelt: Your answer 2 seems incorrect to me. The question was How do I choose the initial uncertainty covariance matrix? which does not refer to the measurement error? $\endgroup$
    – Peter K.
    Jun 20, 2016 at 14:29
  • 2
    $\begingroup$ @Emiswelt : OK. I've edited. Let me know if it's OK. I have a slightly different take on point 3... which leads me back to re-thinking point 1... :-) I'll let it stew for a bit before I compose an answer. $\endgroup$
    – Peter K.
    Jun 20, 2016 at 21:34

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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