16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde 10△t Φ=I+AMt= 120 01 0=o(2, r)BNB'ap (2, t)'di (2,r)=+A(2-r) 110(2-r 120-10r ΦBNBp3=/40-20r [40-20x2 1600-1600r+400280-40r 80-40r 4 3200 ΦBNBΦdr=2 =O R=4 The initial conditions are given to be 0,so P(0)= Using the discrete formulation Time propagation: x+=①x P-1=ΦPΦ+Q measurement update K=P-H(HP-H+R =+K(=- Page 3 of 916.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 3 of 9 [ ] 1 10 0 1 1 20 0 1 1 0 k k t I At H ⎡ ⎤ ∆ Φ = + ∆= ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ = () () () ( ) ( ) 2 0 2, 2, 2, 2 1 10 2 1 20 10 0 1 0 1 T T Q BNB d k I A τ ττ τ τ τ τ =Φ Φ Φ =+ − ⎡ ⎤ − ⎡ ⎤ − = = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ∫ [ ] 2 2 0 40 20 40 20 2 2 1600 1600 400 80 40 80 40 4 3200 80 2 80 8 4 T T T T k k BNB BNB d Q R τ τ τ τ τ τ τ ⎡ ⎤ − Φ Φ= − ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ −+ − = ⎢ ⎥ ⎣ ⎦ − ⎡ ⎤ Φ Φ= = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ = ∫ The initial conditions are given to be 0, so 0 ˆ (0) 0 0 0 (0) 0 0 x P − − ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ Using the discrete formulation: Time propagation: 1 1 ˆ ˆ k k T k k x x PPQ − + + − + + = Φ =Φ Φ + Measurement update: ( ) ( ) ( ) 1 ˆˆ ˆ T T K P H HP H R x x K z Hx P I KH P − − − +− − + − = + =+ − = −