Observers and state estimation Starting point Continuous-time - PowerPoint PPT Presentation

Observers and state estimation Starting point ◮ Continuous-time system: x ( t ) = Ax ( t ) + Bu ( t ) + Nv 1 ( t ) , ˙ y ( t ) = Cx ( t ) + Du ( t ) + v 2 ( t ) . ◮ Discrete-time system: x ( k + 1) = Fx ( k ) + Gu ( k ) + Nv 1 ( k ) , y ( k ) = Hx ( k ) + Ju ( k ) + v 2 ( k ) . ◮ v 1 and v 2 are zero mean white noise, with intensities according to � R 1 � v 1 � � R 12 η = ⇒ Φ η ( ω ) = R η = . R T v 2 R 2 12 1 / 8 hans.norlander@it.uu.se Kalman

Continuous-time observers The estimation error ◮ The observer: ˙ ˆ x ( t ) = A ˆ x ( t ) + Bu ( t ) + K ( y ( t ) − C ˆ x ( t ) − Du ( t )) ◮ The estimation error ˜ x = x − ˆ x is governed by ˙ x ( t ) = ( A − KC )˜ ˜ x ( t ) + Nv 1 ( t ) − Kv 2 ( t ) � � = ( A − KC )˜ x ( t ) + − K η ( t ) N � �� � = B η ◮ ˜ x ( t ) is a stationary stochastic process if A − KC is stable. x T ( t ) , is ◮ The covariance of the estimation error, Π ˜ x = E ˜ x ( t )˜ given by the continuous-time Lyapunov equation x ( A − KC ) T + B η R η B T 0 = ( A − KC )Π ˜ x + Π ˜ η . 2 / 8 hans.norlander@it.uu.se Kalman

Discrete-time observers The estimation error ◮ The observer: x ( k + 1) = F ˆ ˆ x ( k ) + Gu ( k ) + K ( y ( k ) − H ˆ x ( k ) − Ju ( k )) ◮ The estimation error ˜ x = x − ˆ x is governed by x ( k + 1) = ( F − KH )˜ ˜ x ( k ) + Nv 1 ( k ) − Kv 2 ( k ) � � = ( F − KH )˜ x ( k ) + − K η ( k ) N � �� � = G η ◮ ˜ x ( k ) is a stationary stochastic process if F − KH is stable. x T ( k ) , is ◮ The covariance of the estimation error, Π ˜ x = E ˜ x ( k )˜ given by the discrete-time Lyapunov equation x ( F − KH ) T + G η R η G T Π ˜ x = ( F − KH )Π ˜ η . 3 / 8 hans.norlander@it.uu.se Kalman

The Kalman filter Continuous-time systems (Thm. 5.4) ◮ The optimal observer minimizes Π ˜ x . ◮ Introduce the notation P = min K Π ˜ x . ◮ The optimal observer gain is K = ( PC T + NR 12 ) R − 1 2 . ◮ The optimal observer is called the Kalman filter . ◮ P = P T ≥ 0 is the solution to the continuous-time algebraic Riccati equation (CARE) 0 = AP + PA T + NR 1 N T − ( PC T + NR 12 ) R − 1 2 ( PC T + NR 12 ) T ◮ Technical conditions: ◮ ( A, C ) detectable ⇔ ∃ K such that A − KC is stable, ◮ R 2 = R T 2 > 0 , ◮ R η = R T η ≥ 0 . 4 / 8 hans.norlander@it.uu.se Kalman

The Kalman filter Discrete-time systems (Thm. 5.6) ◮ Notation: ˆ x ( k | l ) is the estimate of x ( k ) based on measurements up to the time l , i.e. . . . , y ( l − 2) , y ( l − 1) , y ( l ) ◮ The “standard” observer is x ( k +1 | k ) = F ˆ ˆ x ( k | k − 1)+ Gu ( k )+ K ( y ( k ) − H ˆ x ( k | k − 1) − Ju ( k )) ◮ Introduce the notation P = min K Π ˜ x ( k | k − 1) . ◮ The optimal observer gain is K = ( FPH T + NR 12 )( HPH T + R 2 ) − 1 . ◮ P = P T ≥ 0 is the solution to the discrete-time algebraic Riccati equation (DARE) P = FPF T + NR 1 N T − ( FPH T + NR 12 )( HPH T + R 2 ) − 1 ( FPH T + NR 12 ) T 5 / 8 hans.norlander@it.uu.se Kalman

The Kalman filter Discrete-time systems, cont’d ◮ ˆ x ( k + 1 | k ) is the optimal estimate of x ( k + 1) , given the measurements . . . , y ( k − 1) , y ( k ) . Thus ˆ x ( k + 1 | k ) is the optimal one-step predictor . ◮ Refine the estimate of x ( k ) by use of the present measurement y ( k ) , i.e. get the optimal ˆ x ( k | k ) , obtained as x ( k | k − 1) + ˜ x ( k | k ) = ˆ ˆ K ( y ( k ) − H ˆ x ( k | k − 1) − Ju ( k )) where K = PH T ( HPH T + R 2 ) − 1 . ˜ ◮ Predict m time-steps ahead — the optimal m -step predictor: m − 1 � F m − 1 − l Gu ( k + l ) x ( k + m | k ) = F m ˆ ˆ x ( k | k ) + l =0 6 / 8 hans.norlander@it.uu.se Kalman

Innovations and innovations form ◮ Innovations: The update/feedback ν ( t ) = y ( t ) − C ˆ x ( t ) − Du ( t ) = y ( t ) − ˆ y ( t ) in the Kalman filter is called the output innovations . (For discrete-time: ν ( k ) = y ( k ) − ˆ y ( k | k − 1) .) ◮ Innovations form: A state space representation of the form x ( t ) = Ax ( t ) + Bu ( t ) + Nv ( t ) , ˙ y ( t ) = Cx ( t ) + Du ( t ) + v ( t ) , is in innovations form if A − NC is stable. The Kalman filter can be written in innovations form, by the replacements ◮ ˆ x ↔ x , ◮ K ↔ N , ◮ ν ↔ v . 7 / 8 hans.norlander@it.uu.se Kalman

Properties of the Kalman filter The Kalman filter ◮ is a linear filter: � y ( t ) � � y ( k ) � x ( t ) = F ( p ) ˆ x ( k | k − 1) = F ( q ) ˆ , u ( t ) u ( k ) ◮ is always stable ◮ makes the innovations ν white noise with intensity ◮ R 2 for continuous-time systems ◮ HPH T + R 2 for discrete-time systems ◮ is the optimal linear filter for state estimation ◮ is the optimal state estimator if v 1 and v 2 are Gaussian ◮ is an observer, and R 1 , R 2 and R 12 may be regarded as design parameters — trade-off between convergence speed and noise sensitivity 8 / 8 hans.norlander@it.uu.se Kalman