The Unscented Kalman Filter for State Estimation

Transcription

The Unscented Kalman Filter for State
Estimation
Colin McManus
Autonomous Space Robotics Lab
University 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
Outline
UTIAS
Problem Statement
The Extended Kalman Filter (EKF)
Overview
Example
Summary
The Unscented Kalman Filter (UKF)
Overview
Example
UKF variants
Summary
EKF and UKF Comparison Summary
Questions
May 29th, 2010
2 / 28
Problem Statement
Defining terms
UTIAS
StereoCamera
GPS
Lidar
State:
p(xk |u1:k , y1:k ) → N x̂k , P̂k
May 29th, 2010
3 / 28
Problem Statement
Defining terms
UTIAS
StereoCamera
GPS
Lidar
State:
p(xk |u1:k , y1:k ) → N x̂k , P̂k
Motion model:
xk = h (xk−1 , uk , wk ) ,
wk ∼ N (0, Qk )
May 29th, 2010
3 / 28
Problem Statement
Defining terms
UTIAS
StereoCamera
GPS
Lidar
State:
p(xk |u1:k , y1:k ) → N x̂k , P̂k
Motion model:
xk = h (xk−1 , uk , wk ) ,
wk ∼ N (0, Qk )
Sensor model:
yk = g (xk , nk ) ,
nk ∼ N (0, Rk )
May 29th, 2010
3 / 28
Problem Statement
Goal
UTIAS
Goal at time-step k:
{x̂k−1 , P̂k−1 , uk , yk } → {x̂k , P̂k }
May 29th, 2010
4 / 28
Problem Statement
Goal
UTIAS
Goal at time-step k:
{x̂k−1 , P̂k−1 , uk , yk } → {x̂k , P̂k }
Measurement update (correction step):
x̂k = x̂−
k + Kk (yk − ŷk )
T
P̂k = P̂−
k − Kk Uk
I
x̂−
k : Predicted state
I
P̂−
k : Predicted covariance
I
Kk : Kalman gain
I
ŷk : Predicted measurement
I
Uk : Cross-covariance term
May 29th, 2010
4 / 28
Overview
Extended Kalman Filter
UTIAS
I
Nonlinear extension to the famous ‘Kalman Filter’
I
EKF uses a first-order Taylor series expansion of the motion and
observation models with respect to the current state estimate
May 29th, 2010
5 / 28
Overview
Extended Kalman Filter
UTIAS
I
Nonlinear extension to the famous ‘Kalman Filter’
I
EKF uses a first-order Taylor series expansion of the motion and
observation models with respect to the current state estimate
I
Prediction step:
x̂−
= h(x̂k−1 , uk , 0)
k
P̂−
= Hx,k P̂k−1 HTx,k + Hw,k Qk HTw,k
k
where
Hx,k :=
∂h(xk−1 , uk , wk ) ,
∂xk−1
x̂k−1 ,uk ,0
Hw,k :=
∂h(xk−1 , uk , wk ) .
∂wk
x̂k−1 ,uk ,0
May 29th, 2010
5 / 28
Example
Example
I
UTIAS
Consider a simple example with a quadratic nonlinearity,
h (xk−1 , uk , wk ) = x2k−1 + wk
May 29th, 2010
6 / 28
Example
Example
UTIAS
(Loading)
May 29th, 2010
7 / 28
Summary
Strengths and weaknesses
UTIAS
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; Wan
and van der Merwe, 2000)
I
When analytical Jacobians are not available, numerical Jacobians
are required
May 29th, 2010
8 / 28
Overview
Unscented Kalman Filter
UTIAS
I
Introduced by Julier et al. (1995) as a derivative-free alternative to
the EKF
I
UKF uses a weighted set of deterministically sampled points
called sigma-points, which are passed through the nonlinearity
and are used to approximate the statistics of the distribution
I
Unscented Transformation is accurate to at least third-order for
Gaussian systems
May 29th, 2010
9 / 28
Overview
Unscented Kalman Filter
UTIAS
Image taken from van der Merwe and Wan (2001).
May 29th, 2010
10 / 28
Overview
The Unscented Transformation
UTIAS
A set
1 sigma-points is computed from the prior density,
of 2N + N x̂k−1 , P̂k−1 , according to
SST
:= P̂k−1
X0 := x̂k−1
Xi := x̂k−1 +
Xi+N
:= x̂k−1 −
(Cholesky decomposition)
√
√
N + κ coli S
N + κ coli S
i = 1...N
where N = dim(x̂k−1 ).
Xi− = h (Xi ) ,
for i = 0 . . . 2N
May 29th, 2010
11 / 28
Overview
UTIAS
The mean and covariance are computed as follows:
!
2N
1X −
1
−
−
κX0 +
Xi ,
x̂k =
N +κ
2
i=1
P̂−
=
k
1
N +κ
κ X0− − x̂
2N
T 1 X
T
X0− − x̂ +
Xi− − x̂ Xi− − x̂
2
!
.
i=1
May 29th, 2010
12 / 28
Overview
UTIAS
!
2N
1X −
1
−
−
κX0 +
Xi ,
x̂k =
N +κ
2
i=1
P̂−
=
k
I
1
N +κ
κ X0− − x̂
2N
T 1 X
T
X0− − x̂ +
Xi− − x̂ Xi− − x̂
2
!
.
i=1
Why this deterministic sampling scheme?
May 29th, 2010
12 / 28
Overview
UTIAS
!
2N
1X −
1
−
−
κX0 +
Xi ,
x̂k =
N +κ
2
i=1
P̂−
=
k
1
N +κ
κ X0− − x̂
2N
T 1 X
T
X0− − x̂ +
Xi− − x̂ Xi− − x̂
2
.
i=1
I
Why this deterministic sampling scheme?
I
Consider the expectation of the prior mean plus a disturbance:
x̂−
k = E [h (x̂k−1 + ∆x)] ,
!
∆x ∼ N 0, P̂k−1
May 29th, 2010
12 / 28
Overview
Analytical Linearization
UTIAS
x̂−
= E [h (x̂k−1 + ∆x)]
k
D2 h D3 h D4 h
= h (x̂k−1 ) + E D∆x h + ∆x + ∆x + ∆x + . . .
2!
3!
4!
where,
Dn∆x h
1
=
n!
n!
N
X
i=1
∂
∆xn
∂xn
!n
h (x)
x=x̂k−1
May 29th, 2010
13 / 28
Overview
UTIAS
By symmetry, odd-moments are zero,
2
D∆x h D4∆x h
−
+
+ ...
x̂k = h (x̂k−1 ) + E
2!
4!
May 29th, 2010
14 / 28
Overview
UTIAS
2
D∆x h D4∆x h
−
+
+ ...
x̂k = h (x̂k−1 ) + E
2!
4!
The second-order term is given by,
D2∆x h
E
2!
=
! ∇T E ∆x∆xT ∇
h
2!
x=x̂k−1
=
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
May 29th, 2010
14 / 28
Overview
UTIAS
2
D∆x h D4∆x h
−
+
+ ...
x̂k = h (x̂k−1 ) + E
2!
4!
The second-order term is given by,
D2∆x h
E
2!
=
! ∇T E ∆x∆xT ∇
h
2!
=
x=x̂k−1
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
Resulting in the following
x̂−
k
= h (x̂k−1 ) +
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
D4∆x h
+E
+ ...
4!
May 29th, 2010
14 / 28
Overview
Statistical Linearization
UTIAS
We define some samples according to
Xi := x̂k−1 + σi
Xi−
=
h (Xi )
i = 0 . . . 2N
where N = dim (x̂k−1 ). Now we calculate the weighted sigma-point
expectations and compare with the ‘true’ mean
May 29th, 2010
15 / 28
Overview
UTIAS
2N
X
D2σi h D3σi h D4σi h
1
D
h
+
+
+
+ ...
x̂−
=
h
(x̂
)
+
σ
k−1
i
k
2(N + κ)
2!
3!
4!
!
i=1
May 29th, 2010
16 / 28
Overview
UTIAS
2N
X
1
D
h
+
+
+
+ ...
x̂−
=
h
(x̂
)
+
σ
k−1
i
k
2(N + κ)
2!
3!
4!
!
i=1
The second-order term is given by
T
D2σi h
∇ σi σiT ∇
=
h
2!
2!
x=x̂k−1
May 29th, 2010
16 / 28
Overview
UTIAS
2N
X
1
D
h
+
+
+
+ ...
x̂−
=
h
(x̂
)
+
σ
k−1
i
k
2(N + κ)
2!
3!
4!
!
i=1
The second-order term is given by
T
D2σi h
∇ σi σiT ∇
=
h
2!
2!
x=x̂k−1
Recall
2 D∆x h
E
=
2!
! ∇T E ∆x∆xT ∇
h
2!
x=x̂k−1
=
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
May 29th, 2010
16 / 28
Overview
UTIAS
√
We let σi := ± N + κcoli S, where SST = P̂k−1 , which gives
x̂−
k = h (x̂k−1 ) +
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
2N
X
1
+
2(N + κ)
i=1
D4σi h
+ ...
4!
May 29th, 2010
!
17 / 28
Overview
UTIAS
√
We let σi := ± N + κcoli S, where SST = P̂k−1 , which gives
x̂−
k = h (x̂k−1 ) +
∇T P̂k−1 ∇
2!
! h
x=x̂k−1
2N
X
1
+
2(N + κ)
i=1
Compare above with analytical linearization
! T P̂
∇
∇
k−1
x̂−
=
h
(x̂
)
+
h
k−1
k
2!
x=x̂k−1
D4σi h
+ ...
4!
D4∆x h
+E
+ ...
4!
!
May 29th, 2010
17 / 28
Example
Example
UTIAS
(Loading)
May 29th, 2010
18 / 28
UKF variants
Augmented UKF
UTIAS
xk = h (xk−1 , uk , wk ) ,
yk = g (xk , nk ) ,
wk ∼ N (0, Qk )
nk ∼ N (0, Rk )
May 29th, 2010
19 / 28
UKF variants
Augmented UKF
UTIAS
xk = h (xk−1 , uk , wk ) ,
yk = g (xk , nk ) ,
I
wk ∼ N (0, Qk )
nk ∼ N (0, Rk )
Completely general case, both the prior belief, the process noise
and measurement noise have uncertainty so these are stacked
together in the following way:


x̂k−1
z :=  0  ,
0


P̂k−1 0
0
Y :=  0
Qk 0 
0
0 Rk
May 29th, 2010
19 / 28
UKF variants
Augmented UKF
UTIAS
xk = h (xk−1 , uk , wk ) ,
yk = g (xk , nk ) ,
I
I
nk ∼ N (0, Rk )
Completely general case, both the prior belief, the process noise
and measurement noise have uncertainty so these are stacked
together in the following way:


x̂k−1
z :=  0  ,
0
I
wk ∼ N (0, Qk )


P̂k−1 0
0
Y :=  0
Qk 0 
0
0 Rk
Let N := dim (x̂), P := dim (Qk ), M := dim (Rk )
Number of sigma-points: 2(N + P + M ) + 1
May 29th, 2010
19 / 28
UKF variants
Additive noise
I
UTIAS
If our motion and observation model are given by
xk = h (xk−1 , uk ) + wk ,
yk = g (xk ) + nk ,
I
I
nk ∼ N (0, Rk )
Only 2N + 1 sigma-points are required!
z := x̂k−1 ,
I
wk ∼ N (0, Qk )
Y := P̂k−1
−
Must re-draw sigma-points from {x̂−
k , P̂k } for the correction step
(lose odd-moment information)
Alternatively, we can use 2(N + P ) + 1 sigma-points
√ for the
correction step and avoid re-drawing (incorporate Qk into
sigma-points)
May 29th, 2010
20 / 28
UKF variants
Additive noise
I
I
UTIAS
If our motion and observation model are given by
xk = h (xk−1 , uk , wk ) ,
wk ∼ N (0, Qk )
yk = g (xk ) + nk ,
nk ∼ N (0, Rk )
Only 2(N + P ) + 1 sigma-points are required
x̂k−1
,
z :=
0
P̂
0
Y := k−1
0
Qk
May 29th, 2010
21 / 28
UKF variants
Other efficiency improvements
UTIAS
May 29th, 2010
22 / 28
UKF variants
I
UTIAS
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)
May 29th, 2010
22 / 28
UKF variants
I
I
I
UTIAS
Reduced sigma-point UKF (Quine, 2006)
I
Used a minimal set of N + 1 sigma-points (accurate to
second-order)
May 29th, 2010
22 / 28
UKF variants
I
I
I
I
I
UTIAS
Used a minimal set of N + 1 sigma-points (accurate to
second-order)
Square-root Unscented Kalman Filter (van der Merwe and Wan,
2001)
I
I
Efficient square-root form that avoids refactorizing the state
covariance at the prediction step and reduces computational cost
required to compute the Kalman gain
Present additional benefits in terms of numerical stability and
ensures that the state covariance is always positive-definite
May 29th, 2010
22 / 28
Summary
UTIAS
Strengths:
I
Unscented Transformation is accurate to third-order for Gaussian
systems
I
Derivative-free (easy implementation)
Weaknesses:
I
I
Depending on noise assumptions, can be expensive
Sigma-point scaling issues
May 29th, 2010
23 / 28
Summary
UTIAS
Strengths:
I
systems
I
Weaknesses:
I
I
I
The Scaled Unscented Kalman Filter (Julier, 2002)
May 29th, 2010
23 / 28
Summary
UTIAS
Strengths:
I
systems
I
Weaknesses:
I
I
I
I
The Scaled Unscented Kalman Filter (Julier, 2002)
Additional weight parameters (Wan and van der Merwe, 2000)
May 29th, 2010
23 / 28
Accuracy
UTIAS
0.05
True Posterior
First−order Linearization
Unscented Transformation
0.045
0.04
0.035
P(x)
0.03
0.025
0.02
0.015
0.01
0.005
0
−20
−15
−10
−5
0
5
10
15
20
25
30
x
May 29th, 2010
24 / 28
Accuracy
UTIAS
0.05
True Posterior
First−order Linearization
Unscented Transformation
0.045
0.04
0.035
P(x)
0.03
0.025
0.02
0.015
0.01
0.005
0
−20
−15
−10
−5
0
5
10
15
20
25
30
x
I
Tong, C. and Barfoot, T.D. “A Comparison of the EKF, SPKF, and the
Bayes Filter for Landmark-Based Localization,” In Proceedings of the 7th
Canadian Conference on Computer and Robot Vision (CRV), to appear.
Ottawa, Canada, 31 May - 2 June 2010.
May 29th, 2010
24 / 28
Computational cost
I
UTIAS
In general, EKF cost < UKF cost
May 29th, 2010
25 / 28
Computational cost
I
I
Problem dependent
UKF form:
I
I
I
UTIAS
Additive UKF
Augmented UKF
May 29th, 2010
25 / 28
Computational cost
I
I
Problem dependent
UKF form:
I
I
I
I
UTIAS
Additive UKF
Augmented UKF
Sigma point reduction:
I
I
May 29th, 2010
25 / 28
Questions
Any Questions?
UTIAS
May 29th, 2010
26 / 28
Questions
References
UTIAS
I
Julier, S., Uhlmann, J. and Durrant-Whyte, H. “A New Approach for
Filtering Nonlinear Systems,” Proceedings of the American Control
Conference, 1995, 3, 1628-1632.
I
Julier, S. “The Scaled Unscented Transform,” Proceedings of the
American 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 Kalman
filter,” Automatica, 2006, 42, 19271934.
May 29th, 2010
27 / 28
Questions
References
UTIAS
I
Tong, C. and Barfoot, T.D. “A Comparison of the EKF, SPKF, and the
Bayes Filter for Landmark-Based Localization,” In Proceedings of the 7th
Canadian 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 Kalman
Filter for State and Parameter-Estimation,” IEEE International
Conference on Acoustics, Speech, and Signal Processing, 2001, 6,
3461-3464.
I
Wan, E. and van der Merwe, R. “The Unscented Kalman Filter for
Nonlinear Estimation,” Adaptive Systems for Signal Processing,
Communications, and Control Symposium, 2000, 153-158.
May 29th, 2010
28 / 28

The Unscented Kalman Filter for State Estimation

Transcription

Similar documents

Sigma Points

The Poddington Peas copyright Paul Needs and Colin Wyatt. With

origins - Eric Bryant

academic term and vacation schedule for 2015

“kings” and “queens” party

2016 Wellcraft 220 Fisherman - $ 55,900

Instructions for Applicants, 2015 - UKF-a

Railex-The Brit School

Management Services Business Operations Acquisition Support

Hydro-Filter™ - Phoenix Truckmounts