Sequential State Estimation (Crassidas and Junkins, Chapter 5)

Transcription

1 Sequential State Estimation (Crassidas and Junkins, Chapter 5) Please read: 5.1, The Discrete-Time Kalman Filter The discrete-time Kalman filter is used when the dynamics and measurements are modeled using discrete difference equations and discrete algebraic equations. Design Model x k+1 = Φ k x k + Γ k u k + Υ k w k, w k N(0, Q k ) ỹ k = H k x k + v k, v k N(0, R k ) Initialization ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] Kalman Gain Update Equations K k = P k HT k [H k P k HT k + R k ] 1 ˆx + k = ˆx k + K k[ỹ k H kˆx k ] P + k = [I K kh k ]P k Propagation Equations ˆx k+1 = Φ kˆx k + Γ k u k P k+1 = Φ k P k Φ T k + Υ k Q k Υ T k A slightly modified version of the above equations can be used when measurement noise is correlated to the process noise. The modifications are given in Section 5.3.5, Table

2 5.3.4 The Steady-State Discrete-Time Kalman Filter An alternate way to write down the previous discrete-time Kalman filter is to substitute the update equations directly into the propagation equations. This results in the following alternative form for the discrete-time Kalman filter Design Model x k+1 = Φ k x k + Γ k u k + Υ k w k, w k N(0, Q k ) ỹ k = H k x k + v k, v k N(0, R k ) Initialization Kalman Gain ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] K k = P k H T k [H k P k H T k + R k ] 1 Calculate Covariance P k+1 = Φ k P k Φ T k Φ k P k H T k [H k P k H T k + R k ]H k P k Φ T k + Υ k Q k Υ T k Estimate State ˆx + k = ˆx k + Γ ku k + Φ k K k [ỹ k H kˆx k ] When the system above is time-invariant (i.e. Φ k = Φ, Γ k = Γ, Υ k = Υ, H k = H, Q k = Q, and R k = R), a steady state discrete time Kalman filter can be used Design Model Initialization Kalman Gain x k+1 = Φx k + Γu k + Υw k, w k N(0, Q) ỹ k = Hx k + v k, v k N(0, R) K = P H T [HP H T + R] 1 Calculate Covariance P = ΦP Φ T ΦP H T [HP H T + R] 1 HP Φ T + ΥQΥ T Estimate State ˆx k+1 = Φˆx k + Γ k u k + ΦK[ỹ k H ˆx k ] 2

3 5.4 The Continuous-Time Kalman Filter The continuous-time Kalman filter is used when the dynamics and the measurements are modeled using continuous differential and algebraic equations. Design Model ẋ = F (t)x(t) + B(t)u(t) + G(t)w(t), w(t) N(0, Q(t)) ỹ(t) = H(t)x(t) + v(t), v(t) N(0, R(t)) Initialization ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] Kalman Gain K(t) = P (t)h(t) T R(t) 1 Covariance Calculations P = F (t)p (t) + P (t)f T (t) P (t)h T (t)r(t) 1 H(t)P (t) + G(t)Q(t)G T (t) State Estimation ˆx(t) = F (t)ˆx(t) + B(t)u(t) + K(t)[ỹ(t) H(t)ˆx(t)] A slightly modified version of the above equations can be used when measurement noise is correlated to the process noise. The modifications are given in Section 5.4.5, Table

4 5.4 The Steady-State Continuous-Time Kalman Filter When the system above is time-invariant (i.e. F (t) = F, B(t) = B, G(t) = G, H(t) = H, Q(t) = Q, and R(t) = R), a steady state discrete time Kalman filter can be used Design Model ẋ = F x(t) + Bu(t) + Gw(t), w(t) N(0, Q) ỹ(t) = Hx(t) + v(t), v(t) N(0, R) Initialization Kalman Gain K(t) = P H T R 1 Covariance Calculations 0 = F P + P F T P H T R 1 HP + GQG T State Estimation ˆx(t) = F ˆx(t) + Bu(t) + K[ỹ(t) H ˆx(t)] 4

5 5.5 The Continuous-Discrete Kalman Filter The continuous-discrete Kalman filter is used when the dynamics are are modeled using continuous differential equations and the measurements are modeled using discrete algebraic equations. Design Model ẋ = F (t)x(t) + B(t)u(t) + G(t)w(t), w(t) N(0, Q(t)) ỹ k = H k x k + v k, v k N(0, R k ) Initialization ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] Kalman Gain K k = P k HT k [H k P k HT k + R k ] 1 State and Covariance Update Equations ˆx + k = ˆx k + K k[ỹ k H kˆx k ] P + k = [I K kh k ]P k State and Covariance Propagation Equations P = F (t)p (t) + P (t)f T (t) + G(t)Q(t)G T (t) ˆx(t) = F (t)ˆx(t) + B(t)u(t) 5

6 5.5 The Extended Kalman Filter The continuous-time extended Kalman filter is used when the dynamics and the measurements are modeled using continuous differential and algebraic equations. Design Model ẋ = f(x(t), u(t), t) + G(t)w(t), w(t) N(0, Q(t)) ỹ(t) = h(x(t), t) + v(t), v(t) N(0, R(t)) Initialization Kalman Gain ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] K(t) = P (t)h(ˆx(t), t) T R(t) 1 Covariance Calculations State Estimation P = F (ˆx(t), t)p (t) + P (t)f T (ˆx(t), t) P (t)h T (ˆx(t), t)r(t) 1 H(ˆx(t), t)p (t) + G(t)Q(t)G T (t) F (ˆx(t), t) = f, H(ˆx(t), t) = h x ˆx(t) x ˆx(t) ˆx(t) = f(ˆx(t), u(t), t) + K(t)[ỹ(t) H(ˆx(t), t)] 6

7 5.5 The Extended Kalman Filter(continued) The continuous-discrete extended Kalman filter is used when the dynamics and the measurements are modeled using continuous differential equations and the measurements are modeled using discrete algebraic equations. Design Model ẋ = f(x(t), u(t), t) + G(t)w(t), w(t) N(0, Q(t)) ỹ k = h k (x k ) + v k, v k N(0, R k ) Initialization ˆP 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] Kalman Gain K k = P k HT k (ˆx k )[H k(ˆx k )P k HT k (ˆx k ) + R k] 1 H(ˆx k ) = h x State and Covariance Update Equations ˆx k ˆx + k = ˆx k + K k[ỹ k h k (ˆx k )] P + k = [I K kh k (ˆx k )]P k State and Covariance Propagation Equations ˆx(t) = f(ˆx(t), u(t), t) P = F (ˆx(t), t)p (t) + P (t)f T (ˆx(t), t) + G(t)Q(t)G T (t) F (ˆx(t), t) = f x ˆx(t) 7

8 Shaping Filters and Colored Noise Our standard dynamics and measurement models have been driven by white noise. ẋ = f(x(t), u(t), t) + G(t)w(t), w(t) N(0, Q(t)) ỹ k = h k (x k ) + v k, v k N(0, R k ) What if our dynamics or measurements were being driven by some other kind of error source, like colored noise, or biases? We can introduce a shaping filter which is a linear system driven by white noise. ẋ sf = A sf x sf + B sf w sf, w sf N(0, Q sf ) The output of the shaping filter is the colored noise y sf = H sf x sf To us the color noise in out model, we augment our dynamics with the shaping filter dynamics ẋ = f(x(t), u(t), t) + G(t)w(t), w(t) N(0, Q(t)) ẋ sf = A sf x sf + B sf w sf, w sf N(0, Q sf ) and add the colored noise where appropriate, for example to our measurement model. ỹ k = h k (x k ) + H sf x sf + v k, v k N(0, R k ) Now we have a a measurement model that is driven by more than just white noise. The outputs of the shaping filters can be added to dynamics model as well. Below is an example of adding a bias and two colored noise processes. The dynamics are given by ẋ = f(x(t), u(t), t) + G(t)w(t) + H sf1 x sf1, w(t) N(0, Q(t)) and the measurement are given by ẋ sf1 = A sf1 x sf1 + B sf1 w sf1, w sf1 N(0, Q sf1 ) ẋ sf2 = A sf2 x sf2 + B sf2 w sf2, w sf2 N(0, Q sf2 ) ẋ bias = 0 ỹ k = h k (x k ) + H sf2 x sf2 + x bias + v k, v k N(0, R k ) The most simple and useful colored noise process is a 1st-order Markov Process, also known as an Exponentially Correlated random Variable (ECRV). See class notes for more information. 8

9 Batch State Estimation for Dynamic Systems Batch state estimation for dynamic systems is often achieved with non-linear least squares techniques. For parameter estimation, the non-linear least squares is applied as follows. For dynamics systems we can use a similar approach where we let the parameter be the initial state. Then, the non-linear least squares method will find an initial state that creates a trajectory that will best the measurement data. The dynamics and measurement models are given by ẋ = f(x(t), t) ỹ k = h k (x k ) + v k, v k N(0, R k ) Using the estimate ˆx 0 of the initial state, we can integrate the following differential equations to obtain ˆx(t), and Φ(t, t 0 ). ˆx = f(ˆx(t), t), ˆx 0 = given Φ(t, t 0 ) = F (t)φ(t, t 0 ), Φ(t 0, t 0 ) = I F (t) = f, x ˆx(t) Recall that for any time t k, the transition matrix Φ(t k, t 0 ) relates trajectory perturbations at t 0 to trajectory perorations at time t k δx(t k ) = Φ(t k, t 0 )δx(t 0 ) Next, the measurement equation can be linearized about ˆx(t i ) ỹ k = h k (ˆx k ) + h δx(t k ) + v k x ˆx(tk ) Subtracting h k (ˆx k ) from both sides of the equation gives us δỹ k = ỹ k h k (ˆx k ) = h δx(t k ) + v k x ˆx(tk ) = h k, t 0 )δx(t 0 ) + v k x ˆx(tk )Φ(t δỹ k = H k δx(t 0 ) + v k 9

10 where H k = h Φ(t k, t 0 ) x ˆx(tk ) Now, we have a measurement equation in terms of a parameter δx(t 0 ) and can apply the method of least squares. For m measurements, we compute Σ = R = m k=1 m k=1 H k R 1 k HT k H k R 1 k δỹ k The new best estiamte of the intial state is given by ˆx(t 0 ) = ˆx(t 0 ) + δx(t 0 ) where δx(t 0 ) = Σ 1 R The process can be repeated with the new ˆx(t 0 ) until δx(t 0 ) converges to zero. 10

11

12

13

14

15

16