kalman filter

wiki page
cmu tutorial

State Space Representation

state equation

$$x_k = F_k x_{k-1}+G_k u_k+v_k$$

where $x_k$ is the state vector, $u_k$ is the known input vector, $v_k$ is the unknown zero mean white process noise with covariance $E[v_k v_k’] = Q_k$

measurement equation

$$z_k = H_k x_k + w_k$$

where $w_k$ is the unknown zero mean white measurement noise with known covariance $E[w_k w_k’] = R_k$

example

consider an object falling under a constant gravitational field. Let $y(t)$ denote the height of the object. Then

$$\ddot y(t) = -g$$ $$\dot y(t) = \dot y(t_0) - g(t-t_0)$$ $$y(t) = y(t_0) + \dot y(t_0)(t-t_0) - \frac{g}{2}(t-t_0)$$

we can define the state as $x_k = [y(t), \dot y(t)]’$, and then

$$x_{k+1} = {\left[ \begin{matrix} 1 & 1 \\ 0 & 1 \\ \end{matrix} \right] } x_k + {\left[ \begin{matrix} 1 \\ 0 \\ \end{matrix} \right] } (-g) = Fx_k + G u$$

Assuming we observe the height of the ball directly. The measurement equation is

$$z_k = [1 \quad 0] x_k + w_k = H x_k + w_k$$

State Estimation

The estimates of the state:

• $\hat x_{k|k}$ :estimate of $x_k$ given measurements $z_k, z_{k-1},…$
• $\hat x_{k+1|k}$ :estimate of $x_{k+1}$ given measurements $z_k, z_{k-1},…$

The error covariance matrix of the state estimate:

• $P_{k|k}$ :covariance of $x_k$ given $z_k, z_{k-1},…$
• $P_{k+1|k}$ :covariance of $x_{k+1}$ given $z_k, z_{k-1},…$

State Estimation

1. we know $\hat x_{k|k},u_k,P_{k|k}$ and the new measurement $z_{k+1}$.
2. state prediction $\hat x_{k+1|k}=F_k \hat x_{k|k} + G_k u_k$
3. measurement prediction $\hat z_{k+1|k} = H_k \hat x_{k+1|k}$
4. measurement residual $v_{k+1} = z_{k+1} - \hat z_{k+1|k}$
5. updated state estimate $\hat x_{k+1|k+1}=\hat x_{k+1|k} + W_{k+1}v_{k+1}$, where $W_{k+1}$ is called the Kalman Gain defined below.

State Covariance Estimate

1. state prediction covariance $P_{k+1|k} = F_k P_{k|k} F_k’ + Q_k$
2. measurement prediction covariance $S_{k+1} = H_{k+1} P_{k+1|k}H_{k+1}’ + R_{k+1}$
3. filer kalman gain $W_{k+1} = P_{k+1|k} H_{k+1}’ S_{k+1}^{-1}$
4. updated state covariance $P_{k+1|k+1} = P_{k+1|k} - W_{k+1} S_{k+1} W_{k+1}’$

This process is independent of state, can be performed offline and given by:

$$P_{k+1|k} = F_k \{ P_{k|k-1} - P_{k|k-1}H_k'[H_k P_{k|k-1}H_k'+R_k]^{-1}H_kP_{k|k-1}\}$$

This is the Riccati equation and can be obtained from the Kalman filter equations above.