13.4 Scalar Kalman Filter

Transcription

1 13.4 Scalar Kalma Filter Data Model o derive the Kalma filter we eed the data model: a 1 + u < State quatio > + w < Obervatio quatio > Aumptio 1. u i zero mea Gauia, White, u } σ. w i zero mea Gauia, White, 3. he iitial tate i 1 N ( µ, σ ) { u { w } σ 4. u, w, ad 1 are all idepedet of each other Ca vary with time o implify the derivatio: let µ 0 (we ll accout for thi later) 1

2 Goal ad wo Propertie Goal: Recurively compute ˆ Notatio: X i et of all obervatio i a igle vector-obervatio { 0, 1,, } 0, 1,, X wo Propertie We Need 1. For the joitly Gauia cae, the MMS etimator of zero mea baed o two ucorrelated data vector 1 & i (ee p. 350 of tet) ˆ θ { θ, } { θ } + { θ } 1 1. If θ θ 1 + θ the the MS etimator i ˆ θ { θ } { θ1 + θ } { θ1 } + { θ } (a reult of the liearity of {.} operator)

3 3 Derivatio of Scalar Kalma Filter Iovatio: 1 ˆ Recall from Sectio 1.6 MMS etimate of give X 1 (predictio!!) By MMS Orthogoality Priciple { } 0 X 1 that i ucorrelated with the previou data i part of Now ote: X i equivalet to { } 1, X Why? Becaue we ca get get X from it a follow: 1 1 X X X %"$"# 1 ˆ k k k a

4 What have we doe o far? Have how that X { X 1, } ucorrelated Have plit curret data et ito part: 1. Old data. Ucorrelated part of ew data ( jut the ew fact ) { X } { 1, } X Becaue of thi So what??!! Well ca ow eploit Property #1!! { X 1 } { } ˆ + %"" $ ""# 1 predictio of baed o pat data %"$"# " Update baed o iovatio part of ew data Now eed to look more cloely at each of thee! 4

5 Look at Predictio erm: 1 Ue the Dyamical Model it i the key to predictio becaue it tell u how the tate hould progre from itat to itat 1 Now ue Property #: 1 { X 1 } { a 1 + u X 1 } { 1 X 1 } + { u X 1 } a %""" $ """# 1 %"" $ """ # { u } 0 By Defiitio 1 a 1 By idepedece of u & X-1 See bottom of p. 433 i tetbook. he Dyamical Model provide the update from etimate to predictio!! 5

6 Look at Update erm: { } Ue the form for the Gauia MMS etimate: { } { } { } %" " $ ""# So k ( ˆ 1 ) { } k ˆ 1 Predictio Show Up Agai!!! Put thee Reult ogether: %"$"# a 1 + k by Prop. # + wˆ %"$"# %"$"# + 0 Becaue w i idep. of {0,, -1} hi i the Kalma Filter How to get the gai? 6

7 Look at the Gai erm: Need two propertie A. { ( )} {( )( )} B. Aide <,y> <+z,y> for ay z y { w ( )} proof Liear combo of pat data thu w/ iovatio 0 ˆ he iovatio w i the meauremet oie ad by aumptio i idep. of the dyamical drivig oie u ad -1 I other word: w i idep. of everythig dyamical So {w} 0 i baed o pat data, which iclude {w0,, w-1}, ad ice the meauremet oie ha idep. ample we get w 7

8 So we tart with the gai a defied above: k { } { } { } { } { } + w { } { + w } + w { } { } + { w } + σ + { w } { } M MS whe i etimated by 1-tep predictio Plug i for iovatio (!) Ue Prop. A i um. Ue + w i deomiator 0 by Prop. B (!!) Ue + w i umerator pad 8

9 hi give a form for the gai: k σ M + M 1 1 hi balace the quality of the meaured data agait the predicted tate I the Kalma filter the predictio act like the prior iformatio about the tate at time before we oberve the data at time 9

10 Look at the Predictio MS erm: But ow we eed to kow how to fid M 1!!! M 1 { } 1 { } a 1 + u a 1 Ue dyamical model & eploit form for predictio { ( ) } a u Cro-term 0 t. rror at previou time M a M 1 + σ u Why are the cro-term zero? wo part: 1. 1 deped o {u0 u 1, -1}, which are idep. of u. 1 deped o {0+w0 1+w 1}, which are idep. of u 10

11 Look at a Recurio for MS erm: M By def.: M { } k ( 1 ) { } Now we ll get three term: {A }, {AB}, {B } { } A M erm A { AB} k { } k M { } { } B k k De. of k by defiitio from (!!) i um. k from (!) i de. k erm B Recall: k k Num. of k k M σ M + M 11

12 So thi give M M k M + k M ( 1 k ) M 1 M Puttig all of thee reult together give ome very imple equatio to iterate Called the Kalma Filter We jut derived the form for Scalar State & Scalar Obervatio. O the et three chart we give the Kalma Filter equatio for: Scalar State & Scalar Obervatio Vector State & Scalar Obervatio Vector State & Vector Obervatio 1

13 Kalma Filter: Scalar State & Scalar Obervatio State Model: a 1 + u u WGN; WSS; N(0, σ u ) Obervatio Model: + w w WGN; N(0, σ ) Varie with Iitializatio: 1 M 1 { 1} µ {( 1} 1) } σ Mut Mut Kow: Kow: µ,, σ,, a, a, σ σ u, u, σ σ Predictio: a 1 Pred. MS: Kalma Gai: M a M 1 + σ u M K + M σ Update: ( 1 ) + K t. MS: ( 1 K ) M 1 M 13

14 Kalma Filter: Vector State & Scalar Obervatio State Model: A 1 + Bu p 1; A p p; B p r; u N ( 0, Q) r 1 Obervatio Model: h + w ; h p 1 w WGN; N(0, σ ) Iitializatio: Predictio: Pred. MS (p p): Kalma Gai (p 1): Update: t. MS (p p): : 1 M 1 { 1} µ Mut Mut Kow: Kow: µ,, C,, A, A, B, B, h, h, Q, Q, σ {( 1} 1)( 1} 1) } C A 1 M 1 AM 1 A + K σ M h + h %" "" M $ " "" 1 h # 1 1 BQB + K ( h ˆ ) %"" $ ""# ˆ 1 %""" $ """" # : ( I K h ) M 1 M iovatio 14

15 Kalma Filter: Vector State & Vector Obervatio State Model: A 1 + Bu p 1; A p p; B p r; u N( 0, Q) r 1 Obervatio: H + w ; M 1; H M p; w N( 0, C ) M 1 Iitializatio: Predictio: Pred. MS (p p): Kalma Gai (p M): Update: 1 M 1 { 1} µ Mut Mut Kow: Kow: µ,, C,, A, A, B, B, H, H, Q, Q, C} C} {( 1} 1)( 1} 1) } C A 1 M 1 AM 1 A + K M H BQB C + H 1 %" M "" $ """ H # M M + K ( H ˆ ) %"" $ ""# ˆ 1 %""" $ """" # : iovatio 1 t. MS (p p): : M ( I K H ) M 15

16 Kalma Filter Block Diagram Obervatio Iovatio timated Drivig Noie timated State + Σ K Buˆ + Σ + ˆ 1 H 1 Az -1 Predicted Obervatio mbedded Obervatio Model Predicted State mbedded Dyamical Model Look a lot like Sequetial LS/MMS ecept it ha the mbedded Dyamical Model!!! 16

17 Overview of MMS timatio Aume Gauia Ge. MMS Squared Cot Fuctio θˆ { θ } Force Liear Ay PDF, Kow d Momet Joitly Gauia ˆ θ { θ} + CθC 1 ( { } ) LMMS Bayeia Liear Model θˆ µ 1 ( HC H + C ) ( ) θ + CθH θ w Hµ θ LMMS Liear Model Optimal Seq. Filter (No Dyamic) θˆ ˆ ˆ h θ θ 1 + k 1 Liear Seq. Filter (No Dyamic) Optimal Kalma Filter (w/ Dyamic) + K ( H A 1) Liear Kalma Filter (w/ Dyamic) 17