Optimal Control, Guidance and Estimation
Lecture – 30
Kalman Filter Design – III
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
ContinuousContinuous-Discrete Kalman Filter
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
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 − τ )
E Vk V jT  = Rk δ kj
0 k ≠ j 

1 k = j 
δ kj = 
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
3
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)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
4
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
5
Expression for Pɺ
Xɺ = AX + BU + GW
ɺ
Xˆ = AXˆ + BU
⇒
ɺ
Xɺɶ = Xɺ − Xˆ
= A Xɶ + GW
t
Xɶ (t ) = ϕ (t , t0 ) Xɶ 0 + ∫ ϕ (t , t0 ) G (τ )W (τ ) dτ
0
RWXɶ
t

= E  ∫ W (t ) W T (τ ) G (τ ) ϕ (t ,τ ) dτ 
0

t
= ∫ Q δ (t − τ ) G T (τ ) ϕ (t ,τ ) dτ = 12 Q G T
0
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
6
Expression for Pɺ
(
ɺ
ɺ
ɺ
ɺ
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
)
T
T
Pɺ = ( AP + 12 GQG T ) + ( AP + 12 GQG T )
Pɺ = AP + PAT + GQGT
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
7
Summary
Model
Xɺ (t ) = A (t ) X ( t ) + B (t )U (t ) + G (t ) W (t )
Yk = C k X k + Vk
Initialization
Xˆ ( t 0 ) = Xˆ 0
P0− = E  Xɶ ( t 0 ) Xɶ T ( t 0 ) 
Gain
Computation
K e k = Pk− C k T  C k Pk− C k T + R k 
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
8
−1
Summary
Xˆ k+ = Xˆ k− + K ek  Y k − C k Xˆ k− 
Updation
(
) P (I − K
(
)P
Pk+ = I − K ek C k
= I − K ek C k
Propagation
(using high
accuracy numerical
integration)
−
k
k
−
ek
Ck
T
)
+ K ek R k K eTk
( preferable )
( not preferable )
ɺ
Xˆ = AXˆ + BU
Pɺ (t ) = AP + PAT + GQG T
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
9
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
10
Extended Kalman Filter (EKF)
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
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 `optimum’, 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 )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
12
ContinuousContinuous-Continuous EKF
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
14
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ˆ )
 ∂X  Xˆ
Similarly,
 ∂h 
h( X , t ) = h( Xˆ , t ) +   ( X − Xˆ )
 ∂X  Xˆ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
15
Formulation
 ∂f 
E [ f ( X , U , t ) ] = E  f ( Xˆ ,U , t )  +   ( E [ X ] − Xˆ )
 ∂X  Xˆ
∂f
= f ( Xˆ ,U , t ) +   ( Xˆ − Xˆ )
 ∂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 )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
16
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 )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
17
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ˆ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
18
Summary (Continuous EKF)
Model
Xɺ (t ) = f ( X , U , t ) + G (t ) W (t )
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 )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
19
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ˆ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
20
ContinuousContinuous-Discrete EKF
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
22
Summary:
Continuous-Discrete EKF
Xɺ (t ) = f ( X , U , t ) + G (t ) W (t )
Model
Initialization
Gain
Computation
Y = h ( X k ) + Vk
Xˆ − ( t 0 ) = X 0
P0− = E  Xɶ − ( t 0 ) Xɶ
−T
( t 0 ) 
K e k ( t ) = Pk− C k− T  C k− Pk− C k− T + R 
 ∂h 
w h e re, C k− = 
 ∂ X  Xˆ k−
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
−1
23
Summary:
Continuous-Discrete EKF
Xˆ = Xˆ + K  Y − h ( Xˆ ) 


+
k
Updation
−
k
ek
(
)P (I − K
(
)P
Pk+ = I − K ek C k
= I − K ek C k
Propagation
−
k
k
−
k
k
−
ek
T
)
Ck
+ K ek R k K eTk
( preferable )
( not preferable )
ɺ
Xˆ (t ) = f ( Xˆ , U , t );
Xˆ k+ → Xˆ k−+1
Pɺ (t ) = AP + PAT + GQG T ; Pˆk+ → Pˆk−+1
∂f 
where A(t ) = 
 ∂X  Xˆ ( t )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
24
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 `Iterative EKF'.
Note: One can proceed with a fixed number of iterations.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
25
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
26
Linearized Kalman Filter (LKF)
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 
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
27
Linearized Kalman Filter (LKF)
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
28
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 `Iterated LKF'.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
29
Recommendations/Issues in EKF
Design parameter selection:
o
o
o
Fix R based on the sensor characteristics
Select P0 to be “sufficiently high”
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).
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
30
Recommendations/Issues in EKF
Care should be taken to eliminate the `outliers’. 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)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
31
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
32
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 `optimum’, 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 )
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
33
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)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
34
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
Unscented Kalman Filter (UKF)
Prof. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
35
Motivation
Tuning difficulties in EKF
Change of characteristics of noise PDF
through nonlinear transformation
Gaussian distribution properties can be
propagated easily rather than covariance matrix
Approximation of higher order moment of
the distribution after transformation
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
37
Transformation of PDF:
A pictorial description
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
38
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
39
System Dynamics
Reference: D. Simon, Optimal State Estimation, Wiley, 2006.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
40
Sigma Point Approach
Reference: Merwe et al., Sigma-Point Kalman Filters for
Nonlinear Estimation and Sensor-Fusion: Applications to
Integrated Navigation, AIAA-2004-5120
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
41
UKF Implementation: Propagation of
State Estimation and Covariance
Initialization:
Selection of σ -points:
Note :
( A)
T
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
A=A
42
UKF Implementation: Propagation of
State Estimation and Covariance
Transformation of σ -points:
Calculate new mean and covariance:
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
43
UKF Implementation:
Measurement Update
Selection of σ -points:
Note: previously generated sigma points can also be used.
However, that’ll lead to some performance degradation.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
44
UKF Implementation:
Measurement Update
Transformation of σ -points:
Predicted measurements
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
45
UKF Implementation: Gain Computation,
State & Covariance Update
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
46
Advantage vs.
Computational Effort
Reference: D. Simon, Optimal State Estimation, Wiley, 2006
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
47
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
48
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, AIAA2004-571.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
49
Thanks for the Attention….!!
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore
50