Kalman filtering

Question 1

Mathematical formulation

Answer 1

Assumption: system S is LTI, autonomous

S: x(t+1) = Fx(t) + v1(t)
y(t) = Hx(t) + v2(t)

v1(t) is WN(0, V1)
v2(t) is WN(0, V2)
v1 and v2 are uncorrelated

Question 2

Problem statement

Answer 2

• y(t) can be measured
• The model (F, H, V1, V2, V12) is known
• x(t) is unknown, including the initial state

Goal: to estimate x^(t+k|t)

Question 3

Possible problems solved by Kalman filter

Answer 3

• Filtering problem:
  k=0 - > x^(t|t)
• Prediction problem:
  k > 0 - > x^(t+1|t)
• smoothing problem:
  k < 0 - > x^(t-1|t)

Question 4

Kalman predictor (k=1)

Answer 4

x^(t+1|t) = Fx^(t|t-1) + k(t)*e(t)
y^(t|t-1) = Hx^(t|t-1) 

e(t) = y(t) - y^(t|t-1) is the innovation term

k(t) is the Kalman gain, that minimizes the variance of the prediction error

Question 5

Expression of the dinamic Riccati equation

Answer 5

P(t+1) = FP(t)F’ + V1 - (FP(t)H’ + V12) (HP(t)H’ + V2)^-1 (FP(t)H’ + V12)’

Question 6

Variance of the prediction error

Answer 6

trace{ P(t) = E[(x(t)-x^(t|t-1))(x(t)-x^(t|t-1))’] }

P(t) is the covariance of the prediction error

Question 7

Expression of the Kalman gain

Answer 7

k(t) = (FP(t)H’+V12)(HP(t)H’+V2)^-1

Question 8

Generalization: multi-step predictor

Answer 8

x^(t+k|t) = F^k-1 x^(t+1|t)
y^(t+k|t) = H x^(t+k|t)

Question 9

Generalization: filtering problem

Answer 9

If F is non singular:

x^(t|t) = F^-1 x^(t+1|t)

Question 10

Generalization: exogenous inputs

Answer 10

x(t+1) = Fx(t) + Gu(t) + v1(t)

• > x^(t+1|t) = Fx^(t|t-1) + Gu(t) + k(t)*e(t)
• G is known and u is a measurable input
• k(t) does not change

Question 11

Theorem 1 for the convergence of the DRE

Answer 11

If the system S is stable and V12=0
then, for any P1 > = 0:
- lim(t - > +inf) P(t) = P_ > = 0
- F - k_H is stable

Question 12

Theorem 2 for the convergence of the DRE

Answer 12

If (F,H) is observable and (F,G) is reachable, where G is such that V1 = GG’, and V12=0
then, for any P1 > = 0
- lim(t - > +inf) P(t) = P_ > = 0
- F - k_H is stable

Question 13

Extended Kalman filter: use

Answer 13

Used for non-linear systems, to estimate the variations of the state from its nominal trajectory

System in the form
x(t+1) = f(x(t),t) + v1(t)
y(t) = h(x(t),t) + v2(t)

Nominal trajectory, a priori defined:
x_(t+1)=f(x_(t),t)
y_(t) = y(x_(t))

Question 14

Extended Kalman filter: algorithm

Answer 14

• compute F_(t) and H_(t) using x^(t|t-1)
• compute k_(t): Kalman gain for the linearized system
• compute x^(t+1|t)

Question 15

Formula for x^(t+1|t) for EKF

Answer 15

x^(t+1|t) = f(x^(t|t-1),t) + k_(t)*e(t)
e(t) = y(t) - h(x^(t|t-1),t)