Extended Kalman Filter (EKF)

• The Extended Kalman Filter (EKF) is a Kalman filter version designed for nonlinear systems.
• It approximates non-linear systems by linearizing them around the current estimate and then applying the standard Kalman filter.

Kalman Filter Basics

• Mathematical tool used to estimate the state of a system from noisy measurements
• Extensively used in robotics, economics, and control systems.

Extended Kalman Filter

• Addresses non-linearity by linearizing the system around the current estimate using Taylor series expansion.

EKF Steps

• Prediction: Predict the next state and covariance using a non-linear system model.
• Linearization: Linearize the system model around the predicted state.
• Update: Update the state estimate using the Kalman gain, computed with the linearized system and measurement models.

Jacobian Matrix

• The matrix of all first-order partial derivatives of a vector-valued function. $$ J = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \ \vdots & \ddots & \vdots \ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} $$
• $f: \mathbb{R}^n \rightarrow \mathbb{R}^m$ is a vector-valued function.
• $J$ is an $m \times n$ matrix where each entry $\frac{\partial f_i}{\partial x_j}$ represents the rate of change of the $i$-th component of $f$ with respect to the $j$-th component of $x$.
• Used to linearize a non-linear function around a specific point

Taylor Series Expansion

• Used to approximate a function at a point using its derivatives at another point.
• Scalar function $f(x)$ expansion around a point $a$: $$ f(x) = f(a) + f'(a)(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \cdots $$
• Vector-valued function $f(x)$ expansion around a point $a$: $$ f(x) \approx f(a) + J(a)(x-a) $$ Where $J(a)$ is the Jacobian matrix of $f$ evaluated at $a$.

System Model

• The means to describe the evolution of the system state over time. $$ x_t = f(x_{t-1}, u_{t-1}, w_{t-1}) $$ Where:
  • $x_t$ = the state at time t
  • $x_{t-1}$ = the state at time t-1
  • $u_{t-1}$ = the control input at time t-1
  • $w_{t-1}$ = the process noise at time t-1
  • $f$ = a non-linear function that describes the system dynamics.

Measurement Model

• Depicts the relationship between measurements and the system state $$ z_t = h(x_t, v_t) $$ Where:
  • $z_t$ = the measurement at time t
  • $x_t$ = the state at time t
  • $v_t$ = the measurement noise at time t
  • $h$ = a non-linear function relating the state to the measurement

EKF Algorithm Steps

• Initialization: Initialize the state estimate $\hat{x}_0$ and the covariance matrix $P_0$. $$ \hat{x}_0 = E[x_0] $$ $$ P_0 = E[(x_0 - \hat{x}_0)(x_0 - \hat{x}_0)^T] $$
• Prediction: Predict the state and covariance at time $t$ given the state estimate at time $t-1$. $$ \hat{x}t = f(\hat{x}{t-1}, u_{t-1}, 0) $$ $$ P_t = F_{t-1}P_{t-1}F_{t-1}^T + W_{t-1}Q_{t-1}W_{t-1}^T $$ Where:
  • $F_{t-1}$ is the Jacobian matrix of $f$ with respect to $x$ evaluated at $\hat{x}_{t-1}$
  • $W_{t-1}$ is the Jacobian matrix of $f$ with respect to $w$ evaluated at $0$
  • $Q_{t-1}$ is the covariance matrix of the process noise $w_{t-1}$
• Measurement Update: Update the state estimate and covariance at time $t$ given the measurement at time $t$. $$ \tilde{y}_t = z_t - h(\hat{x}_t, 0) $$ $$ S_t = H_tP_tH_t^T + V_tR_tV_t^T $$ $$ K_t = P_tH_t^TS_t^{-1} $$ $$ \hat{x}_t = \hat{x}_t + K_t\tilde{y}_t $$ $$ P_t = (I - K_tH_t)P_t $$ Where :
  • $H_t$ is the Jacobian matrix of $h$ with respect to $x$ evaluated at $\hat{x}_t$
  • $V_t$ is the Jacobian matrix of $h$ with respect to $v$ evaluated at $0$
  • $R_t$ is the covariance matrix of the measurement noise $v_t$
  • $\tilde{y}_t$ is the measurement residual
  • $S_t$ is the innovation covariance
  • $K_t$ is the Kalman gain

EKF Algorithm Summary

Robot Model

• Considers a robot moving in a 2D plane.
• The state of the robot is given by: $$ x_t = \begin{bmatrix} x \ y \ \theta \end{bmatrix} $$ Where:
  • $x$ and $y$ are the coordinates of the robot in the 2D plane
  • $\theta$ is the orientation of the robot
• The control input is given by: $$ u_t = \begin{bmatrix} v \ \omega \end{bmatrix} $$ Where:
  • $v$ is the linear velocity of the robot
  • $\omega$ is the angular velocity of the robot
• The system model is given by: $$ \begin{bmatrix} x_{t+1} \ y_{t+1} \ \theta_{t+1} \end{bmatrix} = \begin{bmatrix} x_t + v_t\cos(\theta_t)\Delta t \ y_t + v_t\sin(\theta_t)\Delta t \ \theta_t + \omega_t\Delta t \end{bmatrix} + w_t $$ Where:
  • $\Delta t$ is the time step
  • $w_t$ is the process noise
• In this case, the function $f$ is: $$ f(x_t, u_t, w_t) = \begin{bmatrix} x_t + v_t\cos(\theta_t)\Delta t \ y_t + v_t\sin(\theta_t)\Delta t \ \theta_t + \omega_t\Delta t \end{bmatrix} + w_t $$

Measurement Model

• The robot has a sensor measuring landmark distance: $$ z_t = \sqrt{(x_l - x_t)^2 + (y_l - y_t)^2} + v_t $$ Where:
  • $(x_l, y_l)$ are the coordinates of the landmark
  • $v_t$ is the measurement noise
• In this case, the function $h$ is: $$ h(x_t, v_t) = \sqrt{(x_l - x_t)^2 + (y_l - y_t)^2} + v_t $$

EKF Implementation

• Initialization: Initialize the state estimate $\hat{x}_0$ and the covariance matrix $P_0$.
• Prediction: Predict the state and covariance at time $t$ given the state estimate at time $t-1$: $$ \hat{x}t = f(\hat{x}{t-1}, u_{t-1}, 0) = \begin{bmatrix} x_{t-1} + v_{t-1}\cos(\theta_{t-1})\Delta t \ y_{t-1} + v_{t-1}\sin(\theta_{t-1})\Delta t \ \theta_{t-1} + \omega_{t-1}\Delta t \end{bmatrix} $$ $$ P_t = F_{t-1}P_{t-1}F_{t-1}^T + W_{t-1}Q_{t-1}W_{t-1}^T $$ Where:
  • $F_{t-1}$ is the Jacobian matrix of $f$ with respect to $x$ evaluated at $\hat{x}{t-1}$: $$ F{t-1} = \begin{bmatrix} 1 & 0 & -v_{t-1}\sin(\theta_{t-1})\Delta t \ 0 & 1 & v_{t-1}\cos(\theta_{t-1})\Delta t \ 0 & 0 & 1 \end{bmatrix} $$
  • $W_{t-1}$ is the Jacobian matrix of $f$ with respect to $w$ evaluated at $0$: $$ W_{t-1} = \begin{bmatrix} 1 & 0 & 0 \ 0 & 1 & 0 \ 0 & 0 & 1 \end{bmatrix} $$
  • $Q_{t-1}$ is the covariance matrix of the process noise $w_{t-1}$
• Measurement Update: Update the state estimate and covariance at time $t$ given the measurement at time $t$: $$ \tilde{y}_t = z_t - h(\hat{x}_t, 0) = z_t - \sqrt{(x_l - x_t)^2 + (y_l - y_t)^2} $$ $$ S_t = H_tP_tH_t^T + V_tR_tV_t^T $$ $$ K_t = P_tH_t^TS_t^{-1} $$ $$ \hat{x}_t = \hat{x}_t + K_t\tilde{y}_t $$ $$ P_t = (I - K_tH_t)P_t $$ Where:
  • $H_t$ is the Jacobian matrix of $h$ with respect to $x$ evaluated at $\hat{x}_t$: $$ H_t = \begin{bmatrix} \frac{-(x_l - x_t)}{\sqrt{(x_l - x_t)^2 + (y_l - y_t)^2}} & \frac{-(y_l - y_t)}{\sqrt{(x_l - x_t)^2 + (y_l - y_t)^2}} & 0 \end{bmatrix} $$
  • $V_t$ is the Jacobian matrix of $h$ with respect to $v$ evaluated at $0$.
  • $R_t$ is the covariance matrix of the measurement noise $v_t$.

Results

• Iteratively applying the prediction and update steps enables the EKF to estimate the robot's state over time.
• The estimate's accuracy is affected by the accuracy of the system model, the measurement model, and noise characteristics.

Advantages of EKF

• Handles Non-linear Systems by linearizing around the current estimate.
• Computationally Efficient compared to other non-linear filters like particle filters.
• Wide Applicability in robotics, computer vision, and control systems.

Disadvantages of EKF

• Linearization Errors arise, especially in highly non-linear systems.
• Jacobian Matrix Calculation can be difficult or impossible for some systems.
• Tuning Required: careful tuning of noise covariance matrices can be time-consuming.

Conclusion

• The EKF is a robust tool for estimating the state of non-linear systems, widely used in robotics and control systems.
• Limitations: linearization errors and Jacobian calculation.
• May need other non-linear filters like Unscented Kalman Filter (UKF) and particle filters for highly non-linear systems.