EE 570: Location and Navigation

Transcription

1 EE 570: Location and Navigation Error Mechanization (ECEF) Aly El-Osery 1 Stephen Bruder 2 1 Electrical Engineering Department, New Mexico Tech Socorro, New Mexico, USA 2 Electrical and Computer Engineering Department, Emry-Riddle Aeronautical Univesity Prescott, Arizona, USA March 13, 2014 Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 e ]δω e 0 Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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) = Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 e ] = Ĉe (δω i δω ie)ĉe = [Ĉ e(δ ω i δ ω ie) ] (1) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 e ] = Ĉe (δω i δω ie)ĉe = [Ĉ e(δ ω i δ ω ie) ] (1) δ ψ e e = Ĉe (δω i δ ω ie) (2) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

9 ECEF Attitude Error (cont.) δ ψ e e = Ĉe (δω i δ ω ie) = Ĉ e δω i Ĉe ( ω ie ˆ ω ie) = Ĉ e δω i (Ĉe C e I) ω e ie = Ĉ e δω i + δ ψ e e ωe ie Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

10 ECEF Attitude Error (cont.) δ ψ e e = Ĉe (δω i δ ω ie) = Ĉ e δω i Ĉe ( ω ie ˆ ω ie) = Ĉ e δω i (Ĉe C e 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

11 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 ˆ v ie e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ˆ v ie e e (5) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 ˆ v ie e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ˆ v ie 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 ˆ v ie e e = (I [δ ψ e e ])Ce ( f i δ f i )+ ˆ g e 2Ωe ˆ v ie 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

14 Gravity Error δ g e 2g 0(ˆL ) ˆ r e e res e (ˆL ) ˆ r 2(ˆ r e e e e )T δ r e e (7) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

15 Position r e e = ve e (8) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

16 Position r e e = ve e (8) δ r e e = δ v e e (9) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

17 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

18 Notation Used Truth value Measured value x x Estimated or computed value ˆ x Error δ x = x ˆ x Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

19 Notation Used Truth value Measured value Estimated or computed value x x Nothing aove ˆ x Error δ x = x ˆ x Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

20 Notation Used Truth value Measured value Estimated or computed value x x ˆ x Use tilde Error δ x = x ˆ x Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

21 Notation Used Truth value Measured value Estimated or computed value x x ˆ x Error δ x = x ˆ x Use hat Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

22 Notation Used Truth value Measured value x x Estimated or computed value ˆ x Error δ x δ x = x ˆ x ˆ x Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

23 Linearization using Taylor Series Expansion Given a non-linear system x = f( x, t) Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

24 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

28 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 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) 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

30 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

31 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

32 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

33 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

34 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

35 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

36 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

37 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

38 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15

39 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, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 13, / 15