16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Lecture g Last time: Linearized error propagation trajectory surface Integrate the errors at deployment to find the error at the surface e=ee See s E SE,S Or p can be integrated from: d=Fd, whereΦ(0)=1 文=f(x) F where F is the linearized system matrix. But this requires the full a(same number of equations as finite differencing) I, =time when the nominal trajectory impacts e(Ln)=Φ(tn)8 e(n)=旦=Φ,旦 where a, is the upper 3 rows of a(n) Covariance matrix E2=d,EΦ Page 1 of 8
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 1 of 8 Lecture 9 Last time: Linearized error propagation s 1 e Se = Integrate the errors at deployment to find the error at the surface. 1 1 1 T s ss T T T E ee See S SE S = = = Or Φ can be integrated from: , where (0) ( ) F I x fx df F dx Φ= Φ Φ = = = & & where F is the linearized system matrix. But this requires the full Φ (same number of equations as finite differencing). nt = time when the nominal trajectory impacts. 1 2 1 () () ( ) n n rn r et t e et e e = Φ = =Φ where Φr is the upper 3 rows of ( ) n Φ t . Covariance matrix: 2 1 T E E =Φ Φ r r
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde e= Fe E(1)=e(l)e(t) E()=(n)e()y+e()() =Fe(n)e(1)+e()e()F You can integrate this differential equation to t, from E(O)=E. This requires the full6×6 Ematrix E eeee ee ee E,= upper left 3 x3 partition of E(t,) perturbed trajectory e (t) For small times around t e(n=e(t,)+v(,(t-L) e(,)+(v,(t,)+e()(t-L) =e(t,)+v((t-t) 1e(1)=1g+1(n)(t-Ln)=0
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 2 of 8 () () () () () () () () () () () () () () T T T T TT T e Fe Et etet Et etet etet Fe t e t e t e t F FE t E t F = = = + = + = + & & & & You can integrate this differential equation to tn from 1 E E (0) = . This requires the full 6 6 × E matrix. 2 ( ) upper left 3 3 partition of ( ) T T rr rv n T T vr vv n ee ee E t ee ee E Et ⎡ ⎤ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ = × For small times around tn, ( ) ( ) ( )( ) ( ) ( ( ) ( ))( ) ( ) ( )( ) n nn n nn vn n n nn n et et vt t t et v t e t t t et v t t t =+ − =+ + − =+ − 2 2 1 ( ) 1 1 ( )( ) 0 1 ( ) 1 T TT v v v nn n T v i n T v n et e v t t t e t t v = + −= − =−
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde e,= position error at impact I'y e"projection matrix" V 1 altitude, H range, R rack, T (into pag e 3=[rle, COS R= R R=[11y L=unit vectors along the jth axis of the 2 frame expressed in the coordinates of the 3 frame e=re RER
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 3 of 8 3 2 2 2 position error at impact 1 1 1 "projection matrix" 1 T v n T v n T n v T v n e e e v v v I e v = = − ⎡ ⎤ = − ⎢ ⎥ ⎣ ⎦ [ ] 3 2 123 [ ] cos ... ... ... ... ... ... ... ... cos 111 ij ij ij e Re R R R θ θ ′ = ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ = = 1j = unit vectors along the jth axis of the 2 frame expressed in the coordinates of the 3 frame. 3 2 3 2 T e Re E RE R ′ = ′ =
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde er(t=e+v, cosy(t-t,) =已 en()=eH. -v, siny(t-t,)=0 (1-L)= sIn =eg. cot yeH The transformation which relates rhterrors at the nominal end time to r and t errors when h=o e4 Pe If the e defined earlier, based on integration of perturbed trajectories, measured in R, Tcoordinates, then the sensitivity matrix defined at that point is equivalent to e
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 4 of 8 3 3 3 ( ) cos ( ) ( ) ( ) sin ( ) R Rn n T T H Hn n et e v tt et e et e v tt γ γ =+ − = =− − Impact: 3 3 3 3 3 3 3 ( ) sin ( ) 0 1 ( ) sin cos ( ) sin cot ( ) Hi H n i n in H n n Ri R H n R H Ti T et e v t t tt e v v et e e v e e et e γ γ γ γ γ = − −= − = = + = + = The transformation which relates R,H,T errors at the nominal end time to R and T errors when H=0 is: 3 3 3 4 3 3 ( ) ( ) cot 1 0 cot 01 0 R i T i R H T e t e e t e e e e Pe γ γ ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ + = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ = ≡′ ′ ⎢ ⎥ ⎣ ⎦ If the s e defined earlier, based on integration of perturbed trajectories, is measured in R,T coordinates, then the sensitivity matrix defined at that point is equivalent to s 1 r e Se S PR = = Φ
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde E4= PE3 ITR T grr」LpoR0r0 If all the original error sources are assumed normal, R and T will have a joint binormal distribution since they are derived from the error sources by linear operations only. This joint probability density function is f(r,)= where o,, Or and p can be identified from E4. Recall that we are co ng unbiased errors Contour of constant probability density function is r=xcos 8- yin 6 ne Get (6)x2+()xy+()y2=c Coefficient of x, y equals zero for principal axes tan 20=LpGROT Use a 4 quadrant tan.function. ge 5 of 8
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 5 of 8 4 3 2 2 2 2 2 2 T R RT R R T RT T R T T E PE P R RT TR T σ µ σ ρσ σ µ σ ρσ σ σ = ′ ⎡ ⎤ ⎡ ⎤⎡ ⎤ == = ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎣ ⎦ ⎣ ⎦ If all the original error sources are assumed normal, R and T will have a joint binormal distribution since they are derived from the error sources by linear operations only. This joint probability density function is ( ) ( ) 2 2 2 2 2 1 2 1 , 2 1 R RT T r rt t R T f rt e ρ σ σσ σ ρ πσ σ ρ ⎡ ⎤ ⎛ ⎞ ⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎢ ⎥ ⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟ − + ⎝ ⎠ ⎝ ⎠⎝ ⎠ ⎝ ⎠ − − ⎣ ⎦ = − where , σ R T σ and ρ can be identified from E4 . Recall that we are considering unbiased errors. Contour of constant probability density function is 2 2 2 2 R RT T r rt t ρ c σ σσ σ ⎛ ⎞ ⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟ − += ⎝ ⎠ ⎝ ⎠⎝ ⎠ ⎝ ⎠ cos sin sin cos rx y tx y θ θ θ θ = − = + Get: { 2 22 0 () () () θθ θ x xy y c = ++= Coefficient of x, y equals zero for principal axes. 22 22 2 2 tan 2 R T RT RT RT ρσσ µ θ σ σ σσ = = − − Use a 4 quadrant tan-1 function