the unscented kalman filter for state...

The Unscented Kalman Filter for StateEstimation

Colin McManus

Autonomous Space Robotics LabUniversity of Toronto Institute for Aerospace Studies

UTIAS

Presented at the Simultaneous Localization and Mapping (SLAM) Workshop

May 29th, 2010

Colin McManus (UTIAS) The UKF for State Estimation May 29th, 2010 1 / 28

UTIASOutline

Problem Statement

The Extended Kalman Filter (EKF)OverviewExampleSummary

The Unscented Kalman Filter (UKF)OverviewExampleUKF variantsSummary

EKF and UKF Comparison Summary

Questions


UTIAS

Problem Statement

Defining termsStereoCamera GPS

Lidar

State:p(xk|u1:k, y1:k) N

(xk, Pk

)

Motion model:

xk = h (xk1,uk,wk) , wk N (0,Qk)Sensor model:

yk = g (xk,nk) , nk N (0,Rk)


UTIAS

Problem Statement


Lidar


(xk, Pk

)Motion model:

xk = h (xk1,uk,wk) , wk N (0,Qk)

Sensor model:

yk = g (xk,nk) , nk N (0,Rk)


UTIAS

Problem Statement


Lidar


(xk, Pk

)Motion model:

xk = h (xk1,uk,wk) , wk N (0,Qk)Sensor model:

yk = g (xk,nk) , nk N (0,Rk)Colin McManus (UTIAS) The UKF for State Estimation May 29th, 2010 3 / 28

UTIAS

Problem Statement

GoalGoal at time-step k:

{xk1, Pk1,uk, yk} {xk, Pk}

Measurement update (correction step):

xk = xk + Kk (yk yk)Pk = Pk KkU

Tk

I xk : Predicted stateI Pk : Predicted covarianceI Kk : Kalman gainI yk : Predicted measurementI Uk : Cross-covariance term


UTIAS

The Extended Kalman Filter (EKF) Overview

Extended Kalman Filter

I Nonlinear extension to the famous Kalman FilterI EKF uses a first-order Taylor series expansion of the motion and

observation models with respect to the current state estimate

I Prediction step:

xk = h(xk1,uk, 0)Pk = Hx,kPk1H

Tx,k + Hw,kQkH

Tw,k

where

Hx,k :=h(xk1,uk,wk)

xk1

xk1,uk,0

, Hw,k :=h(xk1,uk,wk)

wk

xk1,uk,0

.


UTIAS

The Extended Kalman Filter (EKF) Overview

Extended Kalman Filter

I Nonlinear extension to the famous Kalman FilterI EKF uses a first-order Taylor series expansion of the motion and

observation models with respect to the current state estimateI Prediction step:

xk = h(xk1,uk, 0)Pk = Hx,kPk1H

Tx,k + Hw,kQkH

Tw,k

where

Hx,k :=h(xk1,uk,wk)

xk1

xk1,uk,0

, Hw,k :=h(xk1,uk,wk)

wk

xk1,uk,0

.


UTIAS

The Extended Kalman Filter (EKF) Example

Example

I Consider a simple example with a quadratic nonlinearity,

h (xk1, uk, wk) = x2k1 + wk


UTIAS

The Extended Kalman Filter (EKF) Example

Example

(Loading)


ekf3.wmvMedia File (video/x-ms-wmv)

UTIAS

The Extended Kalman Filter (EKF) Summary

Strengths and weaknesses

Strengths:I Computationally inexpensive

Weaknesses:I For highly nonlinear problems, the EKF is known to encounter

issues with both accuracy and stability (Julier et al. ,1995; Wanand van der Merwe, 2000)

I When analytical Jacobians are not available, numerical Jacobiansare required


UTIAS

The Unscented Kalman Filter (UKF) Overview

Unscented Kalman Filter

I Introduced by Julier et al. (1995) as a derivative-free alternative tothe EKF

I UKF uses a weighted set of deterministically sampled pointscalled sigma-points, which are passed through the nonlinearityand are used to approximate the statistics of the distribution

I Unscented Transformation is accurate to at least third-order forGaussian systems


UTIAS


Unscented Kalman Filter

Image taken from van der Merwe and Wan (2001).


UTIAS


The Unscented Transformation

A set of 2N + 1 sigma-points is computed from the prior density,N(

xk1, Pk1)

, according to

SST := Pk1 (Cholesky decomposition)X0 := xk1Xi := xk1 +

N + coliS

i = 1 . . . NXi+N := xk1 N + coliS

where N = dim(xk1).

Xi = h (Xi) , for i = 0 . . . 2N


UTIAS



The mean and covariance are computed as follows:

xk =1

N +

(X0 +

1

2

2Ni=1

Xi

),

Pk =1

N +

((X0 x

) (X0 x

)T+

1

2

2Ni=1

(Xi x

) (Xi x

)T).

I Why this deterministic sampling scheme?I Consider the expectation of the prior mean plus a disturbance:

xk = E [h (xk1 + x)] , x N(

0, Pk1)


UTIAS




xk =1

N +

(X0 +

1

2

2Ni=1

Xi

),

Pk =1

N +

((X0 x

) (X0 x

)T+

1

2

2Ni=1

(Xi x

) (Xi x

)T).

I Why this deterministic sampling scheme?

I Consider the expectation of the prior mean plus a disturbance:

xk = E [h (xk1 + x)] , x N(

0, Pk1)


UTIAS




xk =1

N +

(X0 +

1

2

2Ni=1

Xi

),

Pk =1

N +

((X0 x

) (X0 x

)T+

1

2

2Ni=1

(Xi x

) (Xi x

)T).

I Why this deterministic sampling scheme?I Consider the expectation of the prior mean plus a disturbance:

xk = E [h (xk1 + x)] , x N(

0, Pk1)


UTIAS


Analytical Linearization

xk = E [h (xk1 + x)]

= h (xk1) + E[

Dxh +D2xh

2!+

D3xh3!

+D4xh

4!+ . . .

]where,

Dnxhn!

=1

n!

(Ni=1

xn

xn

)nh (x)

x=xk1


UTIAS


Analytical LinearizationBy symmetry, odd-moments are zero,

xk = h (xk1) + E[

D2xh2!

+D4xh

4!+ . . .

]

The second-order term is given by,

E[

D2xh2!

]=

(TE

[xxT

]

2!

)h

x=xk1

=

(T Pk1

2!

)h

x=xk1

Resulting in the following

xk = h (xk1) +

(T Pk1

2!

)h

x=xk1

+ E[

D4xh4!

+ . . .

]


UTIAS


Analytical LinearizationBy symmetry, odd-moments are zero,

xk = h (xk1) + E[

D2xh2!

+D4xh

4!+ . . .

]The second-order term is given by,

E[

D2xh2!

]=

(TE

[xxT

]

2!

)h

x=xk1

=

(T Pk1

2!

)h

x=xk1

Resulting in the following

xk = h (xk1) +

(T Pk1

2!

)h

x=xk1

+ E[

D4xh4!

+ . . .

]


UTIAS


Statistical Linearization

We define some samples according to

Xi := xk1 + ii = 0 . . . 2NXi = h (Xi)

where N = dim (xk1). Now we calculate the weighted sigma-pointexpectations and compare with the true mean


UTIAS



xk = h (xk1) +1

2(N + )

2Ni=1

(Dih +

D2ih2!

+D3ih

3!+

D4ih4!

+ . . .

)

The second-order term is given by

D2ih2!

=

(TiTi

2!

)hx=xk1

Recall

E[

D2xh2!

]=

(TE

[xxT

]

2!

)h

x=xk1

=

(T Pk1

2!

)h

x=xk1


UTIAS



We let i := N + coliS, where SST = Pk1, which gives

xk = h (xk1) +

(T Pk1

2!

)h

x=xk1

+1

2(N + )

2Ni=1

(D4ih

4!+ . . .

)

Compare above with analytical linearization

xk = h (xk1) +

(T Pk1

2!

)h

x=xk1

+ E[

D4xh4!

+ . . .

]


UTIAS

The Unscented Kalman Filter (UKF) Example

Example

(Loading)


ukf3.wmvMedia File (video/x-ms-wmv)

UTIAS

The Unscented Kalman Filter (UKF) UKF variants

Augmented UKF

xk = h (xk1,uk,wk) , wk N (0,Qk)yk = g (xk,nk) , nk N (0,Rk)

I Completely general case, both the prior belief, the process noiseand measurement noise have uncertainty so these are stackedtogether in the following way:

z :=

xk100

, Y :=Pk1 0 00 Qk 0

0 0 Rk

I Let N := dim (x), P := dim (Qk), M := dim (Rk)I Number of sigma-points: 2(N + P +M) + 1


UTIAS


Additive noiseI If our motion and observation model are given by

xk = h (xk1,uk) + wk, wk N (0,Qk)yk = g (xk) + nk, nk N (0,Rk)

I Only 2N + 1 sigma-points are required!

z := xk1, Y := Pk1

I Must re-draw sigma-points from {xk , Pk } for the correction step

(lose odd-moment information)I Alternatively, we can use 2(N + P ) + 1 sigma-points for the

correction step and avoid re-drawing (incorporate

Qk intosigma-points)


UTIAS


Additive noise

I If our motion and observation model are given by

xk = h (xk1,uk,wk) , wk N (0,Qk)yk = g (xk) + nk, nk N (0,Rk)

I Only 2(N + P ) + 1 sigma-points are required

z :=[

xk10

], Y :=

[Pk1 0

0 Qk

]


UTIAS


Other efficiency improvements

I Spherical-simplex sigma-points (Julier et al., 2003)I Used a set of N + 2 sigma-points, which were chosen to minimize

the third-order moments (accurate to second-order)I Reduced sigma-point UKF (Quine, 2006)

I Used a minimal set of N + 1 sigma-points (accurate tosecond-order)

I Square-root Unscented Kalman Filter (van der Merwe and Wan,2001)

I Efficient square-root form that avoids refactorizing the statecovariance at the prediction step and reduces computational costrequired to compute the Kalman gain

I Present additional benefits in terms of numerical stability andensures that the state covariance is always positive-definite


UTIAS




the third-order moments (accurate to second-order)

I Reduced sigma-point UKF (Quine, 2006)I Used a minimal set of N + 1 sigma-points (accurate to

second-order)I Square-root Unscented Kalman Filter (van der Merwe and Wan,

2001)I Efficient square-root form that avoids refactorizing the state

covariance at the prediction step and reduces computational costrequired to compute the Kalman gain



UTIAS




the third-order moments (accurate to second-order)I Reduced sigma-point UKF (Quine, 2006)

I Used a minimal set of N + 1 sigma-points (accurate tosecond-order)

I Square-root Unscented Kalman Filter (van der Merwe and Wan,2001)

I Efficient square-root form that avoids refactorizing the statecovariance at the prediction step and reduces computational costrequired to compute the Kalman gain



UTIAS

The Unscented Kalman Filter (UKF) Summary


Strengths:I Unscented Transformation is accurate to third-order for Gaussian

systemsI Derivative-free (easy implementation)

Weaknesses:I Depending on noise assumptions, can be expensiveI Sigma-point scaling issues

I The Scaled Unscented Kalman Filter (Julier, 2002)I Additional weight parameters (Wan and van der Merwe, 2000)


UTIAS






I The Scaled Unscented Kalman Filter (Julier, 2002)

I Additional weight parameters (Wan and van der Merwe, 2000)


UTIAS






I The Scaled Unscented Kalman Filter (Julier, 2002)I Additional weight parameters (Wan and van der Merwe, 2000)


UTIAS


Accuracy

20 15 10 5 0 5 10 15 20 25 300

0.005

0.01

0.015

0.02

0.025

0.03

0.035

0.04

0.045

0.05

x

P(x

)

True PosteriorFirstorder LinearizationUnscented Transformation

I Tong, C. and Barfoot, T.D. A Comparison of the EKF, SPKF, and theBayes Filter for Landmark-Based Localization, In Proceedings of the 7thCanadian Conference on Computer and Robot Vision (CRV), to appear.Ottawa, Canada, 31 May - 2 June 2010.


UTIAS


Computational cost

I In general, EKF cost < UKF cost

I Problem dependentI UKF form:

I Additive UKFI Augmented UKF

I Sigma point reduction:I Spherical-simplex sigma-points (Julier et al., 2003)I Reduced sigma-point UKF (Quine, 2006)


UTIAS


Computational cost

I In general, EKF cost < UKF costI Problem dependentI UKF form:

I Additive UKFI Augmented UKF

I Sigma point reduction:I Spherical-simplex sigma-points (Julier et al., 2003)I Reduced sigma-point UKF (Quine, 2006)


UTIAS

Questions

Any Questions?


UTIAS

Questions

References

I Julier, S., Uhlmann, J. and Durrant-Whyte, H. A New Approach forFiltering Nonlinear Systems, Proceedings of the American ControlConference, 1995, 3, 1628-1632.

I Julier, S. The Scaled Unscented Transform, Proceedings of theAmerican Control Conference, 2002, 6, 4555 - 4559.

I Julier, S. The Spherical Simplex Unscented Transformation,Proceedings of the American Control Conference, 2003.

I Quine, B. A derivative-free implementation of the extended Kalmanfilter, Automatica, 2006, 42, 19271934.


UTIAS

Questions

References

I Tong, C. and Barfoot, T.D. A Comparison of the EKF, SPKF, and theBayes Filter for Landmark-Based Localization, In Proceedings of the 7thCanadian Conference on Computer and Robot Vision (CRV), to appear.Ottawa, Canada, 31 May - 2 June 2010.

I van der Merwe, R. and Wan, E. The Square-Root Unscented KalmanFilter for State and Parameter-Estimation, IEEE InternationalConference on Acoustics, Speech, and Signal Processing, 2001, 6,3461-3464.

I Wan, E. and van der Merwe, R. The Unscented Kalman Filter forNonlinear Estimation, Adaptive Systems for Signal Processing,Communications, and Control Symposium, 2000, 153-158.


Problem StatementThe Extended Kalman Filter (EKF)OverviewExampleSummary

The Unscented Kalman Filter (UKF)OverviewExampleUKF variantsSummary

EKF and UKF Comparison SummaryQuestions

the unscented kalman filter for state...

Documents