You are on page 1of 31

# Optimal States Estimation

Kalman and H Infinity Approaches

Haocheng Li
Department of Aerospace Engineering
Worcester Polytechnic Institute

ECE 5311 Presentation, 2016

Haocheng Li (Worcester Polytechnic Institute)

Optimal States Estimation

ECE 5311 Presentation, 2016

1 / 31

Outline

1

Least Square Estimation
Constant Estimations
Recursive Least Square Estimation

2

Kalman Filter
Uncorrelated Process and Measurement Noise

3

H Infinity Filter
Dynamic Constrained Optimization
A Game Approach to H Infinity Filter

Haocheng Li (Worcester Polytechnic Institute)

Optimal States Estimation

ECE 5311 Presentation, 2016

2 / 31

Outline

1

Least Square Estimation
Constant Estimations
Recursive Least Square Estimation

2

Kalman Filter
Uncorrelated Process and Measurement Noise

3

H Infinity Filter
Dynamic Constrained Optimization
A Game Approach to H Infinity Filter

Haocheng Li (Worcester Polytechnic Institute)

Optimal States Estimation

ECE 5311 Presentation, 2016

3 / 31

Constant Estimations Measurement System y = Hx + v x the unknown constant v the measurement noise H the system matrix determined by sensor setup y the measurement output ˆx the estimation of x y = y − Hˆx measurement residue Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 4 / 31 .

2016 5 / 31 . the optimal estimation of the x is ˆ x = (HT H)−1 HT y Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. Define the cost function of the estimator to be J = T y y Expand the cost function as follow J = (y − Hˆ x)T (y − Hˆ x) = yT y − yT Hˆ x−ˆ xT HT y + xˆT HT Hˆx To minimize the cost function ∂J = −2yT H + ˆ xT HT H = 0 ∂ˆx Therefore.Constant Estimations Theorem The most probable value of x is the ˆ x that minimizes the norm of the measurement residue y .

2016 6 / 31 .Outline 1 Least Square Estimation Constant Estimations Recursive Least Square Estimation 2 Kalman Filter Uncorrelated Process and Measurement Noise 3 H Infinity Filter Dynamic Constrained Optimization A Game Approach to H Infinity Filter Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.

2016 7 / 31 . Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.Recursive Least Square Estimation Recursive Measurement System yk = Hk x + vk Suppose the recursive estimator of x satisfies ˆxk = ˆ xk−1 + Kk (yk − Hˆ xk−1 ) Notice that the recursive estimation error mean E (k ) = E (x − ˆ xk )   =E x−ˆ xk−1 − Kk (yk − Hˆ xk−1 )   =E x−ˆ xk−1 − Kk (Hk x + vk − Hˆxk−1 ) The resulting error mean equation is E (k ) = (I − Kk Hk )E (k−1 ) − Kk E (vk ) The estimation mean error is unbiased.

Recursive Least Square Estimation Define the optimal criterion as the variance of the estimation error     T Jk = E (T  ) = E Tr(  ) = E Tr(P ) k k k k k The covariance matrix of the estimation error is Pk = E (k T k ) !   T =E (I − Kk Hk )k−1 − Kk vk (I − Kk Hk )k−1 − Kk vk T T T = (I − Kk Hk )E (k−1 T k−1 )(I − Kk Hk ) + Kk E (vk vk )Kk T T T − Kk E (vk T k−1 )(I − Kk Hk ) − (I − Kk Hk )E (k−1 vk )Kk Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 8 / 31 .

the gain update equation is T −1 Kk = Pk−1 HT k (Hk Pk−1 Hk + Rk ) Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. the optimal estimation gain can be computed as ∂Tr(Pk ) ∂J = ∂Kk ∂Kk = 2(I − Kk Hk )Pk−1 (−Hk ) + 2Kk Rk =0 Then. Therefore.The measurement noise and previous estimation is independent T E (vk T k−1 ) = E (k−1 vk ) = 0 since the error mean is zero. the recursive estimation variance equation is Pk = (I − Kk Hk )Pk−1 (I − Kk Hk )T + Kk Rk KT k Therefore. 2016 9 / 31 .

2016 10 / 31 .Summary Recursive Least Square Estimation Initialization: ˆx0 = E (x)   P0 = E (x − ˆ x0 )(x − ˆ x0 )T Measurement: yk = Hk x + vk E (vk ) = 0 E (vi vkT ) = Rk δi−k Update Estimation: T −1 Kk = Pk−1 HT k (Hk Pk−1 Hk + Rk ) ˆxk = ˆxk−1 + Kk (yk − Hˆ xk−1 ) Pk = (I − Kk Hk )Pk−1 (I − Kk Hk )T + Kk Rk KT k Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.

Outline 1 Least Square Estimation Constant Estimations Recursive Least Square Estimation 2 Kalman Filter Uncorrelated Process and Measurement Noise 3 H Infinity Filter Dynamic Constrained Optimization A Game Approach to H Infinity Filter Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 11 / 31 .

Qk ) and vk ∼ (0. Rk ) The random process is Markov: E (wk wjT ) = Qk δj−k and E (vk vjT ) = Rk δj−k The process and measurement noise is uncorrelated: E (wk vjT ) = 0 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 12 / 31 .Uncorrelated Process and Measurement Noise Linear Discrete Time System xk = Fk−1 xk−1 + wk−1 yk = H k xk + vk The process and measurement noise is Gaussian: wk ∼ (0.

yk−1 ) ˆ+ a posteriori estimate: x k = E (xk |y1 . y2 . · · · . yk )   − − T ˆ ˆ priori estimation error covariance: P− = E (x − x )(x − x ) k k k   + + T ˆ ˆ posteriori estimation error covariance: P+ = E (x − x )(x − x ) k k k Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. · · · . 2016 13 / 31 . y2 .a priori estimate: xˆ− k = E (xk |y1 .

Propagation of the states and Covariance For a linear discrete system xk =Fk−1 xk−1 + wk−1 The average of the states propagates as follow ¯ xk =Fk−1 ¯ xk−1 The covariance matrix of the estimation error propagates as follow   xk )T Pk = E (xk − ¯xk )(xk − ¯   T ) = Fk−1 E (xk−1 − ¯ xk−1 )(xk−1 − ¯ xk−1 )T Fk−1 + E (wk−1 wk−1     T + Fk−1 E (xk−1 − ¯xk−1 )wk−1 + E wk−1 (xk−1 − ¯xk−1 )T FT k−1 The resulting propagation equation is Pk = Fk−1 Pk−1 Fk−1 + Qk−1 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 14 / 31 .

a priori estimation of current step can be obtained from the posterior estimation from the previous step using the propagation equation: ˆx− x+ k = Fk−1 ˆ k−1 + P− k = Fk−1 Pk−1 Fk−1 + Qk−1 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.Hence. 2016 15 / 31 .

Initialization: ˆx+ 0 = E (x0 )   T P+ x+ x+ 0 = E (x0 − ˆ 0 )(x0 − ˆ 0) Estimation: Prediction of the estimation covariance matrix: + P− k = Fk−1 Pk−1 Fk−1 + Qk−1 Computation of the estimation gain − T T −1 Kk = P− k Hk (Hk Pk Hk + Rk ) Priori Estimation (Prediction) ˆ x− x+ k = Fk−1 ˆ k−1 Posterior Estimation (Correction) ˆ x+ x− x− k =ˆ k + Kk (yk − Hˆ k) Posterior Covariance − T T P+ k = (I − Kk Hk )Pk (I − Kk Hk ) + Kk Rk Kk Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 16 / 31 .

2016 17 / 31 .Outline 1 Least Square Estimation Constant Estimations Recursive Least Square Estimation 2 Kalman Filter Uncorrelated Process and Measurement Noise 3 H Infinity Filter Dynamic Constrained Optimization A Game Approach to H Infinity Filter Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.

2016 18 / 31 . wk ) k=0 subject to the system dynamics for N steps xk+1 = Fk xk + wk Haocheng Li (Worcester Polytechnic Institute) (k = 0.Dynamic Constrained Optimization Dynamic Constrained Optimization Problem Suppose the cost function to be J = ψ(x0 ) + N−1 X Lk (xk . N − 1) Optimal States Estimation ECE 5311 Presentation. · · · .

wk ) + λT k+1 (Fk xk  N−1 X  k=0 N X + wk ) − Lk (xk . wk ) + λT (F x + w − x ) k k+1 k+1 k k Lk (xk . the augmented cost function is Ja = ψ(x0 ) + = ψ(x0 ) + = ψ(x0 ) + N−1 X k=0 N−1 X k=0 N−1 X  Lk (xk . the augmented cost function can be rewritten as Ja = ψ(x0 ) + N−1 X T T (Hk − λT k xk ) − λN xN + λ0 x0 k=0 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.Applying the Lagrange multiplier. 2016 19 / 31 . wk ) + λT k+1 (Fk xk + wk ) − k=0 λT k+1 xk+1 T λT k xk + λ0 x0 k=0 Define the Hamiltonian of the system dynamics: Hk = Lk (xk . wk ) + λT k+1 (Fk xk + wk ) Therefore.

N) The third condition yields the dynamic constrained and the first two conditions can be simplified as follow: ∂ψ =0 ∂x0 −λT N =0 ∂Hk λT (k = 1. N) (k = 0. 2016 20 / 31 . N) ∂wk λT 0 + Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. N − 1) k = ∂xk ∂Hk =0 (k = 0. · · · .The necessary conditions for optimality is ∂Ja =0 ∂xk ∂Ja =0 ∂wk ∂Ja =0 ∂λk (k = 0. · · · . · · · . · · · . N − 1) (k = 0. · · · .

Outline 1 Least Square Estimation Constant Estimations Recursive Least Square Estimation 2 Kalman Filter Uncorrelated Process and Measurement Noise 3 H Infinity Filter Dynamic Constrained Optimization A Game Approach to H Infinity Filter Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 21 / 31 .

2016 22 / 31 .Problem Statement Estimation Problem The system dynamics and measurement model is as follow: xk+1 = Fk xk + wk yk = Hk xk + vk The goal is to estimate a linear combination of states zk = Lk xk Cost Function The performance criterion PN−1 J1 = kx0 − ˆx0 k2 −1 P0 Haocheng Li (Worcester Polytechnic Institute) k=0 kzk P + N−1 k=0 − ˆzk k2Sk kwk k2 −1 + kvk k2 −1 Optimal States Estimation Qk  Rk ECE 5311 Presentation.

seek the a performance bound such that 1 J1 ≤ θ which is equivalent to the following N−1 X  1 1 − kx0 − ˆx0 k2P−1 + kzk − ˆzk k2Sk − kwk k2Q−1 + kvk k2R−1 ≤ 0 0 θ θ k k k=0 Consider the alternative form of the cost function N−1 X  1 1 J = − kx0 − ˆx0 k2P−1 + kzk − ˆzk k2Sk − kwk k2Q−1 + kvk k2R−1 0 θ θ k k k=0 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.The direct minimization is not tractable. therefore. 2016 23 / 31 .

process and measurement noise to maximize the cost We need to choose the estimator to minimize the worst case scenario Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 24 / 31 .x0 Nature can choose the initial condition.vk .The minimax problem The minimax problem Seek estimator that minimize the worst case scenario J ∗ = min max J ˆ zk wk .

The minimax problem is equivalent to J ∗ = min max J ˆ zk wk .yk .x0 The cost function can be transformed to the equivalent form as N−1 X  1 1 kxk − ˆ xk k2S¯ − kwk k2Q−1 + kyk − Hk xk k2R−1 J = − kx0 − ˆx0 k2P−1 + k 0 θ θ k k k=0 = ψ(x0 ) + N−1 X Lk k=0 The Hamiltonian of the system is defined as Hk = Lk + Haocheng Li (Worcester Polytechnic Institute) 2λT k+1 (Fk xk + wk ) θ Optimal States Estimation ECE 5311 Presentation.x0 = min max J ˆ xk wk .vk .vk . 2016 25 / 31 .x0 = min max J ˆ xk wk .

Stationary with respect to x0 and wk The conditions of the stationary points are ∂ψ(x0 ) 2λT 0 + θ ∂x0 2λT N θ ∂Hk ∂wk 2λT k θ Haocheng Li (Worcester Polytechnic Institute) =0 =0 =0 = ∂Hk ∂xk Optimal States Estimation ECE 5311 Presentation. 2016 26 / 31 .

2016 27 / 31 .the stationary conditions are x0 = ˆx0 + P0 λ0 xk = µk + Pk λk wk = Qk λk+1 λN = 0  ¯ k Pk + HT R−1 Hk Pk −1 FT λk+1 λk = I − θS k k k  −1 −1 ¯ k (µk − ˆxk ) ¯ k Pk + HT R Hk Pk θS + I − θS k k  ¯ k Pk + HT R−1 Hk Pk −1 HT R−1 (yk − Hk µk ) + I − θS k k k k −1 T −1 −1 T ¯ Pk+1 = Fk (P − θSk + H R Hk ) F + Qk k k k k µ0 = ˆx0  ¯ k Pk + HT R−1 Hk Pk −1 θS ¯ k (µk − ˆxk ) µk+1 = Fk µk + Fk Pk I − θS k k  ¯ k Pk + HT R−1 Hk Pk −1 HT R−1 (yk − Hk µk ) + F k Pk I − θ S k k k k Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation.

the minimax problem become N−1 X  1 1 J = − kx0 − ˆx0 k2P−1 + kzk − ˆzk k2Sk − kwk k2Q−1 + kvk k2R−1 0 θ θ k k k=0 J ∗ = min max J ˆ xk yk Substitute the stationary conditions.Stationary with respect to yk and xˆk After maximizing with respect to the x0 and wk . the cost function is equivalent to the following quadratic form J= N−1 X ¯ k + θS ¯k P ˜kS ¯ k )(µk − ˆ (µk − ˆxk )T (S xk ) k=0 ¯k P ˜ k HT R−1 (yk − Hk µk ) + 2(µk − ˆxk )T S k k  1 −1 T −1 ˜ H P H R − R )(y − H µ ) + (yk − Hk µk )T (R−1 k k k k k k k k k θ Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 28 / 31 .

the stationary point is clearly satisfies by the following conditions ˆ xk = µ k yk = H k µ k Notice that the stationary condition implies that the cost function is zero J=0 thus. Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. the attenuation condition J1 ≤ 1 θ is satisfied.Therefore. 2016 29 / 31 . the stationary conditions are ∂J ¯ k + θS ¯k P ˜kS ¯ k )(µk − ˆ ¯k P ˜ k HT R−1 (yk − Hk µk ) = 0 = 2(S xk ) + 2 S k k ∂ˆ xk ∂J 2 ˜ k HT R−1 − R−1 )(yk − Hk µk ) + 2R−1 Hk P ˜ k (µk − ˆxk ) = 0 = (R−1 Hk P k k k k ∂yk θ k Therefore.

To ensure the estimator ˆ xk minimizing the cost function require that ∂2J ¯ k + θS ¯k P ˜kS ¯k ) > 0 = 2(S ∂ˆxk ¯ k is chosen to always to be positive definite. 2016 30 / 31 . the second Since the matrix S order condition can be reduced to ˜ k = (P−1 − θS ¯ k + HT R−1 Hk )−1 > 0 P k k k the optimal estimator should be ¯ k Pk + HT R−1 Hk Pk ˆ xk+1 = Fk ˆxk + Fk Pk I − θS k k Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation −1 −1 HT xk ) k Rk (yk − Hk ˆ ECE 5311 Presentation.

Suppose the system dynamics and measurement model are xk+1 = Fk xk + wk yk = Hk xk + vk zk = Lk xk with the performance criterion PN−1 J1 = kx0 − ˆx0 k2 −1 P0 k=0 kzk P + N−1 k=0 − ˆzk k2Sk kwk k2 −1 + kvk k2 −1 Qk ≤ Rk 1 θ then the estimation scheme is ¯ k = LT Sk Lk S k ¯ k Pk + HT R−1 Hk Pk K k = Pk I − θ S k k −1 −1 HT k Rk ˆxk+1 = Fk ˆxk + Fk Kk (yk − Hk ˆ xk ) −1 −1 T ¯ k + H R Hk )−1 FT + Qk Pk+1 = Fk (Pk − θS k k k with the positive definiteness criterion T −1 ¯ P−1 k − θ S k + H k Rk H k > 0 Haocheng Li (Worcester Polytechnic Institute) Optimal States Estimation ECE 5311 Presentation. 2016 31 / 31 .