Crib Sheet : Linear Kalman Smoothing

Transcription

1 Crib Sheet : Linear Kalman Smoothing Gabriel A. Terejanu Department of Computer Science and Engineering University at Buffalo, Buffalo, NY terejanu@buffalo.edu 1 Introduction Smoothing can be separated into three classes [6]: 1. Fixed-interval smoothing. The goal is to obtain the estimates x s k, for k = 0... N, given a fixed observation interval Z N = {z k 0 k N} [6]. 2. Fixed-point smoothing. The goal is to obtain the estimate x s N (the time t N is fixed, N < k) given a set of observations Z k = {z k 0 N k} [6]. 3. Fixed-lag smoothing. The goal is to obtain the estimate x s k given the observations Z k+n = {z k 0 k k +N} [6]. Where k +N is the most current measurement and N is a constant lag. It is possible to employ a single smoothing scheme, based on fixed-interval smoothing to solve all three problems [1]. A state is said to be smoothable if an optimal smoother provides a state estimate superior to that obtained when the final optimal filter estimate is extrapolated backwards in time [4]. Only those states which are controllable by the noise driving the system state vector are smoothable (Weiss 1970).

2 2 Fixed Interval Smoothing The goal is to obtain the estimates x s k, for k = 0... N, given a fixed observation interval Z N = {z k 0 k N} [6]. Fraser and Potter (1969): two optimal linear filters [3, 2] The smoothed estimate is expressed as a linear combination between the forward and backward filter state estimates. The optimal weighting between the two is also known as Millman s theorem which is also an exact analog to maximum likelihood of a scalar with independent measurements [2]. Backward Information Filter Forward Kalman Filter Figure 1: Fixed Interval Smoothing : two-filter approach Figure 2: Advantage of performing optimal smoothing [4] 2

3 x k = A k 1 x k 1 + B k 1 u k 1 + w k 1 Model Forecast Step/Predictor x fk = A k 1x + fk 1 + B k 1u k 1 Backward Information Filter y bn = 0 Y bn = 0 Backward Update y + bk = y bk + HT k R 1 k z k Y + bk = Y bk + HT k R 1 k H k Backard Propagation K bk = Y + ( bk+1 Y + bk+1 + ) 1 Q 1 k y bk = AT k (I K bk) ( y + bk+1 Y+ bk+1 B ) ku k Estimate Y bk = AT k (I K bk)y + bk+1 A k x s k = (I Ks k )x+ fk + Ps k ( y bk K s k = P+ fk Y bk I + P + fk Y bk P s k = (I Ks k )P+ fk Table 1: Fraser and Potter: two optimal linear filters. ) 1 Notes The infinite value of P bn and the boundary condition to specify x bn are difficult to apply [3]. To avoid these problems the information form approach is used to represent the backward filter. The smoother error covariance P s k = ((P + fk ) 1 + (P bk ) 1 ) 1 [2]. At k = N the smoother estimate and covariance have to be the same as the forward Kalman Filter. Thus, P s N = P+ fk yields (P bk ) 1 = 0. Since we use the information form for the backward filter: Y bk = (P bk ) 1 = 0 and y bk = Y bk x bk = 0 (are zero for k = N). The smoothed covariance is equivalent to Joseph s stabilized form. Forward error estimate and backward forecast error are uncorrelated. 3

4 Rauch, Tung, and Striebel (1965): correction to the Kalman Filter [5, 2] Combines the backward filter and the smoother in one single recursive step. Notes x k = A k 1 x k 1 + B k 1 u k 1 + w k 1 Model Forecast Step/Predictor x fk = A k 1x + fk 1 + B k 1u k 1 x s N = x+ fk P s N = P+ fk Update K s k = P+ fk AT k (P ( fk+1 ) 1 P s k = P+ fk Ks k P fk+1 Ps k+1 ( ) ) x s k = x+ fk + Ks k x s k+1 x fk+1 Table 2: Rauch, Tung, and Striebel: correction to the Kalman Filter. (K s k )T The smoother does not depend on either backward covariance or backward estimate. Its form reveals just a correction of the current Kalman Filter using only the data provided by the forward filter. The smoothed estimate does not depend on the smoothed covariance. In order to obtain the smoothed estimate only the forward state estimate and the smoothed gain have to be stored. It is completely derived from optimal control theory [2] Minimize J(w k ) = 1 N (z k H k x k ) T R 1 2 k (z k H k x k ) + w t k Q 1 k w k k=1 + 1 ( ) ( T ) x + 2 f0 x 0 (P + f0 ) 1 x + f0 x 0 subject to x k z k = A k 1 x k 1 + B k 1 u k 1 + w k 1 = H k x k + v k this yields a Two-Point-Boundary-Value-Problem (TPBVP). 4

5 It can be derived as MAP estimate of x k, given the data set Z N, N > k, where x k maximizes the conditional density p(x k Z N ) [6, p.367]. 3 Fixed Point Smoothing The goal is to obtain the estimate x s N (the time t N is fixed, N < k) given a set of observations Z k = {z k 0 N k} [6]. Figure 3: Fixed Point Smoothing Meditch (1967): extended RTS for fixed-point smoothing [6, 2] x k = A k 1 x k 1 + B k 1 u k 1 + w k 1 Model Forecast Step/Predictor x fk = A k 1x + fk 1 + B k 1u k 1 Update x s N N = x+ fn P s N N = P+ fn M N N = I M N k = M N k 1 K s k 1 K s k = P+ fk AT k (P fk+1 ) 1 ( ) P s N k = Ps N k 1 M N k P + fk P fk M T N k ( ) x s N k = xs N k 1 M N k x + fk x fk Table 3: Meditch (1967): extended RTS for fixed-point smoothing 5

6 4 Fixed Lag Smoothing The goal is to obtain the estimate x s k given the observations Z k+n = {z k 0 k k + N} [6]. Where k + N is the most current measurement and N is a constant lag. Figure 4: Fixed Lag Smoothing Meditch (1967): extended RTS for fixed-lag smoothing [6, 2] The fixed-lag-smoothing algorithm for discrete processed is obtained by combinng both the fixedinterval and fixed-lag algorithms [6]. Model Forecast Step/Predictor x k = A k 1 x k 1 + B k 1 u k 1 + w k 1 x fk = A k 1x + fk 1 + B k 1u k 1 Update x s k k+n = x+ fn P s k k+n = P+ fn M k k+n = I M k+1 k+n+1 = M k k+n K s k+n K s k+n = P+ fk+n AT k+n (P fk+n+1 ) 1 P s k+1 k+n+1 = P fk+1 (Ks k ) 1 (P + fk+ Ps k k+n ) (K s k ) T M k+1 k+n+1 K fk+n+1 H k+n+1 P fk+n+1 MT k+1 k+n+1 x s k+1 k+n+1 = A kx s k k+n + B ku k ) +Q k A T k (P+ fk (x ) 1 s k k+n x+ fk +M k+1 k+n+1 K fk+n+1 (z k+n+1 H k+n+1 x fk+n+1 ) Table 4: Meditch (1967): extended RTS for fixed-lag smoothing 6

7 References [1] Doucet A. & Maskell S. Briers, M. Smoothing algorithms for state-space models. Submission IEEE Transactions on Signal Processing, [2] John Crassidis and John Junkins. Optimal Estimation of Dynamic Systems. CRC Press, [3] D. Fraser and J. Potter. The optimum linear smoother as a combination of two optimum linear filters. Automatic Control, IEEE Transactions on, 14(4): , Aug [4] Arthur Gelb. Applied Optimal Estimation. The M.I.T. Press, [5] H. E. Rauch, F. Tung, and C. T. Striebel. Maximum likelihood estimates of linear dynamic systems. J. Amer. Inst. Aeronautics and Astronautics, 3 (8): , [6] Andrew Sage and James Melsa. Estimation Theory with Applications to Communications and Control. McGraw-Hill Book Company,