Extended Kalman Filter (EKF)

This is a class project which create a extended Kalman filter (EKF)
The plant model and the measurement model are non-linear in this project

---

Matrix Gt obtained after linearizing the plant model

function G = plant_jacobian(xs)

returns the plant model Jacobian matrix G evaluated at xs
parameter xs is the estimated location of the robot at time t − 1

---

Matrix Ht obtained after linearizing the measurement model

function H = meas_jacobian(xt)

returns the measurement model Jacobian matrix H evaluated at xt
parameter xt is predicted location of the robot at time t

---

Extended Kalman Filter (EKF)

function [mu, Sigma] = ekf(mu, Sigma, u, z)

returns the estimated state mu and covariance matrix Sigma at time t given the estimated state mu at time (t − 1) parameter Sigma - estimated covariance Sigma at time (t − 1)
parameter u - the control input u at time t
parameter z - the observation z at time t

---

Tester

EKF at each time step t = 0, 1, 2, ..., 25, estimate state covariance Σ0 =

true starting position of the robot is Xtrue = transpose([0 2])

Estimates of the starting location x0

x0 = transpose([0 9])

x0 = transpose([0 -5])

The two different starting points produce such different results

The estimating goes wrong iff when y axis of starting point (xl) is negative value. In general, the initial values only affect the convergence rate of EKF model (i.e. xˆ0 is not close to x0); however, if P0 (note 1) is chosen too small while xˆ0 and x0 differ considerably, the estimator relies on the model predictions more than it should. Therefore, subsequent measurements do not have the impact on the estimator that they need to have. The filter might learn the wrong state too well and diverge. (In this case, xl cause the filter diverge when it becomes negative)

note1: P0 =diag (xˆ0−x0)T(xˆ0−x0) initialize EKF model with this equation gives good estimation result estimate of initial state: x0 = [xu xl]T xˆ0 is a suitable initial state guess, P0 is uncertainty of initial state estimate covariance matrix