Kalman Kalman Kalman Kalman Filter Design Filter Design Filter Design Filter Design - Iii III III III

Optimal Control, Guidance and Estimation
Lecture – 30
Kalman Filter Design – III
Prof. Radhakant Padhi

Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
Continuous-
Continuous-Discrete Kalman Filter

Continuous-Discrete KF
Continuous time model and discrete time measurements
Xɺ (t ) = A (t ) X (t ) + B (t )U (t ) + G (t ) W (t )
Yk = C k X k + Vk
E W (t )W T (τ )  = Qk δ (t − τ )
0 k ≠ j 
E Vk V jT  = Rk δ kj δ kj =  
1 k = j 
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 3

Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
Mechanism
X̂ 3−
Xˆ (t )
X̂ 1+ X̂ 2+
X̂ 1− X̂ 2− X̂ 3+
Xˆ 0− = Xˆ 0+
t0 t1 t2 t3
Time (t)

Principle
Propagate the state-estimate model forward from tk to tk +1
using the initial condition Xˆ k+ i.e., Xˆ k+ → Xˆ k−+1
Correct the value Xˆ k−+1 to Xˆ k++1 using the measurement vector Yk +1
Measurement is available only at discrete time-steps. Hence, the

continuum time propagation model DOES NOT involve any
measurement information. This leads to:
Pɺ (t ) = AP + PAT + GQG T
Expression for Pɺ
Xɺ = AX + BU + GW ɺ
Xɺɶ = Xɺ − Xˆ
ɺ
Xˆ = AXˆ + BU
⇒ = A Xɶ + GW
t
Xɶ (t ) = ϕ (t , t0 ) Xɶ 0 + ∫ ϕ (t , t0 ) G (τ )W (τ ) dτ
0
t 
RWXɶ = E  ∫ W (t ) W T (τ ) G (τ ) ϕ (t ,τ ) dτ 
0 
t
= ∫ Q δ (t − τ ) G T (τ ) ϕ (t ,τ ) dτ = 12 Q G T
0
Expression for Pɺ
( )
T
ɺ ɺ ɺ ɺ
Pɺ = E  Xɶ Xɶ T + Xɶ Xɶ T  = E  Xɶ Xɶ T  + E  Xɶ Xɶ T 
     
E  Xɺɶ Xɶ T  = E ( A Xɶ + GW ) Xɶ T 
 
= A E  Xɶ Xɶ T  + G E W Xɶ T 
= AP + 12 GQG T
Pɺ = ( AP + 12 GQG T ) + ( AP + 12 GQG T )
T
Pɺ = AP + PAT + GQGT
Summary
Model
Xɺ (t ) = A (t ) X ( t ) + B (t )U (t ) + G (t ) W (t )
Yk = C k X k + Vk
Xˆ ( t 0 ) = Xˆ 0
Initialization
P0− = E  Xɶ ( t 0 ) Xɶ T ( t 0 ) 
Gain −1
Computation K e k = Pk− C k T  C k Pk− C k T + R k 

Summary
Xˆ k+ = Xˆ k− + K ek  Y k − C k Xˆ k− 
Updation T
(
Pk+ = I − K ek C k ) P (I − K
k
−
ek Ck ) + K ek R k K eTk
( preferable )
(
= I − K ek C k )P k
−
( not preferable )
ɺ
Propagation Xˆ = AXˆ + BU
(using high
accuracy numerical
integration)
Pɺ (t ) = AP + PAT + GQG T

Note:
Pɺ (t )expression is a continuous time Lyapunov Equation

(its linear in P(t ))
Continuous-discrete Kalman filter facilitates the usage of

non-uniform ∆t
Use Xɺˆ (t ) and Pɺ (t ) expressions to propagate

Xˆ k+ → Xˆ k−+1 and Pˆk+ → Pˆk−+1 respectively.

Extended Kalman Filter (EKF)

Facts to Remember
Nonlinear estimation problems are considerably more difficult than
the linear problem in general. (EKF is just an idea…not a cure for
everything !)
The problem with nonlinear systems is that a Gaussian input doest

not necessarily produce a Gaussian output (unlike linear case)
The EKF even though not truly òptimum’, has been successfully
applied in many nonlinear systems over the decades
The fundamental assumption in EKF design is that the
true state X (t ) is sufficiently close to the estimated state Xˆ (t )
at all time, and hence the error dynamics can be represented
fairly accurately by the linearized system about Xˆ (t )
Continuous-
Continuous-Continuous EKF

Continuous-Continuous
Formulation
(Assumption: Measurement is continuously available)
Xɺ (t ) = f ( X , U , t ) + G (t ) W (t )
Y = h ( X , t ) + V (t )
where f and h are "continuously differentiable" and

W (t ), V (t ) are uncorrelated, zero mean Gaussian
white noises.

Formulation
X (t ) ≜ Xˆ (t ) + Xɶ (t )
Taylor series expansion and neglecting HOTs
 ∂f 
f ( X ,U , t ) ≈ f ( Xˆ ,U , t ) +   Xɶ
 ∂X  Xˆ
 ∂f 
= f ( Xˆ ,U , t ) +   ( X − Xˆ )
Similarly,
 ∂X  Xˆ
 ∂h 
h( X , t ) = h( Xˆ , t ) +   ( X − Xˆ )
 ∂X  Xˆ
Formulation
 ∂f 
E [ f ( X , U , t ) ] = E  f ( Xˆ ,U , t )  +   ( E [ X ] − Xˆ )
 ∂X  Xˆ
= f ( Xˆ ,U , t ) +   ( Xˆ − Xˆ )
∂f
 ∂X  Xˆ
= f ( Xˆ ,U , t )
Similarly, E [ h( X , t )] = h( Xˆ , t )
ɺ
Observer dynamics: ( )
Xˆ (t ) = f Xˆ , U , t + K e ( t ) Y − h ( Xˆ , t ) 
Yˆ = h ( Xˆ , t )
Error Dynamics
Xɶ (t ) ≜ X (t ) − Xˆ (t )
Xɺɶ (t ) ≜ Xɺ (t ) − Xɺˆ (t )
{
= { f ( X ,U , t ) + GW } − f ( Xˆ ,U , t ) + K e  h( X , t ) + V − h( Xˆ , t )  }
=  f ( X ,U , t ) − f ( Xˆ ,U , t )  − K e  h( X , t ) − h( Xˆ , t ) 
+ GW − K eV
 ∂f   ∂h 
=   Xɶ − K e   Xɶ + GW − K eV

∂X  Xˆ  ∂X  Xˆ

A( t ) C (t )

Error Dynamics
Xɺɶ (t ) = [ A(t ) − K e (t )C (t )] Xɶ + GW − K eV

A0 ( t )
This error dynamics is EXACTLY SAME as the error dynamics derived

for the time-varying linear system case. Hence, the dynamics for the
error Covariance matrix is given by
Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

 ∂f   ∂h 
where, A(t ) =   ; C (t ) =  
 ∂X  Xˆ  ∂X  Xˆ
Summary (Continuous EKF)
Xɺ (t ) = f ( X , U , t ) + G (t ) W (t )
Model
Y = h ( X , t ) + V (t )
Initialization
Xˆ ( t 0 ) = Xˆ 0
P0 = E  Xɶ ( t 0 ) Xɶ T ( t 0 ) 
Gain
Computation K e (t ) = P (t )C T (t ) R −1 (t )

Summary
ɺ
Xˆ (t ) = f ( Xˆ ,U , t ) + K e (t ) Y − h( Xˆ , t ) 
Propagation Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

 ∂f   ∂h 
where, A(t ) =   ; C (t ) =  
 ∂X  Xˆ  ∂X  Xˆ

Continuous-
Continuous-Discrete EKF

Motivation: System dynamics is a continuous-time, whereas
measurements are available only at discrete interval of time.
Strategy:
Without the availability of measurement, propagate the state

and co-variance dynamics from Xˆ k+ → Xˆ k−+1 and Pk+ → Pk−+1
respectively, using the "nonlinear system dynamics" and
linear co-variance dynamics.
As soon as the measurement is available, update
Xˆ k− → Xˆ k+ and Pk− → Pk+ respectively.
Summary:
Xɺ (t ) = f ( X , U , t ) + G (t ) W (t )
Model
Y = h ( X k ) + Vk
Xˆ − ( t 0 ) = X 0
Initialization
P0− = E  Xɶ − ( t 0 ) Xɶ −T
( t 0 ) 
−1
Gain
K e k ( t ) = Pk− C k− T  C k− Pk− C k− T + R 
Computation  ∂h 
w h e re, C k− = 
 ∂ X  Xˆ k−
Summary:
Xˆ = Xˆ + K  Y − h ( Xˆ ) 
+ − −

k k

ek k k
Updation (
Pk+ = I − K ek C k )P (I − K
k
−
ek Ck )
T
+ K ek R k K eTk
( preferable )
(
= I − K ek C k )P k
−
( not preferable )
ɺ
Propagation
Xˆ (t ) = f ( Xˆ , U , t ); Xˆ k+ → Xˆ k−+1
Pɺ (t ) = AP + PAT + GQG T ; Pˆk+ → Pˆk−+1
where A(t ) = 
∂f 
 ∂X  Xˆ ( t )
Iterated EKF
One way of improving the performance of EKF is to apply local

iterations to repeatedly calculate Xˆ k+ , Pˆk+ and K e , each time by
k
linearising about the most recent estimate. This approach is known

as Ìterative EKF'.
Note: One can proceed with a fixed number of iterations.

Linearized Kalman Filter (LKF)

This approach involves linearization about a nominal state trajectory
X (t ) (which is selected a priori), instead of the current estimate Xˆ (t ).
In such a situation,
f ( X , U , t ) = f ( X , U , t ) + A(t ) [ X − X ]
h( X , U , t ) = h( X , U , t ) + C (t ) [ X − X ]
 ∂f   ∂h 
where, A(t ) =   ; C (t ) =  
 ∂X  X  ∂X  X

E [ f ( X , U , t ) ] = f ( X , U , t ) + A(t )  Xˆ − X 
E [ h( X , U , t ) ] = h( X , U , t ) + C (t )  Xˆ − X 
Hence, the LKF has the following structure
ɺ
Xˆ (t ) = f ( X , U , t ) + A ( t )  Xˆ − X  +
K e (t ) Y − h ( X , U , t ) − C (t )  Xˆ − X  
 
Yˆ = h ( X , U , t ) + C (t )  Xˆ − X 


The Co-variance matrix can be derived to be
Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

 ∂f   ∂h 
where, A(t ) =   ; C (t ) =  
 ∂X  X (t )  ∂X  X (t )
X (t )is the nominal state

Comments
In general, LKF is less accurate than EKF, since X (t ) is usually
not close to X (t ) as Xˆ (t ) is.
LKF can, however, lead to the a priori computation of K e (t ),
which can then be stored and used with less online computation.
To avoid the large initial chattering of EKF, one can start with
LKF and then swith to EKF. However, conce its often possible
to start the EKF ahead of time, this necessity normally does not arise.
The philosophy of local iterations can also be implemented with LKF,
leading to Ìterated LKF'.
Recommendations/Issues in EKF
Design parameter selection:
o Fix R based on the sensor characteristics
o Select P0 to be “sufficiently high”
o Tune Q until obtaining satisfactory results
The filter should run sufficiently ahead of time prior to its usage, so
that the error stabilizes before its actual usage (else, initial error can
be very large and the associated control can destabilize the closed
loop system)
Keep the measurement equation linear wherever possible
Care should be taken to avoid numerical ill-conditioning. Methods
are available to address this issue (see Crassidis and Jenkins book).

Recommendations/Issues in EKF
Care should be taken to eliminate the òutliers’. For e.g., if the
measurement output is too far away from the predicted output, it
can be treated as an outlier. Data-rejection methods are available.
EKF is `fragile’, i.e., only a narrow band of design variables P0, R,
and Q exists for its success. Hence, tuning is necessary for any
given application and the tuning process should be done very
carefully.
Checks for Consistency of Kalman Filter :
Sigma-bound test
Normalized Error Square (NES) test
Normalized Mean Error (NME) test
Autocorrelation Test
Cramer-Rao Inequality (gives a “lower bound” on error)

Difficulties in Practice
Computer round-off errors
Unchecked error propagation
Asymmetry of covariance matrix: A
symptom of numerical degradation
Solution of Riccati equation
• Square-root filtering: Solution of Riccati
equation via Cholesky factorization
Large initial errors: Information filtering
Point to Remember:
Nonlinear estimation problems are considerably more difficult than
the linear problem in general (EKF is just and idea…not a cure for
everything!).
The problem with nonlinear systems is that a Gaussian input does

not necessarily produce a Gaussian output (unlike linear case).
The EKF even though not truly òptimum’, has been successfully
applied in many nonlinear systems over the decades.
The fundamental assumption in EKF design is that the
true state X (t ) is sufficiently close to the estimated state Xˆ (t )
at all time, and hence the error dynamics can be represented
fairly accurately by the linearized system about Xˆ (t )

Limitations of EKF
Linearization can introduce significant error
No general convergence guarantee
Works in general; but in some cases its

performance can be surprisingly bad
Unreliable for colour noise (shaping filter

philosophy need not hold good in general)
Beyond EKF
Need
• Nonlinear systems
• Non-Gaussian noise
• Correlated (colour) noise
Characteristics of Such Filters

• Are often approximate
• Sacrifices theoretical accuracy in favour of practical
constraints and considerations like robustness,
adaptation, numerical feasibility
• Attempt to cover the limitations of EKF

Unscented Kalman Filter (UKF)

Motivation
Tuning difficulties in EKF
Change of characteristics of noise PDF
through nonlinear transformation
Gaussian distribution properties can be
propagated easily rather than co-
variance matrix
Approximation of higher order moment of
the distribution after transformation
Transformation of PDF:
A pictorial description

A Simple Example
y1 = r cos θ
y2 = r sin θ
Reference: D. Simon,
Optimal State Estimation,
Wiley, 2006
A comparison of the exact, linearized, and unscented mean and covariance

of 300 randomly generated points with r uniformly distributed between
±0.01 and θɶ uniformly distributed between ± 0.35 radians.
System Dynamics
Reference: D. Simon, Optimal State Estimation, Wiley, 2006.

Sigma Point Approach
Reference: Merwe et al., Sigma-Point Kalman Filters for

Nonlinear Estimation and Sensor-Fusion: Applications to
Integrated Navigation, AIAA-2004-5120

UKF Implementation: Propagation of

State Estimation and Covariance
Initialization:
Selection of σ -points:
Note :
( A)T
A=A

UKF Implementation: Propagation of
State Estimation and Covariance
Transformation of σ -points:
Calculate new mean and covariance:

UKF Implementation:
Measurement Update
Selection of σ -points:
Note: previously generated sigma points can also be used.

However, that’ll lead to some performance degradation.

UKF Implementation:
Measurement Update
Transformation of σ -points:
Predicted measurements

UKF Implementation: Gain Computation,

State & Covariance Update

Advantage vs.
Computational Effort
Reference: D. Simon, Optimal State Estimation, Wiley, 2006

References: Books
J. L. Crassidis and J. L. Junkins, Optimal Estimation of
Dynamic Systems, CRC Press, 2004. Second Edition:
2012.
D. Simon, Optimal State Estimation, Wiley, 2006.
P. Zarchan and H. Musoff, Fundamentals of Kalman
Filtering: A Practical Approach, AIAA, 2005.
F. L. Lewis, L. Xie and D. Popa, Optimal and Robust
Estimation, CRC Press, 2007.
A. Gelb, Applied Optimal Estimation, MIT Press, 1974.
M. S. Grewal and A. P. Andrews, Kalman Filtering:
Theory and Practice, Wiley, 2001.

References: Publications
R. E. Kalman, A New Approach to Linear Filtering and
Prediction Problems, Transactions of the ASME – J. of
Basic Engineering, Vol.82 (Series D), pp. 35-45.
(THIS IS THE ORIGINAL FAMOUS PAPER)
M. R. Ananthasayanam, Fascinating Perspectives of
State and Parameter Estimation Techniques,
Proceedings of AIAA GNC Conference, AIAA-2000-4319.
M. R. Ananthasayanam, A Re-look at the Concepts and
Competance of the Kalman Filter, Proceedings of AIAA
Aerospace Sciences Meeting and Exhibit, 2004, AIAA-
2004-571.

Thanks for the Attention….!!


Kalman Kalman Kalman Kalman Filter Design Filter Design Filter Design Filter Design - Iii III III III

Загружено:

Сведения о документе

Исходное описание:

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Kalman Kalman Kalman Kalman Filter Design Filter Design Filter Design Filter Design - Iii III III III

Загружено:

Авторское право:

Доступные форматы

Optimal Control, Guidance and Estimation

Kalman Filter Design – III

Prof. Radhakant Padhi

Prof. Radhakant Padhi

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 3

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 4

Correct the value Xˆ k−+1 to Xˆ k++1 using the measurement vector Yk +1

Measurement is available only at discrete time-steps. Hence, the

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 8

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 9

Pɺ (t )expression is a continuous time Lyapunov Equation

Continuous-discrete Kalman filter facilitates the usage of

Use Xɺˆ (t ) and Pɺ (t ) expressions to propagate

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 10

Prof. Radhakant Padhi

The problem with nonlinear systems is that a Gaussian input doest

Prof. Radhakant Padhi

where f and h are "continuously differentiable" and

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 14

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 17

This error dynamics is EXACTLY SAME as the error dynamics derived

Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 19

Propagation Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 20

Prof. Radhakant Padhi

Without the availability of measurement, propagate the state

One way of improving the performance of EKF is to apply local

linearising about the most recent estimate. This approach is known

Note: One can proceed with a fixed number of iterations.

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 25

Linearized Kalman Filter (LKF)

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 26

Hence, the LKF has the following structure

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 27

Linearized Kalman Filter (LKF)

Pɺ (t ) = AP + PAT − PC T R −1CP + GQG T

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 28

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 30

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 31

The problem with nonlinear systems is that a Gaussian input does

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 33

No general convergence guarantee

Works in general; but in some cases its

Unreliable for colour noise (shaping filter

Characteristics of Such Filters

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 35

Unscented Kalman Filter (UKF)

Prof. Radhakant Padhi

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 38

A comparison of the exact, linearized, and unscented mean and covariance

Reference: D. Simon, Optimal State Estimation, Wiley, 2006.

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 40

Reference: Merwe et al., Sigma-Point Kalman Filters for

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 41

UKF Implementation: Propagation of

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 42

Calculate new mean and covariance:

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 43

Note: previously generated sigma points can also be used.

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 44

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION 45