EE 570: Location and Navigation 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 12, 2016

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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)

Φk−1 = e Fk−1 τs ≈ I + Fk−1 τs

(3)

System Discretization

where Fk−1 is the average of F at times t and t − τs , and first order approximation is used. Leading to

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

~xk = Φk−1 ~xk−1 + w ~ k−1

(4)

~zk = Hk ~xk + ~vk

(5)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

2 / 33

Review: Assumptions ~ k and ~vk are drawn from a Gaussian distribution, uncorrelated w have zero mean and statistically independent. ( Qk i = k T ~i } = E{w~k w (6) 0 i 6= k ( Rk i = k = 0 i 6= k n E{w~k ~viT } = 0 ∀i, k

E{v~k ~viT }

(7) (8)

State covariance matrix i 1h T T Qk−1 ≈ Φk−1 Gk−1 Q(tk−1 )Gk−1 ΦT + G Q(t )G k−1 k−1 k−1 k−1 τs 2 (9) Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

3 / 33

Review: Kalman filter data flow

Initial estimate (~xˆ0 and P0 ) Compute Kalman gain Kk = Pk|k−1 HkT (Hk Pk|k−1 HkT + Rk )−1

Project ahead ~xˆk|k−1 = Φk−1~xˆk−1|k−1 Pk|k−1 = Qk−1 + Φk−1 Pk−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 k H k ) P k|k−1 (I − K k H k )T + K k R k K T k

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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.

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

5 / 33

Correlated State Noise

Given a state space system ~x˙ 1 (t) = F1 (t)~x1 (t) + G1 (t)~ w1 (t) ~y1 (t) = H1 (t)~x1 (t) + ~v1 (t)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

6 / 33

Correlated State Noise

Given a state space system ~x˙ 1 (t) = F1 (t)~x1 (t) + G1 (t)~ w1 (t) ~y1 (t) = H1 (t)~x1 (t) + ~v1 (t) ~ 1 (t) may be non-white, e.g., correlated As we have seen the noise w Gaussian noise, and as such may be modeled as ~x˙ 2 (t) = F2 (t)~x2 (t) + G2 (t)~ w2 (t)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

6 / 33

Correlated State Noise

Given a state space system ~x˙ 1 (t) = F1 (t)~x1 (t) + G1 (t)~ w1 (t) ~y1 (t) = H1 (t)~x1 (t) + ~v1 (t) ~ 1 (t) may be non-white, e.g., correlated As we have seen the noise w Gaussian noise, and as such may be modeled as ~x˙ 2 (t) = F2 (t)~x2 (t) + G2 (t)~ w2 (t) ~ 1 (t) = H2 (t)~x2 (t) w

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

6 / 33

Correlated State Noise Define a new augmented state

~xaug

  ~x1 (t)  = ~x2 (t)

(10)

therefore,        ~x˙ 1 (t) ~x1 (t) F1 (t) G1 H2 (t) 0 =  + w ~x˙ aug =  ~ 2 (t) (11) ~x˙ 2 (t) ~x2 (t) 0 F2 (t) G2 (t) and

  ~x1 (t)  + ~v1 (t) ~y (t) = H1 (t) 0  ~x2 (t) 

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)



(12)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

7 / 33

Correlated Measurement Noise

Given a state space system ~x˙ 1 (t) = F1 (t)~x1 (t) + G1 (t)~ w (t) ~y1 (t) = H1 (t)~x1 (t) + ~v1 (t) In this case the measurement noise ~v1 may be correlated ~x˙ 2 (t) = F2 (t)~x2 (t) + G2 (t)~v2 (t) ~v1 (t) = H2 (t)~x2 (t)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

8 / 33

Correlated Measurement Noise Define a new augmented state

~xaug

  ~x1 (t)  = ~x2 (t)

(13)

therefore,         ~x˙ 1 (t) ~x1 (t) ~ (t) F1 (t) 0 G1 (t) 0 w =  +   ~x˙ aug =  ~x˙ 2 (t) ~x2 (t) ~v2 (t) 0 F2 (t) 0 G2 (t) (14) and     ~x1 (t)  ~y (t) = H1 (1) H2 (t)  (15) ~x2 (t) Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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.

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

10 / 33

Specification

Sampling Rate Fs = 100Hz. Accelerometer specs 1 2

√ VRW = 1mg / Hz. BI = 7mg with correlation time 6s.

Position measurement is corrupted with WGN. ∼ N (0, σp2 ), where σp = 2.5m

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

11 / 33

Input - Acceleration True Acceleration and Acceleration with Noise 2 1.5 1

m/s 2

0.5 0 −0.5 −1 −1.5 −2 0

10

20

30

40

50

Time (sec) Meas Accel True Accel Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

12 / 33

Aiding Position Measurement

Absolute position measurement corrupted with noise 70 60 50

m

40 30 20 10 0 0

10

20

30

40

50

Time (sec)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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 200

20

15 150

m

m/s

10

5

100

0 50 −5 0

−10 0

10

20

30

40

50

0

10

Directly Computed Vel True Vel

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

20

30

40

50

Time (sec)

Time (sec) Directly Computed Pos True Pos

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

14 / 33

Different Approaches

1 2

Clean up the noisy input to the system by filtering Use Kalman filtering techniques with

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

15 / 33

Different Approaches

1 2

Clean up the noisy input to the system by filtering Use Kalman filtering techniques with A model of the system dynamics (too restrictive)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

15 / 33

Different Approaches

1 2

Clean up the noisy input to the system by filtering 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

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

15 / 33

Different Approaches

1 2

Clean up the noisy input to the system by filtering 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.

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

15 / 33

Approach 1 — Filtered input Filtered Accel Measurement

Accelerometer 2 1.5

m/s 2

1 0.5 0 −0.5 −1 −1.5 −2 0

10

20

30

40

50

Time (sec) Measured Filtered

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

Truth

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

16 / 33

Approach 1 — Filtered input Position and Velocity

Velocity

Position 200

20 Directly Computed Vel True Vel Vel (using filtered accel)

15

Directly Computed Pos True Pos Pos (using filtered accel)

150

m

m/s

10

5

100

0 50 −5 0

−10 0

10

20

30

40

50

0

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

17 / 33

Approach 1 — Filtered input Position and Velocity Errors

Velocity Error

Position Error 50

2

Using filtered acc Using unfiltered acc

Using filtered acc Using unfiltered acc

0

0 −2 −50 m

m/s

−4 −6

−100

−8 −150 −10 −200

−12 0

10

20

30

40

50

0

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

18 / 33

Open-Loop Integration

Aiding sensor errors - INS errors

True Pos + errors + Aiding Pos Sensor

Filter

− + INS

+

Inertial errors est.

INS PV + errors Correct INS Output Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

20 / 33

Covariance Matrices State noise covariance matrix (continuous) E{~ w (t)~ w T (τ )} = Q(t)δ(t − τ ) State noise covariance matrix (discrete) ( Qk i = k ~ iT } = E{~ wk w 0 i 6= k Measurement noise covariance matrix ( Rk i = k T E{~vk ~vi } = 0 i= 6 k Initial error covariance matrix P0 = E{(~x0 − ~xˆ0 )(~x0 − ~xˆ0 )T } = E{~ e0~eˆ0T } Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

21 / 33

System Modeling

The position, velocity and acceleration may be modeled using the following kinematic model. p(t) ˙ = v (t)

(16)

v˙ (t) = a(t)

where a(t) is the input. Therefore, our estimate of the position is p(t) ˆ that is the double integration of the acceleration.

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

22 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as a˜(t) = a(t) + b(t) + wa (t)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

(17)

Example April 12, 2016

23 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as a˜(t) = a(t) + b(t) + wa (t)

(17)

and the bias is Markov, therefore 1 ˙ b(t) = − b(t) + wb (t) Tc

(18)

where wa (t) and wb (t) are zero mean WGN with variances, respectively, Fs · VRW 2 E{wb (t)wb (t + τ )} = Qb (t)δ(t − τ )

(19)

2 2σBI Tc and Tc is the correlation time and σBI is the bias instability.

Qb (t) =

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

(20)

Example April 12, 2016

23 / 33

Sensor Model Assuming that the accelerometer sensor measurement may be modeled as a˜(t) = a(t) + b(t) + wa (t)

(17)

and the bias is Markov, therefore 1 ˙ b(t) = − b(t) + wb (t) Tc

(18)

where wa (t) and wb (t) are zero mean WGN with variances, respectively, Fs · VRW 2 E{wb (t)wb (t + τ )} = Qb (t)δ(t − τ )

(19)

2 2σBI (20) Tc and Tc is the correlation time and σBI is the bias instability. Make sure that the VRW and σBI are converted to have SI units.

Qb (t) =

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

23 / 33

Error Mechanization Define error terms as δp(t) = p(t) − p(t), ˆ

(21)

δ p(t) ˙ = p(t) ˙ − pˆ˙ (t) = v (t) − vˆ(t)

(22)

= δv (t) and δ v˙ (t) = v˙ (t) − vˆ˙ (t) = a(t) − aˆ(t)

(23)

= −b(t) − wa (t) where b(t) is modeled as shown in Eq. 18 Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

24 / 33

State Space Formulation



      0 0 0 0 0 1 0 δp(t)                 ~x˙ (t) = δ v˙ (t) = 0 0 −1  δv (t) + 0 −1 0 wa (t)         1 ˙b(t) 0 0 − Tc wb (t) 0 0 1 b(t) δ p(t) ˙



= F (t)~x (t) + G (t)~ w (t) (24)

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

25 / 33

Covariance Matrix

The continuous state noise covariance matrix Q(t) is   0 0 0     Q(t) = 0 VRW 2 0    2 2σBI 0 0 Tc

(25)

The measurement noise covariance matrix is R = σp2 , where σp is the standard deviation of the noise of the absolute position sensor.

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

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)

Φ(k) ≈ I + Fdt

(27)

where with the measurement equation (28)

y (k) = H~x + wp (k) = δp(k) + wp (k) where H = [1 0 0]. The discrete Qd is approximated as 1 Qk−1 ≈ [Φk−1 G (tk−1 )Q(tk−1 )G T (tk−1 ))ΦT k−1 + 2 G (tk−1 )Q(tk−1 )G T (tk−1 )]dt Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

State Augmentation EE 570: Location and Navigation

(29)

Example April 12, 2016

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 200

20

Directly Computed Pos True Pos Estimated Pos (KF)

Directly Computed Vel True Vel Estimated Vel (KF)

15

150

m/s

m/s

10

5

100

0 50 −5 0

−10 0

10

20

30

40

50

0

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

28 / 33

Approach 2 — Open-Loop Compensation Position and Velocity Errors

Velocity Error

Position Error 50

2 0

0 −2 −50 m

m/s

−4 −6

−100

−8

Error in Estimated Vel (KF) Error in Computed

−10

Error in Pos Measurement Error in Computed Pos Error in Estimated Pos (KF)

−150

−200

−12 0

10

20

30

40

50

0

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

29 / 33

Approach 2 — Open-Loop Compensation Pos Error & Bias Estimate

Position Error

Bias

8

1.5

6 1

4 2 m

m

0.5 0 −2 0 −4 −6

−0.5

−8 0

10

20

30

40

50

0

10

Time (sec)

Aly El-Osery, Kevin Wedeward (NMT)

30

40

50

Time (sec)

Error in Pos Measurement Error in Estimated Pos (KF)

Kalman Filter

20

Est. Bias True Bias

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

30 / 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 Velocity

Position 200

20

Directly Computed Pos True Pos Estimated Pos (KF)

Directly Computed Vel True Vel Estimated Vel (KF)

15

150

m/s

m/s

10

5

100

0 50 −5 0

−10 0

10

20

30

40

0

50

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

31 / 33

Approach 3 — Closed-Loop Compensation Position and Velocity Errors

Velocity Error

Position Error 50

2 0

0 −2 −50 m

m/s

−4 −6

−100

−8

Error in Estimated Vel (KF) Error in Computed

−10

Error in Pos Measurement Error in Computed Pos Error in Estimated Pos (KF)

−150

−200

−12 0

10

20

30

40

50

0

Kalman Filter Aly El-Osery, Kevin Wedeward (NMT)

10

20

30

40

50

Time (sec)

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

32 / 33

Approach 3 — Closed-Loop Compensation Pos Error & Bias Estimate

Position Error

Bias

8

1.5

6 1

4 2 m

m

0.5 0 −2 0 −4 −6

−0.5

−8 0

10

20

30

40

50

0

10

Time (sec)

Aly El-Osery, Kevin Wedeward (NMT)

30

40

50

Time (sec)

Error in Pos Measurement Error in Estimated Pos (KF)

Kalman Filter

20

Est. Bias True Bias

State Augmentation EE 570: Location and Navigation

Example April 12, 2016

33 / 33