EE 570: Location and Navigation

Transcription

1 EE 570: Location and Navigation Error Mechanization (ECEF) Aly El-Osery Electrical Engineering Department, New Mexico Tech Socorro, New Mexico, USA April 11, 2013 Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

2 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

3 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

4 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

5 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) [δ ψ e e ]δω e 0 Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

6 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) = Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

7 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

8 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) [δ ψ e e ] = Ĉe (δω i δω ie )Ĉ e = [Ĉe (δ ω i δ ω ie ) ] (1) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

9 ECEF Attitude Error Ċ e = Ce Ω e = Ce (Ω i Ω ie ) = d dt [ ] (I +[δ ψ e e ])Ĉe = (I +[δ ψ e e ])Ĉe Ω e = [δ ψ e e ]Ĉe +(I +[δ ψ e e ]) Ĉ e = (I +[δ ψ e e ])Ĉe ( ˆΩ e + δω i δω ie ) (I +[δ ψ e e ])Ĉe ˆΩ e + Ĉe (δω i δω ie ) Ĉe (δω i δω ie ) [δ ψ e e ] = Ĉe (δω i δω ie )Ĉ e = [Ĉe (δ ω i δ ω ie ) ] (1) δ ψ e e = Ĉe (δω i δ ω ie ) (2) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

10 ECEF Attitude Error (cont.) δ ψ e e = Ĉe (δω i δ ω ie ) = Ĉe δω i Ĉe ( ω ie ˆ ω ie ) = Ĉe δω i (Ĉe Ce I) ωe ie = Ĉe δω i + δ ψ e e ωe ie Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

11 ECEF Attitude Error (cont.) δ ψ e e = Ĉe (δω i δ ω ie ) = Ĉe δω i Ĉe ( ω ie ˆ ω ie ) = Ĉe δω i (Ĉe Ce I) ωe ie = Ĉe δω i + δ ψ e e ωe ie δ ψ e e = Ĉe δω i Ω e ie δ ψ e e (3) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

12 Velocity v e e = Ce f i + ge 2Ωe ie v e e (4) ˆ v e e = Ĉe ˆ f i + ˆ g e 2Ωe ie ˆ v e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ie ˆ v e e (5) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

13 Velocity v e e = Ce f i + ge 2Ωe ie v e e (4) ˆ v e e = Ĉe ˆ f i + ˆ g e 2Ωe ie ˆ v e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ie ˆ v e e δ v e e = v e e ˆ v e e = [δ ψ e e ]Ce fi + Ĉe δ f i + δ g e 2Ωe ie δ v e e = [δ ψ e e ]Ĉe ˆ f i + Ĉe δ f i + δ g e 2Ωe ie δ v e e (5) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

14 Velocity v e e = Ce f i + ge 2Ωe ie v e e (4) ˆ v e e = Ĉe ˆ f i + ˆ g e 2Ωe ie ˆ v e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ie ˆ v e e δ v e e = v e e ˆ v e e = [δ ψ e e ]Ce fi + Ĉe δ f i + δ g e 2Ωe ie δ v e e = [δ ψ e e ]Ĉe ˆ f i + Ĉe δ f i + δ g e 2Ωe ie δ v e e (5) δ v e e = [ C ˆ e ˆ f i ]δ ψ e e + Ĉe δ f i + δ g e 2Ωe ˆ v ie e e (6) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

15 Gravity Error δ g e 2g 0(ˆL ) r e es (ˆL ) ˆ r e e ˆ r e e 2(ˆ r e e )T δ r e e (7) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

16 Position r e e = v e e (8) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

17 Position r e e = v e e (8) δ r e e = δ v e e (9) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

18 Summary - in terms of δ f i, δ ω i δ δ δ ψ e e v e e r e e Ω e ie ) = [ C ˆ e f ˆ i ] 2Ωe 2g 0 ( ˆL ) ˆ r e ie res e e ( ˆL ) 2(ˆ r e ˆ r e e e )T I Ĉ e Ĉ e 0 δ f i δ ω i 0 0 δ ψ e e δ v e e δ r e e + (10) Attitude Velocity Gravity Position Summary Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

19 Notation Used Truth value Measured value x x Estimated or computed value ˆ x Error δ x = x ˆ x Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

20 Notation Used Truth value Measured value Estimated or computed value x x Nothing aove ˆ x Error δ x = x ˆ x Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

21 Notation Used Truth value Measured value Estimated or computed value x x ˆ x Use tilde Error δ x = x ˆ x Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

22 Notation Used Truth value Measured value Estimated or computed value x x ˆ x Error δ x = x ˆ x Use hat Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

23 Notation Used Truth value Measured value x x Estimated or computed value ˆ x Error δ x δ x = x ˆ x ˆ x Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

24 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

25 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Let s assume we have an estimate of x, i.e., ˆ x such that x = ˆ x + δ x x = ˆ x + δ x = f(ˆ x + δ x, t) (11) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

26 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Let s assume we have an estimate of x, i.e., ˆ x such that x = ˆ x + δ x Using Taylor series expansion x = ˆ x + δ x = f(ˆ x + δ x, t) (11) f(ˆ x + δ x, t) = ˆ x + δ x = f(ˆ x, t) + ˆ x + f( x, t) x f( x, t) x δ x x= ˆ x x= ˆ x δ x + H.O.T Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

27 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Let s assume we have an estimate of x, i.e., ˆ x such that x = ˆ x + δ x x = ˆ x + δ x = f(ˆ x + δ x, t) (11) Using Taylor series expansion f(ˆ x + δ x, t) = ˆ x + δ x f( x, t) = f(ˆ x, t) + x = ˆ x f( x, t) + δ x x x= ˆ x x= ˆ x δ x + H.O.T Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

28 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Let s assume we have an estimate of x, i.e., ˆ x such that x = ˆ x + δ x Using Taylor series expansion x = ˆ x + δ x = f(ˆ x + δ x, t) (11) f(ˆ x + δ x, t) = ˆ x + δ x = f(ˆ x, t) + ˆ x + δ x f( x, t) x f( x, t) x f( x, t) x δ x x= ˆ x x= ˆ x x= ˆ x δ x + H.O.T δ x (12) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

29 Actual Measurements Initially the accelerometer and gyroscope measurements, f i and ω i, respectively, will e modeled as f i = f i + f i (13) ω i = ω i + ω i (14) where f i and ω i are the specific force and angular rates, respectively; and f i and ω i represents the errors. In later lectures we will discuss more detailed description of these errors. Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

30 Actual Measurements Initially the accelerometer and gyroscope measurements, f i and ω i, respectively, will e modeled as f i = f i + f i (13) these terms may ω i = ω i + ω e expanded further i (14) where f i and ω i are the specific force and angular rates, respectively; and f i and ω i represents the errors. In later lectures we will discuss more detailed description of these errors. Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

31 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Gyroscopes ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

32 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Biases Gyroscopes ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

33 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Misalignment and SF Errors Gyroscopes ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

34 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Non-linearity Gyroscopes ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

35 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Gyroscopes G-Sensitivity ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

36 Error Modeling Example Accelerometers f i = a +(I + M a ) f i + nla + w a (15) Noise Gyroscopes ω i = g +(I + M g ) ω i + G g f i + w g (16) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

37 Pos, Vel, Force and Angular Rate Errors Position error Velocity error δ r γ β = r γ β ˆ r γ β (17) δ v γ β = v γ β ˆ v γ β (18) Specific force errors Angular rate errors δ f i = f i ˆ f i (19) e f i = f i ˆ f i = δ f i (20) δ ω i = ω i ˆ ω i (21) e ω i = ω i ˆ ω i = δ ω i (22) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

38 Attitude Error Definition Define δc γ = Cγ Ĉ γ = e [δ ψ γ γ ] I +[δ ψ γ γ ] (23) This is the error in attitude resulting from errors in estimating the angular rates. Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

39 Attitude Error Properties The attitude error is a multiplicative small angle transformation from the actual frame to the computed frame Ĉ γ = (I [δ ψ γ γ ])Cγ (24) Similarly, C γ = (I +[δ ψ γ γ ])Ĉγ (25) Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15

40 Specific Force and Agnular Rates Similarly we can attempt to estimate the specific force and angular rate y applying correction ased on our estimate of the error. ˆ f i = f i ˆ f i (26) ˆ ω i = ω i ˆ ω i (27) where ˆ f i and ˆ ω i are the accelerometer and gyroscope estimated caliration values, respectively. Aly El-Osery (NMT) EE 570: Location and Navigation April 11, / 15