Greg Welch 1 and Gary Bishop 2 TR 95-041 Department of Computer Science University of North Carolina at Chapel Hill 3 Chapel Hill, NC 27599-3175 : 2006 7 24 2007 1 8 1960 1 welch@cs.unc.edu, http://www.cs.unc.edu/ welch 2 gb@cs.unc.edu, http://www.cs.unc.edu/ gb 3 1
Welch & Bishop, 2 1 1960 [Kalman60] [Maybeck79] [Sorenson70] [Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93] x R n x k = Ax k 1 + Bu k 1 + w k 1, (1.1) z R m z k = Hx k + v k. (1.2) w k v k 1 p(w) N(0, Q), (1.3) p(v) N(0, R). (1.4) Q R u k 1 w k 1 1.1 n n A k 1 k A n l B u R l 1.2 m n H x k z k H ˆx k Rn ˆ k k ˆx k R n z k k e k x k ˆx k, e k x k ˆx k 1 process noise
Welch & Bishop, 3 P k = E[e k e T k ], (1.5) P k = E[e k e k T ], (1.6) 1.7 ˆx k z k H ˆx k ˆx k 1.7 ˆx k = ˆx k + K(z k H ˆx k ) (1.7) 1.7 (z k H ˆx k ) 1.7 n m K 1.6 K 1.7 e k e k 1.6 1.6 P k K K [Maybeck79, Brown92, Jacobs93] K K k = P k HT (HP k HT + R) 1 = P k HT HP k HT + R. (1.8) 1.8 R K R lim K k = H 1. R k 0 P k P k K lim K k = 0. P k 0 K R z k z k H ˆx k P k z k z k H ˆx k
Welch & Bishop, 4 1.7 ˆx k z k x k ˆx k E[x k ] = ˆx k E[(x k ˆx k )(x k ˆx k ) T ] = P k. 1.7 1.3 1.4 1.6 z k x k p(x k z k ) N(E[x k ], E[(x k ˆx k )(x k ˆx k ) T ]) = N(ˆx k, P k ). Brown92, Jacobs93] [Maybeck79, 1-1
Welch & Bishop, 5 1-1: 1-1 1-2 1-1: ˆx k = Aˆx k 1 + Bu k 1 (1.9) P k = AP k 1A T + Q (1.10) 1-1 x k P k k 1 k A B 1.1 Q 1.3 1-2: K k = P k HT (HP k HT + R) 1 (1.11) ˆx k = ˆx k + K k(z k H ˆx k ) (1.12) P k = (I K k H)P k (1.13) K k 1.11 1.8 z k 1.12 1.7 1.13
Welch & Bishop, 6 2 [Brown92] 1-2 1-1 1-2 1-2: 1-1 1-1 1-2 R R Q x k Q 2 1.12 1.13 1.9 1.10
Welch & Bishop, 7 Q R R K k 1-2 [Grewal93] P k R Q Q Q k Q k Q k 2 x R n Extended Kalman Filter EKF x R n z R m x k = f(x k 1, u k 1, w k 1 ), (2.1) z k = h(x k, v k ), (2.2) w k v k 2.1 f k 1 k 2.2 u k w k h x k z k w k v k x k = f(ˆx k 1, u k 1, 0) (2.3)
Welch & Bishop, 8 z k = h( x k, 0), (2.4) x k k [Julier96] Julier et al. 2.3 2.4 x k x k + A(x k 1 ˆx k 1 ) + W w k 1, (2.5) z k z k + H(x k x k )V v k. (2.6) x k z k x k z k 2.3 2.4 ˆx k k 1.3 1.4 w k v k A f x A [i,j] = f [i] x [j] (ˆx k 1, u k 1, 0), W f w W [i,j] = f [i] w [j] (ˆx k 1, u k 1, 0), H h x H [i,j] = h [i] x [j] ( x k, 0),
Welch & Bishop, 9 V h v V [i,j] = h [i] v [j] ( x k, 0). A, W, H, V k ẽ xk x k x k, (2.7) ẽ zk z k z k, (2.8) 2.7 x k, 2.8 z k x k 2.7 2.8 ẽ xk A(x k 1 ˆx k 1 ) + ɛ k, (2.9) ẽ zk Hẽ xk + η k, (2.10) ɛ k η k W QW T V RV T Q R 1.3 1.4 2.9 2.10 1.1 1.22.8 ẽ zk 2.9 ẽ xk ê k 2.7 ˆx k = x k + ê k. (2.11) 2.9 2.10 p(ẽ xk ) N(0, E[ẽ xk ẽ T x k ]) p(ɛ k ) N(0, W Q k W T ) p(η k ) N(0, V R k V T ) ê k ê k ê k = K k ẽ zk. (2.12) 2.8 2.12 2.11
Welch & Bishop, 10 ˆx k = x k + K k ẽ zk = x k + K k (z k z k ) (2.13) 2.13 x k z k 2.3 2.4 1.11 K k 2-1 2-2 ˆx k x k A, W, H, V 2-1: ˆx k = f(ˆx k 1, u k 1, 0) (2.14) P k = A kp k 1 A T k + W kq k 1 W T k (2.15) 2-1 k 1 k 2.14 f 2.3 A k W k k Q k 1.3 k 2-2: K k = P k HT k (H kp k HT k + V kr k Vk T (2.16) ˆx k = ˆx k + K k(z k h(ˆx k, 0)) (2.17) P k = (I K k H k )P k (2.18) 2-2 z k 2.17 h 2.4 H k V k R k 1.4 k k R k 1-1 2-1 1-1 2-1 2-2
Welch & Bishop, 11 2-1: 1-1 2-1 2-2 K k H k h z k H k z k h(ˆx k, 0) z k h 3 Andrew Straw http://www.scipy.org/cookbook/ KalmanFiltering Python/SciPy Root-
Welch & Bishop, 12 Mean-Square RMS 0.1 z R 1 x k = Ax k 1 + Bu k 1 + w k = x k 1 + w k, z k = Hx k + v k = x k + v k. A = 1 u = 0 H = 1 k ˆx k = ˆx k 1, P k = P k 1 + Q. K k = P k (P k + R) 1 = P k P k + R, (3.1) ˆx k = ˆx k + K k(z k ˆx k ), P k = (I K k )P k. Q Q = 10 5 Q = 0 ˆx k 1 = 0 P k 1 P 0 ˆx 0 = 0 P 0 = 0 ˆx 0 P 0 = 0 ˆx k = 0 P 0 P 0 0 P 0 = 1
Welch & Bishop, 13 x = 0.37727 x ˆ 50 z k 0 0.1 0.1 R = (0.1) 2 = 0.01 3-1 x = 0.37727 0.0-0.1 noisy measurements a posteri estimate truth value -0.2 Voltage -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 P 0 P 0 0 3-2 P k 50 1 0.0002
Welch & Bishop, 14 0.010 0.008 2 (Voltage) 0.006 0.004 0.002 0.000 0 10 20 30 40 50 Iteration 3-2: 50 P k 1 0.0002 Q R 3-3 3-4 R 100 3-3100 R = 1
Welch & Bishop, 15 0.0-0.1 noisy measurements a posteri estimate truth value -0.2 Voltage -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
Welch & Bishop, 16 0.0-0.1 noisy measurements a posteri estimate truth value -0.2 Voltage -0.3-0.4-0.5-0.6-0.7 0 10 20 30 40 50 Iteration 3-4: R = 0.0001 3-3
[Brown92] [Gelb74] [Grewal93] [Jacobs93] [Julier96] Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random Signals and Applied Kalman Filtering, Second Edition, John Wiley & Sons, Inc. Gelb, A. 1974. Applied Optimal Estimation, MIT Press, Cambridge, MA. Grewal, Mohinder S., and Angus P. Andrews (1993). Kalman Filtering Theory and Practice. Upper Saddle River, NJ USA, Prentice Hall. Jacobs, O. L. R. 1993. Introduction to Control Theory, 2nd Edition. Oxford University Press. Julier, Simon and Jeffrey 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 Systems by 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 Julier s 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 ASME Journal 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