Error covariance Pk for update estimate Put( 8 )back to(7) (P HCHPT H+r)-- P=PK-KKHPK-PIKKH]+K(HPKH+RkKk--(7) P=P -,HP-PHKK+Kk(HP H+R)Ky P=P-(PHCHP H+R)HPK-PH[(P H( H+r)]+ (PHCHP H+)(HPH+R(PH(HPH+R)I-99 The last term of (9) (P HCHP H +R)(HP H+R(P H (HPLH+R (PHOI( H(HP H+R)I So(9)becomes P=P-(P H(HP H+R)HP--P H[(P H(HPH+R)]+ (PKH( H +R)J P=P-(PH(HPH+R)HP P=(-KKH)PK (10 SFMKalman vga 16
SFM Kalman V9a 16 Error covariance Pk for update estimate • 1 1 1 Put (8) back to (7) ( )( ) (8) [ ] ( ) (7) ( ) ( )( ) [( )( ) ] ( T T k k k T T T k k k k k k k k k k T T T T k k k k k k k k k k T T T T T T k k k k k k k k k K P H HP H R P P K HP P K H K HP H R K P P K HP P H K K HP H R K P P P H HP H R HP P H P H HP H R P − − − − − − − − − − − − − − − − − − − − − = + − − − − − − − − − = − − + + − − = − − + + = − + − + + 1 1 1 1 1 1 )( ) ( )[( )( ) ] (9) The last term of (9) ( )( ) ( )[( )( ) ] ( )[( )( ) ] So (9) becomes ( )( ) T T T T T T k k k k k T T T T T T k k k k k k T T T T k k k T T k k k k k H HP H R HP H R P H HP H R P H HP H R HP H R P H HP H R P H P H HP H R P P P H HP H R HP − − − − − − − − − − − − − − − − − − − − − − + + + − − + + + = + = − + − 1 1 1 [( )( ) ] ( )[( )( ) ] ( )( ) ( ) (10) T T T T k k k T T T T k k k T T k k k k k k k k P H P H HP H R P H P H HP H R P P P H HP H R HP P I K H P − − − − − − − − − − − − − − + + + = − + = − − − − − − − − − − −
Error covariance prediction ek= xx -xk, also ek=xk--ik, by definition as described earlier Because xx= Ak-xk-+wk-I =(4x1+-)-4 ek akek-lt wk-l The mean(expected value)of w,=0, SmeP=Ee门]E4-+m、4A-+my P=E(4 +w e +1 k-1 P=E(44.-)+4+m(4..) +Wk-1 Since wand ek- have no correlation, A ek-wk=0, wk(aekd=o P=E(4e24y+vxm-]E4,)14)m:y P=4-E{.el-]41+E A-B-14-1+Q or P=Ak-P-Ak+o=(error covariance prediction SFM Kalman Via Since a is constant. so P=AP-A+g=(error covariance prediction 17
Error covariance prediction • SFM Kalman V9a 17 ( ) ( )( ) ( )( ) ( )(( ) ) ( )( ) ( ) ( ) ( )( ) ( )( ) (error covariance prediction ) Since is constant ,so or (error covariance prediction ) , Since and have no correlatio n, 0, 0, Since The mean (expected value) of 0, ˆ Because , ˆ , also ˆ ,by definition as described earlier 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 = + = = + = = + = + = + = + = = = + + + = + + = = + + = = + = + − = + − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − − P AP A Q A P A P A Q P A E e e A E w w A P A Q P E A e A e w w E A e e A w w w e A e w w A e P E A e A e A e w w A e w w P E A e w A e w P E e e E A e w A e w w e A e w e A x w A x x A x w e x x e x x T k k T k k k k T k k k T k k T k T k k k k T k k T k T k k k T k k T k k k k k T k k k T k k k k k T k k T k k k T k k k T k k k k k T k T k k k k k k T k k k k k k T k k k k k k k k k k k k k k k k k k k k k k k k
Kalman filter(KF) recursive functions a posterior state =xk a priori state =xp a posterior est. state xk a priori est. state =xk a posterior err. cov. P I a priori err. cov. P Prediction equations: project into next state Fk=Axk=(state prediction PK=AP-A+2=(error covariance prediction Update equations Kalman gain Kk=(PKH(HPKH+r) Update Estimate xx =xk+Kk(=k-Hxy) Update Covariance Pk=(l-Kkh)PK SFM Kalman vga
SFM Kalman V9a 18 Kalman filter (KF) recursive functions • − − − − − − − − − − = − = + − = + = + = = = k k k k k k k k T k T k k T k k k k P I K H P x x K z Hx K P H HP H R P AP A Q x Ax Update Covariance ( ) Update Estimate ˆ ˆ ( ˆ ) Kalman gain ( )( ) Update equations (error covariance prediction ) ˆ ˆ (state prediction ) Prediction equations : project into next state 1 1 1 − − − = = = k k k k k k P P x x x x a posterior err. cov. | a priori err. cov. a posterior est.state ˆ | a priori est.state ˆ a posterior state | a priori state
xk- n state variables, and zk- m measurement variables Kalman Filter(KF) Iteration flow Prediction equations: project into next state FK=Alk-=(state prediction P=APK-1A+Q=(error covariance prediction pdate equations Kalman gain Kk=(P H'XHPH'+R Update Estimate i=i+Kk(=k-HEk Initial states Update Covariance P=(I-K,H)P x, and p k-1 SFMKalman vga 19
SFM Kalman V9a 19 Kalman Filter (KF) Iteration flow − − − − − − = − = + − = + k k k k k k k k T k T k k P I K H P x x K z Hx K P H HP H R Update Covariance ( ) Update Estimate ˆ ˆ ( ˆ ) Kalman gain ( )( ) Update equations 1 (error covariance prediction ) ˆ ˆ (state prediction ) Prediction equations : project into next state 1 1 = + = = = − − − − P AP A Q x Ax T k k k k 1 1 ˆ and Initial states k− Pk− x xk= n state variables, and zk= m measurement variables
xk- n state variables, and zk- m measurement variables Kalman Filter(Kf) Iteration flow with matrix dimensions Prediction equations: project into next state (n×n)k-1(n×1) (state prediction) A(nxn)+O nxn n×n nxn (error covariance prediction) pdate equations Kalman gain KK(mkm)=(P& (mxm H"cnxm ) PH n×m) +r (m×n)2k(nxn) Update Estimate xk omx)=XK(mx)+ KK(mxm)(Ek(mx)-Humxn) xk (mxl) Update Covariance Pk(mxn)=(I(nxn)-KK k(nxm)(nxm)/ k(nxn) Initial states k-l(n×l) ane k-l(n×n) SFMKalman vga
SFM Kalman V9a 20 Kalman Filter (KF) Iteration flow with matrix dimensions ( ) ( ) ( ) ( ) ( ) ( 1) ( 1) ( ) ( 1) ( ) ( 1) 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) Update Covariance ( ) Update Estimate ˆ ˆ ( ˆ ) Kalman gain ( )( ) Update equations k n n n n k n m n m k n n k n k n k n m k m m n k n n m m m T m n k n n n m T k n m k n n P I K H P x x K z H x K P H H P H R − − − − − − = − = + − = + (error covariance prediction) ˆ ˆ (state prediction) Prediction equations: project into next state ( ) ( ) ( ) ( ) 1( ) ( 1) ( ) 1( 1) = = + = = − − − − n n n n T k n n n n k n n k n n n k n P A P A Q x A x 1(n 1) 1(n n) ˆ and Initialstates k− Pk− x xk= n state variables, and zk= m measurement variables