EE 570: Location and Navigation

Transcription

1 EE 570: Location and Navigation Error Mechanization (Tangential) 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 23, 2014 Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

2 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

3 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

4 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

5 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) [δ ψ t t ]δω t 0 Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

6 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) = Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

7 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) [δ ψ t t ] = Ĉt (δω i δω ie)ĉt = [Ĉ t(δ ω i δ ω ie) ] (1) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

8 Tangential Attitude Error Ċ t = Ct Ω t = Ct (Ω i Ω ie) = d dt [ (I +[δ ψ t t ] ])Ĉt = (I +[δ ψ t t ])Ĉt Ω t = [δ ψ t t ]Ĉt +(I +[δ ψ t t ]) Ĉ t = (I +[δ ψ t t ])Ĉt ( ˆΩ t + δω i δω ie) (I +[δ ψ t t ])Ĉt ˆΩ t + Ĉt (δω i δω ie) [δ ψ t t ] = Ĉt (δω i δω ie)ĉt = [Ĉ t(δ ω i δ ω ie) ] (1) δ ψ t t = Ĉt (δω i δ ω ie) (2) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

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

10 Tangential Attitude Error (cont.) δ ψ t t = Ĉt (δω i δ ω ie) = Ĉ t δω i Ĉt ( ω ie ˆ ω ie) = Ĉ t δω i (Ĉt C t I) ω t ie = Ĉ t δω i + δ ψ t t ωt ie δ ψ t t = Ĉt δω i Ω t ie δ ψ t t (3) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

11 Velocity v t t = Ct f i + gt 2Ωt ie v t t (4) ˆ v t t = Ĉt ˆ f i + ˆ g t 2Ωt ˆ v ie t t = (I [δ ψ t t ])Ct ( f i δ f i )+ ˆ g t 2Ωt ˆ v ie t t (5) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

12 Velocity v t t = Ct f i + gt 2Ωt ie v t t (4) ˆ v t t = Ĉt ˆ f i + ˆ g t 2Ωt ˆ v ie t t = (I [δ ψ t t ])Ct ( f i δ f i )+ ˆ g t 2Ωt ˆ v ie t t δ v t t = v t t ˆ v t t = [δ ψ t t ]Ct fi + Ĉt δ f i + δ g t 2Ωt ieδ v t t = [δ ψ t t ]Ĉt ˆ f i + Ĉt δ f i + δ g t 2Ωt ieδ v t t (5) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

13 Velocity v t t = Ct f i + gt 2Ωt ie v t t (4) ˆ v t t = Ĉt ˆ f i + ˆ g t 2Ωt ˆ v ie t t = (I [δ ψ t t ])Ct ( f i δ f i )+ ˆ g t 2Ωt ˆ v ie t t δ v t t = v t t ˆ v t t = [δ ψ t t ]Ct fi + Ĉt δ f i + δ g t 2Ωt ieδ v t t = [δ ψ t t ]Ĉt ˆ f i + Ĉt δ f i + δ g t 2Ωt ieδ v t t (5) δ v t t = [ C ˆ t ˆ f i ]δ ψ t t + Ĉt δ f i + δ g t 2Ωt ˆ v ie t t (6) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 15

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

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

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

17 Summary - in terms of δ f i, δ ω i δ ψ t t Ω t ie δ v t t = [ ˆ C t ˆ f i ] 2Ωt ie Ĉe t 2g 0 ( ˆL ) ˆ r res e e e ( ˆL ) 2(ˆ r e ˆ r e e e )T Ĉt e δ r t t I Ĉ t Ĉ t 0 δ f i δ ω i 0 0 δ ψ t t δ v t t δ r t t + (10) Attitude Velocity Gravity Position Summary Aly El-Osery, Stephen Bruder (NMT,ERAU) EE 570: Location and Navigation March 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 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 23, / 15