Extended Kalman Filter

Normal Discrete Kalman FIlter

Initialization

$\hat{x}_{0|0} = E[x_0], \quad P_{0|0} = E\left[(x_0 - \hat{x}_{0|0})(x_0 - \hat{x}_{0|0})^\top\right].$

Prediction Step

$\hat{x}_{k+1|k} = A_d \hat{x}_{k|k} + B_d u_k,$

$P_{k+1|k} = A_d P_{k|k} A_d^\top + Q.$

Measurement Update Step

$L_{k+1} = P_{k+1|k}C^\top \left(CP_{k+1|k}C^\top + R\right)^{-1}.$

$\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + L_{k+1}\left(y_{k+1} - C\hat{x}_{k+1|k}\right).$

$P_{k+1|k+1} = \left(I - L_{k+1}C\right)P_{k+1|k}.$

Let’s remind ourselves how we linearise a typical continuous system. A nonlinear, time-varying system is represented in state space as:

$\dot{x}(t) = f(x(t), u(t), t),$

$y(t) = h(x(t), u(t), t),$

The vector field $f(x, u)$ is expanded as (uses the Taylor series formula for multi-variate functions):

$f(x, u) = f(x^*, u^*) + \frac{\partial f}{\partial x}(x^*, u^*) x_p + \frac{\partial f}{\partial u}(x^*, u^*) u_p + \text{higher-order terms},$

The linearized system around a nominal trajectory is given by (p standing for pertubations around the trajectory):

$\dot{x}_p(t) = A(t)x_p(t) + B(t)u_p(t),$

where:

- $A(t) = \frac{\partial f}{\partial x}(x^*(t), u^*(t))$ is the Jacobian of $f$ with respect to $x$.

- $B(t) = \frac{\partial f}{\partial u}(x^*(t), u^*(t))$ is the Jacobian of $f$ with respect to $u$.

$f$ is a vector that consists of multiple functions.

So now let’s look at the non-linear discrete time system.

State Transition (Process Model):

$x_{k+1} = f(x_k, u_k) + w_k$

Observation (Measurement Model):

$y_k = h(x_k) + v_k$

Similar to the continuous method, we want to linearise this system by finding matrices A and C. We choose our nominal point to linearise around to be $\hat{x}_{k|k}$ (and $u_k$), which is the estimate of $x_k$ at time $k$ given measurements up to $k$.

Notice we are linearising around an estimated state as we are trying to create a Kalman filter. Refer back to the original discrete Kalman filter to see how and where $\hat{x}_{k|k}$ is used.

Define Jacobians for Dynamics:

$A_k = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k|k}, u_k}$

This is the same as our standard linearisation process.

Start with the nonlinear measurement model (shifted by one):

$y_{k+1} = h(x_{k+1}) + v_{k+1}$

Linearise $h(x_{k+1})$ around the predicted state $\hat{x}_{k+1|k}$

We use the first-order Taylor expansion of $h(x)$ about $\hat{x}_{k+1|k}$:

$h(x_{k+1}) \approx h(\hat{x}_{k+1|k}) + \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k+1|k}} (x_{k+1} - \hat{x}_{k+1|k}) + \text{higher order terms}$

This is the standard Taylor expansion, shown previously:

$h(x) \approx h(a) + h'(a)(x - a) + \text{H.O.T.}$

In our case:

- $x = x_{k+1}$

- $a = \hat{x}_{k+1|k}$

Now, here’s a trick.

We define the $C$ Jacobian matrix as the following (since we shifted by one, we use k+1):

$C_{k+1} = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k+1|k}}$

So now we go from

$y_{k+1} = h(x_{k+1}) + v_{k+1}$

to a linearised version (h is now evaluated so no longer a function, we also drop the v after),

$\Rightarrow y_{k+1} \approx h(\hat{x}_{k+1|k}) + C_{k+1}(x_{k+1} - \hat{x}_{k+1|k}) + v_{k+1}$

Using these ideas (not sure how), we get the following: