http://bbs.elecfans.com/ http://www.elecfans.

com

Greg Welch1and Gary Bishop2

TR 95-041
Department of Computer Science
University of North Carolina at Chapel Hill3
Chapel Hill, NC 27599-3175

: 2006724
200718

1960

welch@cs.unc.edu, http://www.cs.unc.edu/welch
gb@cs.unc.edu, http://www.cs.unc.edu/gb
3

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

1960
[Kalman60]

[Maybeck79]
[Sorenson70]
[Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93]

x <n

xk = Axk1 + Buk1 + wk1 ,

(1.1)

z <m
zk = Hxk + vk .

(1.2)

wk vk 1

p(w) N (0, Q),

(1.3)

p(v) N (0, R).

(1.4)

Q R

uk1 wk1 1.1 n n

A k 1 k
A n l B
u <l 1.2 m n H xk
zk H

n
k
x

k <
k x
k <n zk k

k xk x
k,
ek xk x
k
1

process noise

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

Pk = E[e
k ek ],

(1.5)

Pk = E[ek ek T ],

(1.6)

1.7 x

zk H x
k x
k 1.7

x
k = x

k + K(zk H x
k)

(1.7)

1.7 (zk H x

k )

1.7 n m K 1.6
K 1.7
ek ek 1.61.6 Pk
K K [Maybeck79,
Brown92, Jacobs93] K
Kk = Pk H T (HPk H T + R)1
=

Pk H T
.
HPk H T + R

(1.8)

1.8 R K
R
lim Kk = H 1 .

Rk 0

Pk K
Pk
lim Kk = 0.

Pk 0

K R zk
zk H x

Pk zk zk
Hx

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

1.7 x
k

zk xk x
k

E[xk ] = x
k
E[(xk x
k )(xk x
k )T ] = Pk .

1.7
1.31.41.6
zk xk

p(xk |zk ) N (E[xk ], E[(xk x

k )(xk x
k )T ])
= N (
xk , Pk ).
[Maybeck79,
Brown92, Jacobs93]

1-1

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

1-1:

1-11-2
1-1:
x

xk1 + Buk1
k = A
Pk

= APk1 A + Q

(1.9)
(1.10)

1-1 x
k

P k k 1 k A B 1.1 Q 1.3

1-2:
Kk = Pk H T (HPk H T + R)1
x
k =

Pk = (I

Kk (zk H x

k)

Kk H)Pk

(1.11)
(1.12)
(1.13)

Kk 1.111.8
zk 1.121.7
1.13

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

[Brown92]
1-21-11-2

1-2: 1-11-11-2

R
R

Q
xk Q

1.121.131.91.10

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

Q R
R Kk 1-2

[Grewal93] Pk
R

Q
Q Qk

Qk
Qk

x <n

Extended Kalman FilterEKF

x <n

xk = f (xk1 , uk1 , wk1 ),

(2.1)

z <m
zk = h(xk , vk ),

(2.2)

wk vk 2.1
f k 1 k
2.2 uk wk h
xk zk
wk vk

x
k = f (
xk1 , uk1 , 0)

(2.3)

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

zk = h(
xk , 0),

(2.4)

x
k k

[Julier96]Julier et
al.

2.3
2.4
xk x
k + A(xk1 x
k1 ) + W wk1 ,

(2.5)

zk zk + H(xk x
k )V vk .

(2.6)

xk zk
x
k zk 2.32.4
x
k k
1.31.4 wk vk

A f x
A[i,j] =

f[i]
(
xk1 , uk1 , 0),
x[j]

W f w
W[i,j] =

f[i]
(
xk1 , uk1 , 0),
w[j]

H h x
H[i,j] =

h[i]
(
xk , 0),
x[j]

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

V h v
V[i,j] =

h[i]
(
xk , 0).
v[j]

A, W, H, V k

exk xk x
k ,

(2.7)

ezk zk zk ,

(2.8)

2.7 xk ,
2.8 zk
xk 2.72.8

exk A(xk1 x
k1 ) + k ,

(2.9)

ezk H exk + k ,

(2.10)

k k W QW T V RV T
Q R 1.31.4
2.92.10
1.11.22.8 ezk
2.9 exk
ek 2.7
x
k = x
k + ek .

(2.11)

2.92.10
p(
exk ) N (0, E[
exk eTxk ])
p(k ) N (0, W Qk W T )
p(k ) N (0, V Rk V T )
ek ek

ek = Kk ezk .

(2.12)

2.82.122.11

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

10

x
k = x
k + Kk ezk
=x
k + Kk (zk zk )

(2.13)

2.13 x
k
zk 2.32.41.11
Kk
2-12-2 x

k
x
k A, W, H, V

2-1:
x

xk1 , uk1 , 0)
k = f (

Pk = Ak Pk1 ATk + Wk Qk1 WkT

(2.14)
(2.15)

2-1
k 1 k 2.14 f 2.3 Ak
Wk k Qk 1.3 k

2-2:
Kk = Pk HkT (Hk Pk HkT + Vk Rk VkT )1
x

x
k =
+ Kk (zk h(
x
k , 0))
Pk = (I Kk Hk )Pk

(2.16)
(2.17)
(2.18)

2-2
zk 2.17 h 2.4 Hk V
k Rk 1.4 k
k Rk
1-1
2-11-12-12-2

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

11

2-1: 1-12-12-2
Kk
Hk
h zk Hk
zk h(
x
k , 0)
zk h

Andrew Straw http://www.scipy.org/Cookbook/

KalmanFiltering Python/SciPy

Root-

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

12

Mean-SquareRMS 0.1

xk = Axk1 + Buk1 + wk
= xk1 + wk

z <1
zk = Hxk + vk
= xk + vk

A = 1 u = 0
H = 1
k

k1 ,
k =x
Pk = Pk1 + Q.

Kk = Pk (Pk + R)1
=

Pk
Pk + R

(3.1)

x
k = x

k + Kk (zk x
k ),
Pk = (I Kk )Pk .
Q Q = 105 Q = 0

x
k1 = 0
Pk1 P0 x
0 = 0
P0 = 0 x
0 P0 = 0
x
k = 0 P0
P0 6= 0 P0 = 1
UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

13

x = 0.37727 x
50 zk 0
0.10.1

R = (0.1)2 = 0.01

3-1
x = 0.37727

0.0
noisy measurements
a posteri estimate
truth value

-0.1

Voltage

-0.2

-0.3

-0.4

-0.5

-0.6

-0.7
0

10

20

30

40

50

Iteration

3-1: R = (0.1)2 = 0.01

x = 0.37727

P0 P0 6= 0
3-2 Pk
5010.0002

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

14

0.010

0.008

(Voltage)

0.006

0.004

0.002

0.000

10

20

30

40

50

Iteration

3-2: 50 Pk 10.0002

Q
R 3-33-4 R 100
3-3100 R = 1

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

15

0.0
noisy measurements
a posteri estimate
truth value

-0.1

Voltage

-0.2

-0.3

-0.4

-0.5

-0.6

-0.7
0

10

20

30

40

50

Iteration

3-3: R = 1

3-4100 R = 0.0001

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com
Welch & Bishop,

16

0.0
noisy measurements
a posteri estimate
truth value

-0.1

Voltage

-0.2

-0.3

-0.4

-0.5

-0.6

-0.7
0

10

20

30

40

50

Iteration

3-4: R = 0.0001

3-3

UNC-Chapel Hill, TR 95-041, July 24, 2006

http://bbs.elecfans.com/ http://www.elecfans.com

[Brown92]

Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random Signals and Applied Kalman Filtering, Second Edition,
John Wiley & Sons, Inc.

[Gelb74]

Gelb, A. 1974. Applied Optimal Estimation, MIT Press, Cambridge, MA.

[Grewal93] Grewal, Mohinder S., and Angus P. Andrews (1993). Kalman

Filtering Theory and Practice. Upper Saddle River, NJ USA,
Prentice Hall.
[Jacobs93]

Jacobs, O. L. R. 1993. Introduction to Control Theory, 2nd

Edition. Oxford University Press.

[Julier96]

Julier, Simon and Jerey Uhlman. A General Method of

Approximating Nonlinear Transformations of Probability
Distributions,Robotics Research Group, Department of
Engineering Science, University of Oxford [cited 14 November
1995]. Available from http://www.robots.ox.ac.uk/siju/
work/publications/Unscented.zip.
Also see: A New Approach for Filtering Nonlinear
Systemsby S. J. Julier, J. K. Uhlmann, and H. F.
Durrant-Whyte, Proceedings of the 1995 American Control
Conference, Seattle, Washington, Pages:1628-1632. Available
from
http://www.robots.ox.ac.uk/siju/work/publications/
ACC95 pr.zip.
Also
see
Simon
Juliers
home
page
at
http://www.robots.ox.ac.uk/siju/.

[Kalman60] Kalman, R. E. 1960. A New Approach to Linear Filtering and

Prediction Problems,Transaction of the ASMEJournal of
Basic Engineering, pp. 35-45 (March 1960).
[Lewis86]

Lewis, Richard. 1986. Optimal Estimation with an Introduction

to Stochastic Control Theory, John Wiley & Sons, Inc.

[Maybeck79] Maybeck, Peter S. 1979. Stochastic Models, Estimation, and

Control, Volume 1, Academic Press, Inc.
[Sorenson70] Sorenson, H. W. 1970. Least-Squares estimation: from
Gauss to Kalman,IEEE Spectrum, vol. 7, pp. 63-68, July
1970.

17