advertisement

Potter Square Root Filter ASEN 5070 October 30, 2002 George H. Born Introduction In general, square root filters are more numerically stable than the conventional Kalman filter. Note that the condition number for the square root of a covariance matrix is the square root of the condition number of the covariance matrix. Hence, the square root filter will be less affected by numerical problems The first square root filter development is due to Potter who developed an algorithm for the limited case of uncorrelated scalar observations with no process noise. This filter was used in the Lunar Excursion Module (LEM) for the Apollo Program. Potter Algorithm Derivation Begin with the time update equation for the estimation error covariance (we will assume that the time update is from tk −1 to tk and drop the indices) P = ΦPΦT . (1) WW T = P . (2) P = ΦWW T ΦT ≡ WW T (3) W = ΦW . (4) Define the square root of P From Eq. (1) and (2) where Next write the expression for the Kalman gain and the measurement update for P in terms of W. (Note that observations are processed one at a time and are assumed to have uncorrelated errors.) ( % % T +σ 2 K = PH% T HPH ( ) −1 T %T % = WW H% T HWW H +σ 2 T where σ 2 is the variance of the observation error. 1 ) −1 (5) Let T %T % α ≡ ( HWW H +σ 2 ) −1 (6) where α is a scalar. Define F% = W T H% T . (7) then α = ( F% T F% + σ 2 ) −1 (8) and K = αWF% . (9) The measurement update for P is ( ) % % ) WW = ( I − αWFH % % )W = W ( I − α FHW % % )W = W ( I − α FF P = WW T = I − KH% P T T T T . (10) Potter observed that if a matrix A could be found such that ( % %T AAT = I − α FF ) (11) then P = WAATW T = WW T . (12) To find A introduce the scalar γ so that %% ) ( )( I − γα FF %% ) = ( I − α FF . % %T AAT = I − γα FF T T (13) Solving for γ , % % T = I − 2γα FF % % T + γ 2α 2 FF % % T FF % %T . I − α FF 2 (14) Define β ≡ F%1×Tn F%n×1 , (15) where β is a scalar. Then % % T = I − 2γα FF % % T + γ 2α 2 β FF % %T I − α FF or %% (γ α β − 2γα + α ) FF %% = (αβγ − 2γ + 1)α FF 2 2 2 T =0 T =0 . % % T = 0 is a trivial solution. α FF Thus αβγ 2 − 2γ + 1 = 0 . (16) Using the solution for a quadratic equation (after some algebra) γ= 1 1 m ασ 2 , (17) where the + sign is chosen to prevent the possibility that γ = ∞ when ασ 2 = 1 . Recall from Eq. (12) that W = WA ( % %T = W I − γα FF ) (18) and K = αWF% . (19) W = W − γ KF% T (20) Hence, which is the measurement update for W. 3 The Potter Computational Algorithm: Given Wk , xk , yk , H% k where Pk = WkWkT . Compute 1. F%k = WkT H% kT ( 2. α k = F% Tk F%k + σ 2 ) −1 3. K k = α kWk F%k ( 4. xˆk = xk + K k yk − H% k xk 5. γ k = ) 1 1 + α kσ 2 6. Wk = Wk − γ k K k F%kT , Pk = WkWkT & = AΦ forward to k + 1 7. Integrate the reference orbit and Φ 8. Time update Wk and xˆk to k + 1 Wk +1 = Φ ( tk , tk +1 ) Wk xk +1 = Φ ( tk , tk +1 ) xˆk 9. Return to step 1 with k = k + 1 We have assumed here that the observation vector, yk, contains a single measurement. If yk contains more than one data type at each observation time, we would return to step 1 after completing step 6 for each element of yk. After we have processed all elements of yk, we would proceed to the time update of step 8. Note that unlike P, W contains n2 distinct elements (i.e., W is not symmetric). The Carlson algorithm reduces W to an upper (or lower) triangular matrix (see Bierman, 1977). 4