Extended Kalman Filter: Why can we simply replace the model matrices by the Jacobians?

301 Views Asked by At

The main difference between the standard Kalman Filter (KF) and the Extended Kalman Filter (EKF) is, that it can be applied to non-linear models. The latter uses a first order taylor approximation to linearize the model around the current state and uses the Jacobians of the model equations to calculate the covariances.

Calculating the predicted estimate covariance in the KF with a linear model goes like this: $$ \hat{x}_k^- = A_{k-1}\hat{x}_{k-1}+B_{k-1}u_k\\ P_k^- = A_{k-1}P_{k-1}A_{k-1}^T+Q $$

Whereas in the EKF with a nonlinear model like is: $$ \hat{x}_k^- = f_k(x_{k-1}, u_k) \\ P_k^- = F_{k-1}P_{k-1}F_{k-1}^T+Q $$

where $x$ is the state vector, $P$ is the error covariance, $Q$ is the covariance of the process noise and $F$ is simply the Jacobian of $f_k$

$$F_k = \frac{\partial f_k}{\partial x }$$

I also know that the Variance of a random Variable $X$ multiplied by a matrix $A$ is given by: $$Var(A \cdot X) = A \cdot Var(X) \cdot A^T$$

In general we can notice that within the Extended Kalman Filter equations the matrices from the state space model are simply replaced by the Jacobians of the model. But how does this work out mathematically? Why can we just use the Jacobian to calculate the covariances? Why don't the formulas change?

1

There are 1 best solutions below

2
On

First consider the case when $f(0, u_k) = 0$, and $x_{k-1}$ has mean $0$. We'll Taylor expand at $0$.

Rewrite $f(x_{k-1},u_k) \equiv F_k x_{k-1} + err(x_{k-1})$ where $err$ is simply the difference $f(x_{k-1},u_k) - F_k x_{k-1}$.

We know from calculus that $err \rightarrow 0$ quadratically in $x_{k-1}$.

The derivations follow by assuming $err = 0$, and then we simply get back the Kalman Filter equations.

Now we examine the general case. Decompose $x_{k-1} = \bar x_{k-1} + \Delta x_{k-1}$ where $\bar x_{k-1}$ is the mean of $x_{k-1}$ and therefore $\Delta x_k$ is a $0$-centered Gaussian with the same covariance as $x_{k-1}$.

Choose $F_k$ to be $\frac {\delta f }{\delta x}(\bar x_{k-1}, u_k)$. Then by Taylor expansion we have $f_k(x_{k-1}, u_k) = f_k(\bar x_{k-1} + \Delta x_{k-1}, u_k) = f_k(\bar x_{k-1}, u_k) + F_k \Delta x_{k-1} + err(\Delta x_{k-1})$.

Then we approximate $\hat x^- = f_k(\bar x_{k-1}, u_k) + F_k \Delta x_{k-1}$.

Since $\Delta x_{k-1}$ has the same covariance matrix as $x_{k-1}$, the covariance formula doesn't change.