EE 565: Position, Navigation, and Timing

EE 565: Position, Navigation, and Timing Kalman Filtering Example 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 1, 218 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 1 / 33

Review: System Model x(t) = F (t) x(t) + G(t) w(t) (1) y(t) = H(t) x(t) + v(t) (2) System Discretization Φ k 1 = e F k 1τ s I + F k 1 τ s (3) where F k 1 is the average of F at times t and t τ s, and first order approximation is used. Leading to x k = Φ k 1 x k 1 + w k 1 (4) z k = H k x k + v k (5) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 2 / 33

Review: Assumptions w k and v k are drawn from a Gaussian distribution, uncorrelated have zero mean and statistically independent. { E{ w k w i T Q k i = k } = (6) i k { E{ v k v i T R k i = k } = (7) E{ w k v T i } = i k { i, k (8) State covariance matrix Q k 1 1 [ ] Φ k 1 G k 1 Q(t k 1 )Gk 1 T 2 ΦT k 1 + G k 1Q(t k 1 )Gk 1 T τ s (9) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 3 / 33

Review: Kalman filter data flow Initial estimate (ˆ x and P) Compute Kalman gain Kk = P k k 1 Hk T (HkP k k 1Hk T + Rk) 1 Project ahead ˆ x k k 1 = Φk 1ˆ x k 1 k 1 P k k 1 = Qk 1 + Φk 1P k 1 k 1 Φ T k 1 Update estimate with measurement zk ˆ x k k = ˆ x k k 1 + Kk( zk Hk ˆ x k k 1 ) k = k + 1 Update error covariance P k k = (I K khk) P k k 1 (I K khk) T + K krkk T k Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 4 / 33

Remarks Kalman filter (KF) is optimal under the assumptions that the system is linear and the noise is uncorrelated Under these assumptions KF provides an unbiased and minimum variance estimate. If the Gaussian assumptions is not true, Kalman filter is biased and not minimum variance. If the noise is correlated we can augment the states of the system to maintain the uncorrelated requirement of the system noise. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 5 / 33

Correlated State Noise Given a state space system x 1 (t) = F 1 (t) x 1 (t) + G 1 (t) w 1 (t) y 1 (t) = H 1 (t) x 1 (t) + v 1 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 6 / 33

Correlated State Noise Given a state space system x 1 (t) = F 1 (t) x 1 (t) + G 1 (t) w 1 (t) y 1 (t) = H 1 (t) x 1 (t) + v 1 (t) As we have seen the noise w 1 (t) may be non-white, e.g., correlated Gaussian noise, and as such may be modeled as x 2 (t) = F 2 (t) x 2 (t) + G 2 (t) w 2 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 6 / 33

Correlated State Noise Given a state space system x 1 (t) = F 1 (t) x 1 (t) + G 1 (t) w 1 (t) y 1 (t) = H 1 (t) x 1 (t) + v 1 (t) As we have seen the noise w 1 (t) may be non-white, e.g., correlated Gaussian noise, and as such may be modeled as x 2 (t) = F 2 (t) x 2 (t) + G 2 (t) w 2 (t) w 1 (t) = H 2 (t) x 2 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 6 / 33

Correlated State Noise Define a new augmented state x aug = x 1(t) (1) x 2 (t) therefore, and x aug = x 1 (t) = F 1(t) G 1 H 2 (t) x 1(t) + w 2 (t) (11) x 2 (t) F 2 (t) x 2 (t) G 2 (t) y(t) = ( ) H 1 (t) x 1(t) + v 1 (t) (12) x 2 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 7 / 33

Correlated Measurement Noise Given a state space system x 1 (t) = F 1 (t) x 1 (t) + G 1 (t) w(t) y 1 (t) = H 1 (t) x 1 (t) + v 1 (t) In this case the measurement noise v 1 may be correlated x 2 (t) = F 2 (t) x 2 (t) + G 2 (t) v 2 (t) v 1 (t) = H 2 (t) x 2 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 8 / 33

Correlated Measurement Noise Define a new augmented state x aug = x 1(t) (13) x 2 (t) therefore, x aug = x 1 (t) = F 1(t) x 1(t) + G 1(t) w(t) (14) x 2 (t) F 2 (t) x 2 (t) G 2 (t) v 2 (t) and y(t) = ( H 1 (1) ) H 2 (t) x 1(t) (15) x 2 (t) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 9 / 33

Design Example You are to design a system that estimates the position and velocity of a moving point in a straight line. You have: 1 an accelerometer corrupted with noise 2 an aiding sensor allowing you to measure absolute position that is also corrupted with noise. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 1 / 33

Specification Sampling Rate Fs = 1Hz. Accelerometer specs 1 VRW = 1mg/ Hz. 2 BI = 7mg with correlation time 6s. Position measurement is corrupted with WGN. N (, σ 2 p), where σ p = 2.5m Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 11 / 33

Input - Acceleration True Acceleration and Acceleration with Noise 2 1.5 1.5 m/s 2.5 1 1.5 2 1 2 3 4 5 Meas Accel Kalman Filter True Accel State Augmentation Example Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 12 / 33

Aiding Position Measurement Absolute position measurement corrupted with noise 7 6 5 4 m 3 2 1 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 13 / 33

Computed Position and Velocity Using only the acceleration measurement and an integration approach to compute the velocity, then integrate again to get position. Velocity Position 2 2 15 15 1 m/s 5 m 1 5 5 1 1 2 3 4 5 1 2 3 4 5 Directly Computed Vel True Vel Directly Computed Pos True Pos Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 14 / 33

Different Approaches 1 Clean up the noisy input to the system by filtering 2 Use Kalman filtering techniques with Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 15 / 33

Different Approaches 1 Clean up the noisy input to the system by filtering 2 Use Kalman filtering techniques with A model of the system dynamics (too restrictive) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 15 / 33

Different Approaches 1 Clean up the noisy input to the system by filtering 2 Use Kalman filtering techniques with A model of the system dynamics (too restrictive) A model of the error dynamics and correct the system output in Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 15 / 33

Different Approaches 1 Clean up the noisy input to the system by filtering 2 Use Kalman filtering techniques with A model of the system dynamics (too restrictive) A model of the error dynamics and correct the system output in open-loop configuration, or closed-loop configuration. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 15 / 33

Approach 1 Filtered input Filtered Accel Measurement m/s 2 2 1.5 1.5.5 1 1.5 2 Accelerometer 1 2 3 4 5 Measured Filtered Truth Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 16 / 33

Approach 1 Filtered input Position and Velocity Velocity Position 2 15 1 Directly Computed Vel True Vel Vel (using filtered accel) 2 15 Directly Computed Pos True Pos Pos (using filtered accel) m/s 5 m 1 5 5 1 1 2 3 4 5 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 17 / 33

Approach 1 Filtered input Position and Velocity Errors Velocity Error Position Error 2 Using filtered acc Using unfiltered acc 5 Using filtered acc Using unfiltered acc 2 4 5 m/s 6 m 1 8 1 15 12 1 2 3 4 5 2 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 18 / 33

Open-Loop Integration True Pos + errors Aiding sensor errors - INS errors Aiding Pos Sensor + Filter INS + + Inertial errors est. INS PV + errors Correct INS Output Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 19 / 33

Closed-Loop Integration If error estimates are fedback to correct the INS mechanization, a reset of the state estimates becomes necessary. Aiding Pos Sensor + Filter INS INS Correction Correct INS Output Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 2 / 33

Covariance Matrices State noise covariance matrix (continuous) State noise covariance matrix (discrete) E{ w(t) w T (τ)} = Q(t)δ(t τ) E{ w k w T i } = Measurement noise covariance matrix Initial error covariance matrix E{ v k v T i } = { Q k i = k i k { R k i = k i k P = E{( x ˆ x )( x ˆ x ) T } = E{ e ˆ e T } Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 21 / 33

System Modeling The position, velocity and acceleration may be modeled using the following kinematic model. ṗ(t) = v(t) v(t) = a(t) (16) where a(t) is the input. Therefore, our estimate of the position is ˆp(t) that is the double integration of the acceleration. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 22 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as ã(t) = a(t) + b(t) + w a (t) (17) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 23 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as ã(t) = a(t) + b(t) + w a (t) (17) and the bias is Markov, therefore ḃ(t) = 1 b(t) + w b (t) (18) T c where w a (t) and w b (t) are zero mean WGN with variances, respectively, Fs VRW 2 E{w b (t)w b (t + τ)} = Q b (t)δ(t τ) (19) and T c is the correlation time and σ BI is the bias instability. Q b (t) = 2σ2 BI T c (2) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 23 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as ã(t) = a(t) + b(t) + w a (t) (17) and the bias is Markov, therefore ḃ(t) = 1 b(t) + w b (t) (18) T c where w a (t) and w b (t) are zero mean WGN with variances, respectively, Fs VRW 2 E{w b (t)w b (t + τ)} = Q b (t)δ(t τ) (19) Q b (t) = 2σ2 BI T c (2) and T c is the correlation time and σ BI is the bias instability. Make sure that the VRW and σ BI are converted to have SI units. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 23 / 33

Error Mechanization Define error terms as δp(t) = p(t) ˆp(t), (21) and δṗ(t) = ṗ(t) ˆp(t) = v(t) ˆv(t) = δv(t) δ v(t) = v(t) ˆv(t) = a(t) â(t) = b(t) w a (t) (22) (23) where b(t) is modeled as shown in Eq. 18 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 24 / 33

State Space Formulation δṗ(t) 1 δp(t) x(t) = δ v(t) = 1 δv(t) + 1 w a (t) ḃ(t) 1 T c b(t) 1 w b (t) = F (t) x(t) + G(t) w(t) (24) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 25 / 33

Covariance Matrix The continuous state noise covariance matrix Q(t) is Q(t) = VRW 2 2σ 2 BI Tc (25) The measurement noise covariance matrix is R = σ 2 p, where σ p is the standard deviation of the noise of the absolute position sensor. Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 26 / 33

Discretization Now we are ready to start the implementation but first we have to discretize the system. x(k + 1) = Φ(k) x(k) + w d (k) (26) where with the measurement equation Φ(k) I + Fdt (27) y(k) = H x + w p (k) = δp(k) + w p (k) (28) where H = [1 ]. The discrete Q d is approximated as Q k 1 1 2 [Φ k 1G(t k 1 )Q(t k 1 )G T (t k 1 ))Φ T k 1 + G(t k 1 )Q(t k 1 )G T (t k 1 )]dt (29) Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 27 / 33

Approach 2 Open-Loop Compensation Position and Velocity Open-loop Correction Best estimate = INS out (pos & vel) + KF est error (pos & vel) Velocity Position 2 15 Directly Computed Vel True Vel Estimated Vel (KF) 2 15 Directly Computed Pos True Pos Estimated Pos (KF) 1 m/s 5 m 1 5 5 1 1 2 3 4 5 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 28 / 33

Approach 2 Open-Loop Compensation Position and Velocity Errors Velocity Error Position Error 2 5 2 4 5 m/s 6 m 1 8 1 Error in Estimated Vel (KF) Error in Computed 15 Error in Pos Measurement Error in Computed Pos Error in Estimated Pos (KF) 12 1 2 3 4 5 2 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 29 / 33

Approach 2 Open-Loop Compensation Pos Error & Bias Estimate 8 Position Error 1.5 6 4 1 m 2 2 m.5 4 6.5 8 1 2 3 4 5 Error in Pos Measurement Error in Estimated Pos (KF) Bias 1 2 3 4 5 Est. Bias True Bias Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 3 / 33

Approach 3 Closed-Loop Compensation Position and Velocity Closed-loop Correction Best estimate = INS out (pos,vel, & bias) + KF est error (pos, vel & bias) Use best estimate on next iteration of INS Accel estimate = accel meas - est bias Reset state estimates before next call to KF 2 15 Directly Computed Vel True Vel Estimated Vel (KF) 2 15 Directly Computed Pos True Pos Estimated Pos (KF) 1 m/s 5 m 1 5 5 Velocity 1 1 2 3 4 5 Position 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 31 / 33

Approach 3 Closed-Loop Compensation Position and Velocity Errors Velocity Error Position Error 2 5 2 4 5 m/s 6 m 1 8 1 Error in Estimated Vel (KF) Error in Computed 15 Error in Pos Measurement Error in Computed Pos Error in Estimated Pos (KF) 12 1 2 3 4 5 2 1 2 3 4 5 Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 32 / 33

Approach 3 Closed-Loop Compensation Pos Error & Bias Estimate 8 Position Error 1.5 6 4 1 m 2 2 m.5 4 6.5 8 1 2 3 4 5 Error in Pos Measurement Error in Estimated Pos (KF) Bias 1 2 3 4 5 Est. Bias True Bias Aly El-Osery, Kevin Wedeward (NMT) EE 565: Position, Navigation, and Timing April 1, 218 33 / 33