EE 570: Location and Navigation

Transcription

1 EE 570: Location and Navigation Aided INS Aly El-Osery Kevin Wedeward Electrical Engineering Department, New Mexico Tech Socorro, New Mexico, USA In Collaboration with Stephen Bruder Electrical and Computer Engineering Department Embry-Riddle Aeronautical Univesity Prescott, Arizona, USA April 13, 2016 Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

2 Notation Used Truth value Measured value x x Estimated or computed value ˆ x Error δ x = x ˆ x Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

3 Actual Measurements Initially the accelerometer and gyroscope measurements, f b ib and ω b ib, respectively, will be modeled as f b ib = f b ib + f b ib = ˆ f b ib + ˆ f b ib (1) ω b ib = ω b ib + ω b ib = ˆ ω b ib + ˆ ω b ib (2) where f b ib and ω b ib are the specific force and angular rates, respectively; and f b ib and ω b ib represents the errors. In later lectures we will discuss more detailed description of these errors. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

4 Error Modeling Example Accelerometers f b ib = b a + (I + M a ) f b ib + nl a + w a (3) Gyroscopes ω b ib = b g + (I + M g ) ω b ib + G g f b ib + w g (4) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

5 Pos, Vel, Force and Angular Rate Errors Position error Velocity error Specific force errors δ r γ βb = r γ βb ˆ r γ βb (5) δ v γ βb = v γ βb ˆ v γ βb (6) δ f b ib = f b ib ˆ f b ib (7) ef b ib = f b ib ˆ f b ib = δf b ib (8) Angular rate errors δ ω b ib = ω b ib ˆ ω b ib (9) e ω b ib = ω b ib ˆ ω b ib = δ ω b ib (10) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

6 ECEF Error Mechanization Recall δ ψ e Ω e ie eb δ v e = [ Cˆ eb b e f ˆ b ib ] 2Ωe 2g 0 (ˆL b ) ˆ r eb e ie (ˆ r e δ r e res e (ˆL b ) ˆ r eb e 2 eb )T δ ψ e eb δ v e eb + eb I δ r e 3 3 eb 0 Ĉ e ( ) b Ĉb e 0 ef b ib 0 0 e ω b ib (11) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

7 Errors After Calibration In reality there will be error terms in the sensor that can not be calibrated. These terms may be estimated. The error in the estimation of these terms may be expressed as e f b ib = f b ib ˆ f b ib = F va δ x a + ς a (12) e ω b ib = ω b ib ˆ ω b ib = F ψg δ x g + ς g (13) These terms represent the difference between what we estimate the errors in the sensors to be (either through calibration or online estimation) and the actual errors in the sensor. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

8 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 biases, e.g., δ x a = δ b 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. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

9 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. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

10 State Augmentation After state augmentation δ ψ e eb δ v Ω e ie Ĉb ef ψg e eb [ C ˆ b e f ˆ b ib ] 2Ωe 2g 0(ˆL b ) ˆ r eb e ie (ˆ r r δ r e es e (ˆL b ) ˆ r eb e 2 eb e )T Ĉb ef va = eb I δ x a F aa δ x g F gg Ĉ b e ς g Ĉb e ς a I w a I 3 3 w g = F (t) x + G w δ ψ e eb δ v e eb δ r e + eb δ x a δ x g (16) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

11 Need for Integration Gyros ω b ib Position r Aiding Sensor (e.g., GPS) Accelerometers IMU f b INS Mechanization Equations Velocity Attitude v C 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 INS Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

12 Open-Loop Integration True PVA + errors Aiding sensors errors - INS errors Aiding Sensors + Filter INS + + Inertial errors est. True PVA + errors Correct INS Output Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

13 Closed-Loop Integration If error estimates are fedback to correct the INS mechanization, a reset of the state estimates becomes necessary. Aiding Sensors + Filter INS INS Correction Correct INS Output Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

14 Kalman Filter ˆ x k k 1 = Φ k 1ˆ xk 1 k 1 (17) Project Ahead 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) Update 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 kp k k 1 H T k + R k) 1 (21) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

15 Closed-Loop Kalman Filter Since the errors are being fedback to correct the INS, the state estimate must be reset after each INS 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 kp k k 1 H T k + R k) 1 (26) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

16 Discretization Φ k 1 I + F t (27) nrg 2 I n 2 ag I Q = (28) nbad 2 I nbgd 2 I 3 3 where t is the sample time, nrg 2, nag 2, nbad 2, n2 bgd are the PSD of the gyro and accel random noise, and accel and gyro bias variation, respectively. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17

17 Discrete Covariance Matrix Q k Assuming white noise, small time step, G is constant over the integration period, and the trapezoidal integration Q k [ ] Φ k 1 G k 1 Q(t k 1 )G T k 1 ΦT k 1 + G k 1Q(t k 1 )G T k 1 t (29) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 13, / 17