CSE276A - Kalman Filter

Basis

What is Kalman Filter

An 1 dimensional example: We have two measurements $x_1$ and $x_2$ measuring a value. Each measurement has an uncertainty (standard deviation), $\sigma_1$ and $\sigma_2$. How to find the optimal estimate, $\hat{x}$, that best combines $x_1$ and $x_2$.

The idea is to use a weighted least squares, We want to find an $\hat{x}$ that minimizes the “cost” function $S = \sum_{i=1}^{n}w_i(\hat{x} - x_i)^2$. We want to give more weight to the measurement we trust more (the one with lower uncertainty), so we set $w_i = \frac{1}{\sigma_i^2}$. We solve $\hat{x}$:

$$\hat{x} = \frac{\sum w_i x_i}{\sum w_i}$$

$$\hat{x} = \left( \frac{\sigma_2^2}{\sigma_1^2 + \sigma_2^2} \right) x_1 + \left( \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} \right) x_2$$

The new uncertainty of $\hat{x}$: Since $\text{Var}(a \cdot X + b \cdot Y) = a^2 \cdot \text{Var}(X) + b^2 \cdot \text{Var}(Y)$, we get:

$$\sigma_{\hat{x}}^2 = \left( \frac{\sigma_2^2}{\sigma_1^2 + \sigma_2^2} \right)^2 \sigma_1^2 + \left( \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} \right)^2 \sigma_2^2$$

$$\sigma_{\hat{x}}^2 = \frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}$$

Rewrite the above formula as an update step:

$$\hat{x} = x_1 + \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} (x_2 - x_1)$$

Discrete Kalman Filter

In robotics case, the $x_1$ is the prediction of the robot’s state given previous state, and $x_2$ is the actual measurement of robot’s state from sensor.

The Process Model

$$s_t = Fs_{t-1} + Gu_t + w_t$$

• $s_t$: The true state of the system at time $t$.
• $F$: It defines how the state would change if there were no control inputs. For example, $s = \begin{bmatrix} p \\ v \end{bmatrix}$ (where $p$ is position, $v$ is velocity), then $p_t = p_{t-1} + v_{t-1} \cdot \Delta t$ and $v_t = v_{t-1}$, then

$$\begin{bmatrix} p_t \\ v_t \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{t-1} \\ v_{t-1} \end{bmatrix}$$

• $Gu_t$: $u_t$ is control input. $G$ maps that command to a change in the state. For example, $u_t = [a]$ which is acceleration, then $\Delta v = a \cdot \Delta t$, $\Delta p = \frac{1}{2} a (\Delta t)^2$. As a result,

$$Gu_t = \begin{bmatrix} \Delta p \\ \Delta v \end{bmatrix} = \begin{bmatrix} \frac{1}{2} (\Delta t)^2 \\ \Delta t \end{bmatrix} \begin{bmatrix} a \end{bmatrix}$$

• $w_t$: Process noise, for example, a bump in the road or wheel slippage. $p(w) \sim N(0, Q)$, $Q$ represents “How much do I distrust my system model?”.

$$z_t = Hs_t + v_t$$

• $z_t$: The observation that sensor gives.
• $H$: For example, your state $s_t$ might be a 6D vector (position x, y, z; velocity vx, vy, vz), but measurement $z_t$ is only a 3D vector (position x, y, z). The $H$ matrix would be a 3x6 matrix that just “selects” the first three components of the state.
• $v_t$: Measurement Noise by the sensor itself. $p(v) \sim N(0, R)$. $R$ represents “How much do I distrust my sensor?”.

Kalman Prediction

$$s_{t|t-1} = Fs_{t-1|t-1} + Gu_t$$

• $s_{t-1|t-1}$: The corrected state after the last update.
• $s_{t|t-1}$: The new predicted state for the current time $t$, based only on information from $t-1$.

$$\Sigma_{t|t-1} = F\Sigma_{t-1|t-1}F^T + Q_t$$

• $\Sigma_{t-1|t-1}$: Uncertainty covariance matrix of last best estimate $s_{t-1|t-1}$.
• $\Sigma_{t|t-1}$: New, larger predicted uncertainty for the current time $t$.

Kalman Updating

State Update:

$$s_{t|t} = s_{t|t-1} + K_t(z_t - H s_{t|t-1})$$

• The above formula is equivalent to $\hat{x} = x_1 + K(x_2 - x_1)$.
• $s_{t|t}$: The corrected state for time $t$.
• $(z_t - H s_{t|t-1})$: $z_t$ is new measurement, $H s_{t|t-1}$ is expected measurement. The difference is called “surprise”.

$$K_t = \Sigma_{t|t-1} H^T S_t^{-1}$$

$$S_t = H \Sigma_{t|t-1} H^T + R_t$$

• $S_t$: Equivalent to $\sigma_1^2 + \sigma_2^2$ in the 1D example. $\Sigma_{t|t-1}$ is predicted uncertainty (like $\sigma_1^2$), and $H \Sigma_{t|t-1} H^T$ is the predicted uncertainty projected into the measurement space. $R_t$ is measurement sensor’s uncertainty (like $\sigma_2^2$).
• $K_t$: This is the matrix version of $K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}$.

Covariance Update:

$$\Sigma_{t|t} = (I - K_t H) \Sigma_{t|t-1}$$

• $\Sigma_{t|t}$: Corrected, smaller uncertainty (like $\sigma_{\hat{x}}^2$).

Extended Kalman Filter

The previous formula all assumed a linear world. However, in many cases, the state change is non-linear. For example, a robot’s state $[x, y, \theta]$:

• $x_t = x_{t-1} + v_t \cdot \cos(\theta_{t-1}) \cdot \Delta t$
• $y_t = y_{t-1} + v_t \cdot \sin(\theta_{t-1}) \cdot \Delta t$
• $\theta_t = \theta_{t-1} + \omega_t \cdot \Delta t$

As a result, there is no way to write the relationship with $s_t = F s_{t-1} + G u_t$. In this section, we will use a non-linear expression:

$$s_t = f(s_{t-1}, u_{t-1}) + w_{t-1}$$

$$z_t = h(s_t) + v_k$$

If the system is non-linear, we can’t use the Kalman filter. So, at every single time step, we will approximate the non-linear function with a linear one. The best linear approximation of a function at a specific point is its tangent line (or tangent plane in multi-dimensions) found by first-order Taylor series expansion.

$$s_t \approx f(s_{t-1|t-1}, u_{t-1}, 0) + F(s_{t-1} - s_{t-1|t-1}) + w_{t-1}$$

$$z_t \approx h(s_{t|t}, 0) + H(s_t - s_{t|t}) + v_k$$

where $F_{ij} = \frac{\partial f_i}{\partial s_j}(s_{t-1|t-1}, u_{t-1}, 0)$ and $H_{ij} = \frac{\partial h_i}{\partial s_j}(s_{t|t}, 0)$.

EKF Prediction

$$s_{t|t-1} = f(s_{t-1|t-1}, u_{t-1}, 0)$$

$$\Sigma_{t|t-1} = F_t \Sigma_{t-1|t-1} F_t^T + Q_{t-1}$$

EKF Updating

$$K_t = \Sigma_{t|t-1} H_t^T (H_t \Sigma_{t|t-1} H_t^T + R_t)^{-1}$$

$$s_{t|t} = s_{t|t-1} + K_t (z_t - h(s_{t|t-1}, 0))$$

$$\Sigma_{t|t} = (I - K_t H_t) \Sigma_{t|t-1}$$