Extended Kalman Filter (EKF)

And some other useful Kalman stuff!

The Linear (normal) KF

𝒙𝑘 = 𝐹𝒙𝑘−1 + 𝐺𝒖𝑘−1 + 𝒘𝑘−1 𝒚𝑘 = 𝐻𝒙𝑘 + 𝒗𝑘

z ¹̄ HF

G xk+1 xkwk vk

uk yk

z ¹̄ HF

G xkK

What if the system are not described in a linear manner?I.e. if we divide by a state variable, multiplies two state variables or e.g. applies a function (e.g. a sinus) on one or more of our state variables.A lot of real world models are non-linear.How can we extend the Kalman filter – which is an linear optimal state estimator – to real world problems, which not always, in the first point of view, may not be described in a linear manner?

𝒙𝑘 = 𝐹𝒙𝑘−1 + 𝐺𝒖𝑘−1 + 𝒘𝑘−1 𝒚𝑘 = 𝐻𝒙𝑘 + 𝒗𝑘

We goes from this system description, which is linear

To this system description, which may be non-linear

How should we handle the Kalman gain?

z¯¹ h(Ȉ)f(Ȉ)xk+1 xk

wk

vk

yk

z¯¹ h(Ȉ)f(Ȉ)xk

?

uk

First approach

The obvious solution is to use a Kalman filter on a non-linear system, by linearize the model around a point of operation.But, this could require a significantly higher order of model, to be able to describe the non-linear behavior. Should we use a non-linear function in place of K?

The solution is much smarter!

K will be replaced by another K, just obtained “slightly” different.Extension of the linear Kalman filter to the non-linear cases requires a few more steps in the implementation. But the most of the calculations may be done in compile time, which make the online calculations only a little more intense, in case than the non-linear functions themselves are not computationally heavier. We still only have to compute a single matrix inverse.

Partial Derivative Matrix

A.k.a. Jacobian matrix will be the tool to handle the EKF. Remember that the Jacobian matrix is defined as

Remember this!

𝐽=ۏێێێ𝑓1ሺ𝒙ሻ𝜕𝑥1��ۍ ⋯ 𝜕𝑓1ሺ𝒙ሻ𝜕𝑥𝑛⋮ ⋱ ⋮𝜕𝑓𝑚ሺ𝒙ሻ𝜕𝑥1 ⋯ 𝜕𝑓𝑚ሺ𝒙ሻ𝜕𝑥𝑛 ے

ۑۑۑې

Quiz Time!

𝒙𝑘+1 = 𝑓ሺ𝒙𝑘ሻ= ൬𝑓1ሺ𝒙𝑘ሻ𝑓2ሺ𝒙𝑘ሻ൰= ቀ

𝑥1𝑥2𝑥2 + cosሺ𝑥1ሻቁ 𝐽= ቂ𝑥2 𝑥1−sinሺ𝑥1ሻ 1ቃ

Quiz

Consider the system described by the following state update function:

1,1

2,1

1,2

2,2

The EKF algorithm (1/4)

The system and measurement equations are given as follows:

Initialize the system:𝑥ො��0+ = 𝔼ሾ𝑥0ሿ 𝑃0+ = 𝔼ሾሺ𝑥0 − 𝑥ො��0+ሻሺ𝑥0 − 𝑥ො��0+ሻ𝑇ሿ

𝑥𝑘 = 𝑓𝑘−1ሺ𝑥𝑘−1,𝑢𝑘−1,𝑤𝑘−1ሻ 𝑦𝑘 = ℎ𝑘ሺ𝑥𝑘,𝑣𝑘ሻ 𝑤𝑘~𝑊𝑁ሺ0,𝑄𝑘ሻ 𝑣𝑘~𝑊𝑁ሺ0,𝑅𝑘ሻ

The EKF algorithm (2/4)

For each time step k=1,2,…, compute the following:a) Compute the following partial derivative

matrices (Jacobian):

𝐹𝑘−1 = 𝜕𝑓𝑘−1𝜕𝑥 ฬ𝑥ො��𝑘−1+ 𝐿𝑘−1 = 𝜕𝑓𝑘−1𝜕𝑤 ฬ𝑥ො��𝑘−1+

The EKF algorithm (3/4)

b) Perform the time update of the state estimate and estimation-error co-variance:

c) Compute the partial derivative matrices:

𝑃𝑘− = 𝐹𝑘−1𝑃𝑘−1+ 𝐹𝑘−1𝑇 + 𝐿𝑘−1𝑄𝑘−1𝐿𝑘−1𝑇 𝑥ො��𝑘− = 𝑓𝑘−1ሺ𝑥𝑘−1+ ,𝑢𝑘−1,0ሻ

𝐻𝑘 = 𝜕ℎ𝑘𝜕𝑥ฬ𝑥ො��𝑘− 𝑀𝑘 = 𝜕ℎ𝑘𝜕𝑣ฬ𝑥ො��𝑘−

The EKF algorithm (4/4)

d) Perform the measurement update of the state estimate and the estimation error co-variance matrix:

…and go back to a).Note that numerical more precise equations are available for the co-variance update function.

𝐾𝑘 = 𝑃𝑘−𝐻𝑘𝑇൫𝐻𝑘𝑃𝑘−𝐻𝑘𝑇+ 𝑀𝑘𝑅𝑘𝑀𝑘𝑇൯−1 𝑥ො��𝑘+ = 𝑥ො��𝑘− + 𝐾𝑘൫𝑦𝑘 − ℎ𝑘ሺ𝑥ො��𝑘−,0ሻ൯ 𝑃𝑘+ = ሺ𝐼− 𝐾𝑘𝐻𝑘ሻ𝑃𝑘−

Discretization (1/5)There are different approaches to discretization. E.g. we could have a linear continuous time state space model described by the system equations

This could be transformed to a discrete model

by

𝒙ሶ= 𝐴𝒙+ 𝐵𝒖 𝒚= 𝐶𝒙

𝒙𝑘 = 𝐹𝒙𝑘−1 + 𝐺𝒖𝑘−1 𝒚𝑘 = 𝐻𝒙𝑘

𝐹= 𝑒𝐴Δ𝑡 𝐺= 𝐹න 𝑒−𝐴𝜏𝑑𝜏Δ𝑡

0 𝐵= 𝐹ሾ𝐼− 𝑒−𝐴𝜏ሿ𝐴−1𝐵

Discretization (2/5)

Another, and maybe the simplest approach, will be to transform the individual sums of products in the continuous time update equation from the definition of the derivative. E.g. like

where Δt is the samplingstime.

𝑥ሶ1 = 𝑥2 ⇓ 𝑥1ሺ𝑡+ Δ𝑡ሻ− 𝑥1ሺ𝑡ሻΔ𝑡 ≈ 𝑥2ሺ𝑡ሻ ⇓ 𝑥1ሺ𝑡+ Δ𝑡ሻ≈ 𝑥1ሺ𝑡ሻ+ Δ𝑡𝑥2ሺ𝑡ሻ⇒ 𝑥1𝑘+1 ≈ 𝑥1𝑘 + Δ𝑡𝑥2𝑘

Discretization (3/5)

An obviously approach will also be to use Taylor expansion. This could make us skip the step of going through the continuous time state space model to reach the discrete time model .E.g. if M is a constant and F and ϴ (moment and angular position) are state variables we could discretize the expression 𝐹ሺ𝑡ሻ= 𝑀sin൫𝜃ሺ𝑡ሻ൯

Discretization (4/5)

rmm g

mc

θ

𝐹ሺ𝑡+ Δ𝑡ሻ= 𝐹ሺ𝑡ሻ+ Δ𝑡 𝑀cos൫𝜃ሺ𝑡ሻ൯𝜔ሺ𝑡ሻ+ Δ𝑡22 ቀ−𝑀sin൫𝜃ሺ𝑡ሻ൯𝜔2ሺ𝑡ሻ+ 𝑀cos൫𝜃ሺ𝑡ሻ൯𝛼ሺ𝑡ሻቁ

Discretization (5/5)

Using up to second order the discrete time model will be

𝐹ሺ𝑡ሻ= 𝑀sin൫𝜃ሺ𝑡ሻ൯

Other Need to Know about Kalman

System is Reachable ((F,G) pair is Reachable) if, starting from a null initial state, we can always find an input signal which leads the system to any a-priori selected final state in finite time.

Condition to check Reachability

System is Reachable if and only if the reachability matrix is full rank. I.e. its rank is n.

Note that the system may be reachable only from the process noise point of view. Here the must be a factorization of process noise co-variance matrix .

Other Need to Know about Kalman

System S is called Observable (the (F,H) pair is observable) if two different initial states do not exist, such that their corresponding outputs are exactly the same, for each t≥0.

Condition to check Observability

System is observable if and only if the observability matrix is full rank. I.e. it has rank n, where n is the system order.

Let’s check Observability

We have the system 𝑥1𝑥2𝑥3൩𝑘+1 = 1 𝑎12 00 1 00 0 1൩𝑥1𝑥2𝑥3൩𝑘 ሾ𝑦1ሿ𝑘 = ሾ1 0 0ሿ𝑥1𝑥2𝑥3൩𝑘

Let’s check Observability

We have the system 𝑥1𝑥2𝑥3൩𝑘+1 = 1 𝑎12 00 1 𝑎230 0 1 ൩𝑥1𝑥2𝑥3൩𝑘 ሾ𝑦1ሿ𝑘 = ሾ1 0 0ሿ𝑥1𝑥2𝑥3൩𝑘

Nice to Know about Kalman

If the following conditions hold:• Uncorrelated process and measurement noise!• And one of either:a) System is reachable from the noise’s point of view, and

the system is observable.b) Or all F eigenvalues are strictly inside the unitary circle.

It will make the system asymptotically converge.

Gain of Asymptotic Convergence

If a linear system asymptotically converges, we can calculate the estimation error co-variance matrix and hence the Kalman gain analytical at compile time, which will give us a much more, computational, efficient filter.Otherwise we have to analyze in each time instance.

Exercise on the class!

See the uploaded document:A Mechanical System.pdf

References[1] S. M. Savaresi, ”Model Identification and Adaptive Systems - KALMAN FILTER”, Milan: Politecnico di Milano, 2011.

[2] D. Simon, “Optimal State Estimation, Kalman, H∞ and Nonlinear Approaches”, Hoboken, New Jersey: Wiley, 2006.

[3] Welch, G and Bishop, G. 2001. “An introduction to the Kalman Filter”, http://www.cs.unc.edu/~welch/kalman/