EE 570: Location and Navigation Kalman Filtering Example

Aly El-Osery1

Stephen Bruder2

1 Electrical

Engineering Department, New Mexico Tech Socorro, New Mexico, USA 2 Electrical and Computer Engineering Department, Embry-Riddle Aeronautical Univesity Prescott, Arizona, USA

April 3, 2014

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

1 / 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 the uncorrelated requirement of the system noise.

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

2 / 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) + ~v (t) ~ 1 (t) non-white, e.g., correlated Gaussian As we have seen the noise w 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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

3 / 33

Correlated State Noise

Define a new augmented state ~xaug

  ~x1 (t) = ~x2 (t)

(1)

therefore, ~x˙ aug = and

~x˙ 1 (t) ~x˙ 2 (t)

!

     ~x1 (t) 0 F1 (t) G1 H2 (t) ~ 2 (t) w + = ~x2 (t) G2 (t) 0 F2  
~x1 (t) ~y (t) = H1 0 + ~v (t) ~x2 (t)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

(3)

State Augmentation EE 570: Location and Navigation

(2)

Example April 3, 2014

4 / 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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

5 / 33

Correlated Measurement Noise

Define a new augmented state ~xaug

  ~x1 (t) = ~x2 (t)

(4)

therefore, ~x˙ aug =

~x˙ 1 (t) ~x˙ 2 (t)

!

      ~ (t) ~x1 (t) w G1 (t) 0 F1 (t) 0 (5) + = ~v2 ~x2 (t) 0 G2 (t) 0 F2

and ~y (t) = H1 H2

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

 
~x1 (t) ~x2 (t)

(6)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

6 / 33

Problem Statement

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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

7 / 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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

8 / 33

Input - Acceleration

True Acceleration and Acceleration with Noise 2 Meas Accel True Accel

1.5

m/s 2

1 0.5 0 -0.5 -1 -1.5 0

10

20

30

40

50

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

9 / 33

Aiding Position Measurement

Absolute position measurment corrupted with noise

zoomed version

70

64.4

60

64.2 64

50

63.8

m

m

40 63.6

30 63.4 20 63.2 10

63

0

62.8 22

−10

0

10

20

30

40

50

24

26

28 Time (sec)

30

32

34

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

10 / 33

Different Approaches

1 2

Clean up the noisy input to the system. Use Kalamn filtering techiques with A model of the system dyanmics (too ristrictive) A model of the error dyanmics and correct the system output in open-loop configuration, or closed-loop configuration.

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

11 / 33

Open-Loop Integration

True Pos + errors

Aiding Pos Sensor

Aided errors - INS errors + Filter

− + INS

+

Inertial erros est.

INS PV + errors Correct INS Output Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

12 / 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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

13 / 33

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 ~z k ~xˆ k|k = ~xˆ k|k−1 + Kk (~z k − 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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

14 / 33

Covariance Matrices State noise covariance matrix (continuous) ~ˆ T (τ )} = Q(t)δ(t − τ ) E{~ w (t)w 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 Error covariance matrix Pk = E{(~xk − ~xˆk )(~xk − ~xˆk )T } = E{~ek ~eˆkT } Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

15 / 33

System modeling

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

(7)

v˙ (t) = a(t) where a(t) is the input.

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

16 / 33

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

(8)

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

(9)

where both wa (t) and wb (t) are WGN. In addition we have the position measurment from which we can derive a delta position δp(t) = p(t) − pˆ(t)

(10)

where pˆ is the derived position by double integrating the measured acceleration. Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

17 / 33

Error Mechanization

Define the position and velocity error terms as ˙ ˙ δ p(t) = p(t) − pˆ˙ (t)

= v (t) − vˆ (t)

(11)

= δv (t) and

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

(12)

= −b(t) − wa (t) where b(t) is modeled as shown in Eq. 9

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

18 / 33

First Order Markov Noise

State Equation 1 ˙ b(t) = − b(t) + wb (t) Tc

(13)

Autocorrelation Function E{b(t)b(t + τ )} = σ 2 e −|τ |/Tc

(14)

E{wb (t)wb (t + τ )} = Qb (t)δ(t − τ )

(15)

where

Qb (t) =

2 2σBI Tc

(16)

and Tc is the correlation time and σBI is the bias instability. Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

19 / 33

State Space Formulation

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

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

(17)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

20 / 33

Covariance Matrics

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

(18)

make sure that the VRW and σBI are converted to have SI units.

The measurement noise covariance matrix is R = σp2

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

21 / 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

(19)

Φ(k) ≈ I + Fdt

(20)

where with the measurement equation y (k) = H~x + wp (k) = δp(k) + wp (k)

(21)

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, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

(22)

Example April 3, 2014

22 / 33

Discrete First Order Markov Noise

State Equation 1

bk = e − Tc dt bk−1 + wbd,k−1

(23)

System Covariance Matrix 2

2 Qbd = σBI [1 − e − Tc dt ]

(24)

where wbd is the discrete noise for the bias. You should use Eqs. 23 and 24 to overwrite their values in Φ and Qd since they don’t need to be approximated.

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

23 / 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 300

15 Computed Pos True Pos

250

Computed Vel True Vel 10

200 5

m

m/s

150 100

0

50 -5

0 -50 0

10

20

30

40

50

-10 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

24 / 33

Approach 1 — Filtered input Filtered Accel Measurement

2 Meas Accel Filtered Accel True Accel

1.5

m/s 2

1 0.5 0 -0.5 -1 -1.5 0

10

20

30

40

50

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

25 / 33

Approach 1 — Filtered input Position and Velocity

Velocity

Position 300

15 Computed Pos True Pos Corrected Pos

250

Computed Vel True Vel Corrected Vel

10

200 5

m

m/s

150 100

0

50 -5

0 -50 0

10

20

30

40

50

-10 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

26 / 33

Approach 1 — Filtered input Position and Velocity Errors

Velocity Error

Position Error 50

2 Error in Filtered Pos Error in Computed Pos

0

Error in Filtered Vel Error in Computed Vel

0 -2 -4

m

m/s

-50 -100 -150

-6

-200

-8

-250

-10

-300 0

10

20

30

40

50

-12 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

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 300

15 Computed Pos True Pos Corrected Pos

250

Computed Vel True Vel Corrected Vel

10

200 5

m

m/s

150 100

0

50 -5

0 -50 0

10

20

30

40

50

-10 0

Time (sec) Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec) State Augmentation EE 570: Location and Navigation

Example April 3, 2014

28 / 33

Approach 2 — Open-Loop Compensation Position and Velocity Errors

Velocity Error

Position Error 40

2 Error in Meas Pos Error in Computed Pos Error in Estimated Pos

20

-2 m/s

0 m

Error in Estimated Vel Error in Computed Vel

0

-20

-4 -6

-40 -8 -60

-10

-80 0

10

20

30

40

50

-12 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

29 / 33

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

Position Error

Bias

8

1 Error in Meas Pos Error in Estimated Pos

6

Est Bias True Bias

4

0.5 m/s 2

m

2 0

0

-2 -4

-0.5

-6 -8

-1 0

10

20

30

40

50

0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

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 300

15 Computed Pos True Pos Corrected Pos

250

Computed Vel True Vel Corrected Vel

10

200 5

m

m/s

150 100

0

50 -5

0 -50 0

10

20

30

40

50

-10 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

31 / 33

Approach 3 — Closed-Loop Compensation Position and Velocity Errors

Velocity Error

Position Error 40

2 Error in Meas Pos Error in Computed Pos Error Error in Estimated Pos

20

-2 m/s

0 m

Error in Estimated Vel Error in Computed Vel

0

-20

-4 -6

-40 -8 -60

-10

-80 0

10

20

30

40

50

-12 0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

32 / 33

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

Position Error

Bias

8

1 Error in Meas Pos Error in Estimated Pos

6

Est Bias True Bias

4

0.5 m/s 2

m

2 0

0

-2 -4

-0.5

-6 -8

-1 0

10

20

30

40

50

0

Time (sec)

Kalman Filter Aly El-Osery, Stephen Bruder (NMT,ERAU)

10

20

30

40

50

Time (sec)

State Augmentation EE 570: Location and Navigation

Example April 3, 2014

33 / 33