Lecture Aided EE 570: Location and Navigation Lecture Notes Update on April 13, 2016 Aly El-Osery and Kevin Wedeward, Electrical Engineering Dept., New Mexico Tech In collaoration with Stephen Bruder, Electrical & Computer Engineering, Emry-Riddle Aeronautical University.1 1 Overview 1.1 ECEF as and Example Notation Used Truth value Measured value Estimated or computed value Error x x ˆ x δ x = x ˆ x.2 1.2 Inertial Measurements Actual Measurements Initially the accelerometer and gyroscope measurements, f i and ω i, respectively, will e modeled as f i = f i f i = ˆ i ˆ i (1) ω i = ω i ω i = ˆ ω i ˆ ω i (2) 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..3 Error Modeling Example Accelerometers f i = a (I M a ) i nl a w a (3) 1
Gyroscopes ω i = g (I M g ) ω i G g i w g (4).4 Pos, Vel, Force and Angular Rate Errors Position error Velocity error Specific force errors δ r γ β = r γ β ˆ r γ β (5) δ v γ β = v γ β ˆ v γ β (6) δ i = i ˆ i (7) Angular rate errors e i = i ˆ i = δ i (8) δ ω i = ω i ˆ ω i (9) e ω i = ω i ˆ ω i = δ ω i (10).5 ECEF Error Mechanization Recall δ ψ e Ω e ie 0 3 3 0 3 3 e δ v e = [ C ˆe e δ r ˆ f i ] 2Ωe 2g 0( ˆL ) ˆ r e ie e res e ( ˆL e (ˆ r e ) ˆ r e e 2 e )T δ ψ e e δ v e e e 0 3 3 I 3 3 0 3 3 δ r e e 0 Ĉe ( ) Ĉe 0 ef i 0 0 e ω i (11).6 Errors After Caliration In reality there will e error terms in the sensor that can not e calirated. These terms may e estimated. The error in the estimation of these terms may e expressed as e i = i ˆ i = F va δ x a ς a (12) e ω i = ω i ˆ ω i = F ψg δ x g ς g (13) These terms represent the difference etween what we estimate the errors in the sensors to e (either through caliration or online estimation) and the actual errors in the sensor..7 Error Terms The matrics F va and F ψg, depend on the needed level of complexity in modeling the errors. For example if we only model iases, e.g., δ x a = δ a, then F va = I. If more error terms are modeled, then most likely, we will end up with non-linear equations, and therefore linearization is necessary..8 Error Modeling δ x a = F aa δ x a w a (14) δ x g = F gg δ x g w g (15) The matrics F aa and F gg are specific to accelerometers and the gyroscopes and there specific configuration within the IMU..9 2
State Augmentation After state augmentation δ ψ e e Ω e ie 0 δ v 3 3 0 3 3 0 3 3 Ĉe F ψg e e [ C δ r ˆ e ˆ f i ] 2Ωe 2g 0( ˆL ) ˆ r e ie res e e ( ˆL e (ˆ r ) ˆ r e e 2 e e )T Ĉe F va 0 3 3 e = δ x 0 3 3 I 3 3 0 3 3 0 3 3 0 3 3 a δ x 0 3 3 0 3 3 0 3 3 F aa 0 3 3 g 0 3 3 0 3 3 0 3 3 0 3 3 F gg Ĉe 0 3 3 0 3 3 0 3 3 0 3 3 ς g 0 3 3 Ĉe 0 3 3 0 3 3 0 3 3 ς a 0 3 3 0 3 3 0 3 3 0 3 3 0 3 3 0 3 1 0 3 3 0 3 3 0 3 3 I 3 3 0 3 3 w a 0 3 3 0 3 3 0 3 3 0 3 3 I 3 3 w g = F (t) x G w δ ψ e e δ v e e δ r e e δ x a δ x g (16).10 1.3 Background Need for Integration Aiding Sensor (e.g., GPS) Gyros ω i Position r Accelerometers IMU f Mechanization Equations Velocity Attitude v C http://nptel.ac.in/courses/105104100/lectureb_11/b_11_3gdop.htm Advantages Immune to RF Jaming High data rate High accuracy in short term Disadvantages Drifts Errors are time dependent Need Initialization Advantages Errors time-indep. No initialization Disadvantages Sensitive to RF Interference No attitude information Aided.11 2 Integration Architectures Open-Loop Integration 3
True PVA errors Aiding sensors errors - errors Aiding Sensors Filter Inertial errors est. True PVA errors Correct Output.12 Closed-Loop Integration If error estimates are fedack to correct the mechanization, a reset of the state estimates ecomes necessary. Aiding Sensors Filter Correction Correct Output.13 3 Integration Filter Kalman Filter ˆ x k k 1 = Φ k 1ˆ xk 1 k 1 (17) P k k 1 = Q k 1 Φ k 1 P k 1 k 1 Φ T k 1 (18) ˆ x k k = ˆ x k k 1 K k ( z k H k ˆ xk k 1 ) (19) P k k = (I K k H k ) P k k 1 (I K k H k ) T K k R k K T k (20) K k = P k k 1 H T k (H k P k k 1 H T k R k ) 1 (21).14 4
Closed-Loop Kalman Filter Since the errors are eing fedack to correct the, the state estimate must e reset after each correction. ˆ x k k 1 = 0 (22) P k k 1 = Q k 1 Φ k 1 P k 1 k 1 Φ T k 1 (23) ˆ x k k = K k z k (24) P k k = (I K k H k ) P k k 1 (I K k H k ) T K k R k K T k (25) K k = P k k 1 H T k (H k P k k 1 H T k R k ) 1 (26).15 Discretization Φ k 1 I F t (27) n 2 rgi 3 3 0 3 3 0 3 3 0 3 3 0 3 3 0 3 3 n 2 agi 3 3 0 3 3 0 3 3 0 3 3 Q = 0 3 3 0 3 3 0 3 3 0 3 3 0 3 3 (28) 0 3 3 0 3 3 0 3 3 n 2 ad I 3 3 0 3 3 0 3 3 0 3 3 0 3 3 0 3 3 n 2 gd I 3 3 where t is the sample time, n 2 rg, n 2 ag, n 2 ad, n2 gd are the PSD of the gyro and accel random noise, and accel and gyro ias variation, respectively..16 Discrete Covariance Matrix Q k Assuming white noise, small time step, G is constant over the integration period, and the trapezoidal integration Q k 1 1 2 [ Φk 1 G k 1 Q(t k 1 )G T k 1Φ T k 1 G k 1 Q(t k 1 )G T ] k 1 t (29).17 5