EE363

Winter 2005-06

Lecture 8 The Extended Kalman lter

Extended Kalman lter

Nonlinear ltering

Linearization and random variables

81

Nonlinear ltering
nonlinear Markov model: x(t + 1) = f (x(t), w(t)), y(t) = g(x(t), v(t))

f is (possibly nonlinear) dynamics function g is (possibly nonlinear) measurement or output function w(0), w(1), . . . , v(0), v(1), . . . are independent even if w, v Gaussian, x and y need not be

nonlinear ltering problem: nd, e.g., x(t|t1) = E(x(t)|y(0), . . . , y(t1)), x(t|t) = E(x(t)|y(0), . . . , y(t))

general nonlinear ltering solution involves a PDE, and is not practical

The Extended Kalman lter 82

Extended Kalman lter

extended Kalman lter (EKF) is heuristic for nonlinear ltering problem often works well (when tuned properly), but sometimes not widely used in practice based on linearizing dynamics and output functions at current estimate propagating an approximation of the conditional expectation and covariance

The Extended Kalman lter

83

Linearization and random variables

consider : Rn Rm suppose E x = x, E(x x)(x x)T = x, and y = (x) if x is small, is not too nonlinear, y y = () + D()(x x) x x x y N ((), D()xD()T ) x x gives approximation for mean and covariance of nonlinear function of random variable: y (), x x x y D()xD()T

if x is not small compared to curvature of , these estimates are poor

The Extended Kalman lter 84

 a good estimate can be found by Monte Carlo simulation: y y mc y 1 N 1 = N

N i=1 N

(x(i))
i=1 (i) mc (i) mc T

(x ) y

(x ) y

where x(1), . . . , x(N ) are samples from the distribution of x, and N is large another method: use Monte Carlo formulas, with a small number of nonrandom samples chosen as typical, e.g., the 90% condence ellipsoid semi-axis endpoints x(i) = x vi, x = V V T

The Extended Kalman lter

85

Example
x N (0, 1), y = exp(x) (for this case we can compute mean and variance of y exactly)

exact values linearization Monte Carlo (N = 10) Monte Carlo (N = 100) Sigma points (x = x, x 1.5x)

e1/2

y = 1.649 1.000 1.385 1.430 1.902

y e2 e = 2.161 1.000 1.068 1.776 2.268

The Extended Kalman lter

86

Extended Kalman lter

initialization: x(0| 1) = x0, (0| 1) = 0 measurement update linearize output function at x = x(t|t 1): C V = = g ((t|t 1), 0) x x g g ((t|t 1), 0)v ((t|t 1), 0)T x x v v

measurement update based on linearization x(t|t) = x(t|t 1) + t|t1C

T

Ct|t1C + V
1

...

t|t = t|t1 t|t1C T Ct|t1C T + V

The Extended Kalman lter

. . . (y(t) g((t|t 1), 0)) x

Ct|t1
87

 time update linearize dynamics function at x = x(t|t): A = W = f ((t|t), 0) x x f f ((t|t), 0)w ((t|t), 0)T x x w w

time update based on linearization x(t + 1|t) = f ((t|t), 0), x t+1|t = At|tAT + W

replacing linearization with Monte Carlo yields particle lter

replacing linearization with sigma-point estimates yields unscented Kalman lter (UKF)

The Extended Kalman lter

88

Example
p(t), u(t) R2 are position and velocity of vehicle, with (p(0), u(0)) N (0, I) vehicle dynamics: p(t + 1) = p(t) + 0.1u(t), w(t) are IID N (0, I) measurements: noisy measurements of distance to 9 points pi R2 yi(t) = p(t) pi + vi(t), vi(t) are IID N (0, 0.32)
The Extended Kalman lter 89

u(t + 1) =

0.85 0.15 0.1 0.85

u(t) + w(t)

i = 1, . . . , 9,

EKF results
EKF initialized with x(0| 1) = 0, (0| 1) = I, where x = (p, u) pi shown as stars; p(t) as dotted curve; p(t|t) as solid curve
9 8 7 6 5

p2 PSfrag replacements

4 3 2 1 0 1 3

p1
The Extended Kalman lter 810

Current position estimation error

p(t|t) p(t) versus t
4.5 4 3.5 3 2.5 2 1.5 1 0.5 0

PSfrag replacements

20

40

60

80

100

120

140

160

180

200

The Extended Kalman lter

811

Current position estimation predicted error

((t|t)11 + (t|t)22)
1.6

1/2

versus t

1.4

1.2

0.8

0.6

PSfrag replacements

0.4

0.2

100

200

300

400

500

600

700

800

900

1000

The Extended Kalman lter

812