kalman_intro_chinese

kalman_intro_chinese - Greg Welch1and Gary Bishop2 TR...

Info iconThis preview shows page 1. Sign up to view the full content.

View Full Document Right Arrow Icon
This is the end of the preview. Sign up to access the rest of the document.

Unformatted text preview: Greg Welch1and Gary Bishop2 TR 95-041 Department of Computer Science University of North Carolina at Chapel Hill3 Chapel Hill, NC 27599-3175 : 2006 7 24 2007 1 8 1960 1 2 3 welch@cs.unc.edu, http://www.cs.unc.edu/~welch gb@cs.unc.edu, http://www.cs.unc.edu/~gb 1 Welch & Bishop, 2 1 1960 [Kalman60] [Maybeck79] [Sorenson70] [Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93] x n xk = Axk-1 + Buk-1 + wk-1 , z m (1.1) zk = Hxk + vk . wk vk p(w) N (0, Q), p(v) N (0, R). Q uk-1 A A u l 1 (1.2) (1.3) (1.4) R 1.1 k B H xk nn k-1 1.2 H wk-1 nl mn zk x- ^k k n - ^ xk ^ n k zk k e- xk - x- , ^k k ek xk - xk ^ 1 process noise UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 3 - Pk = E[e- e- ], k k T (1.5) Pk = E[ek ek T ], 1.7 zk H x- ^k xk = x- + K(zk - H x- ) ^ ^k ^k 1.7 (zk - H x- ) ^k nm ek K - - Kk = Pk H T (HPk H T + R)-1 (1.6) x- ^k xk ^ 1.7 (1.7) 1.7 ek K 1.6 K K Brown92, Jacobs93] 1.6 K 1.7 1.6 Pk [Maybeck79, = 1.8 R - Pk H T . - HPk H T + R (1.8) K R lim Kk = H -1 . - Pk Rk 0 - Pk K - Pk 0 lim Kk = 0. R zk zk zk K H x- ^k zk - Pk H x- ^k UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 4 1.7 zk xk x- ^k xk ^ E[xk ] = xk ^ E[(xk - xk )(xk - xk )T ] = Pk . ^ ^ 1.7 1.3 1.4 zk xk 1.6 p(xk |zk ) N (E[xk ], E[(xk - xk )(xk - xk )T ]) ^ ^ = N (^k , Pk ). x [Maybeck79, Brown92, Jacobs93] 1-1 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 5 1-1: 1-1 1-2 1-1: x- = A^k-1 + Buk-1 ^k x - Pk (1.9) (1.10) x- k 1.1 Q = APk-1 A + Q T P- k 1-1 k-1 k A B 1.3 1-2: - - Kk = Pk H T (HPk H T + R)-1 (1.11) (1.12) (1.13) xk = ^ x- ^k + Pk = (I Kk (zk - H x- ) ^k - - Kk H)Pk Kk 1.12 1.11 1.7 1.8 zk 1.13 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 6 2 [Brown92] 1-2 1-1 1-2 1-2: 1-1 1-1 1-2 R R Q xk Q 2 1.12 1.13 1.9 1.10 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 7 Q R Kk R 1-2 [Grewal93] R Pk Q Q Qk Qk Qk 2 x n Extended Kalman Filter EKF x n xk = f (xk-1 , uk-1 , wk-1 ), z m (2.1) zk = h(xk , vk ), wk f 2.2 xk vk uk zk wk vk k-1 wk k (2.2) 2.1 h xk = f (^k-1 , uk-1 , 0) ~ x (2.3) UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 8 zk = h(~k , 0), ~ x xk ~ k (2.4) [Julier96] al. Julier et 2.3 2.4 xk xk + A(xk-1 - xk-1 ) + W wk-1 , ~ ^ zk zk + H(xk - xk )V vk . ~ ~ (2.5) (2.6) xk xk ~ xk ^ A zk zk ~ k 1.3 f x A[i,j] = f[i] (^k-1 , uk-1 , 0), x x[j] 1.4 wk vk 2.3 2.4 W f w W[i,j] = f[i] (^k-1 , uk-1 , 0), x w[j] H h x H[i,j] = h[i] (~k , 0), x x[j] UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 9 V h v V[i,j] = h[i] (~k , 0). x v[j] k A, W, H, V exk xk - xk , ~ ~ (2.7) ezk zk - zk , ~ ~ 2.7 xk 2.7 2.8 xk , 2.8 zk (2.8) exk A(xk-1 - xk-1 ) + ~ ^ ezk H exk + k , ~ ~ k k, (2.9) (2.10) V RV T k Q R 2.9 1.1 ek ^ W QW T 1.3 2.10 1.2 2.9 2.7 xk = xk + ek . ^ ~ ^ 2.8 1.4 ezk ~ exk ~ (2.11) 2.9 2.10 p(~xk ) N (0, E[~xk eTk ]) e e ~x p( k ) N (0, W Qk W T ) p(k ) N (0, V Rk V T ) ek ^ ek ^ ek = Kk ezk . ^ ~ 2.8 2.12 2.11 (2.12) UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 10 xk = xk + Kk ezk ^ ~ ~ = xk + Kk (zk - zk ) ~ ~ zk ~ 2.13 2.3 2-1 xk ~ 2.4 Kk 2-2 1.11 x- ^k A, W, H, V (2.13) xk ~ 2-1: x- = f (^k-1 , uk-1 , 0) ^k x (2.14) (2.15) - T Pk = Ak Pk-1 AT + Wk Qk-1 Wk k k-1 Wk k k Qk 2-1 2.14 1.3 k f 2.3 Ak 2-2: - T - T Kk = Pk Hk (Hk Pk Hk + Vk Rk VkT )-1 (2.16) (2.17) (2.18) xk = ^ + Kk (zk - h(^- , 0)) xk - Pk = (I - Kk Hk )Pk 2-2 2.17 1.4 k 1-1 x- ^k zk k k 2-1 Rk 1-1 2-1 h 2.4 Hk V Rk 2-2 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 11 2-1: 1-1 2-1 Kk 2-2 Hk h zk zk - h(^- , 0) xk zk Hk h 3 KalmanFiltering Andrew Straw http://www.scipy.org/Cookbook/ Python/SciPy Root- UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 12 Mean-Square RMS 0.1 xk = Axk-1 + Buk-1 + wk = xk-1 + wk z 1 , zk = Hxk + vk = xk + vk A=1 H =1 k . u=0 x- = xk-1 , ^k ^ - Pk = Pk-1 + Q. - - Kk = Pk (Pk + R)-1 = - Pk - Pk + R , (3.1) xk = x- + Kk (zk - x- ), ^ ^k ^k - Pk = (I - Kk )Pk . Q Q = 10-5 Q=0 xk-1 = 0 ^ Pk-1 P0 = 0 xk = 0 ^ P0 = 0 P0 x0 ^ x0 = 0 ^ P0 = 0 P0 P0 = 1 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 13 50 0.1 x = -0.37727 zk x 0 0.1 ^ R = (0.1)2 = 0.01 3-1 x = -0.37727 0.0 noisy measurements a posteri estimate truth value -0.1 -0.2 Voltage -0.3 -0.4 -0.5 -0.6 -0.7 0 10 20 Iteration 30 40 50 3-1: x = -0.37727 R = (0.1)2 = 0.01 P0 50 1 3-2 0.0002 P0 = 0 Pk UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 14 0.010 0.008 0.006 (Voltage) 0.004 0.002 0.000 2 0 10 20 Iteration 30 40 50 3-2: 50 - Pk 1 0.0002 Q R 3-3 3-3 100 3-4 R=1 R 100 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 15 0.0 noisy measurements a posteri estimate truth value -0.1 -0.2 Voltage -0.3 -0.4 -0.5 -0.6 -0.7 0 10 20 Iteration 30 40 50 3-3: R=1 3-4 100 R = 0.0001 UNC-Chapel Hill, TR 95-041, July 24, 2006 Welch & Bishop, 16 0.0 noisy measurements a posteri estimate truth value -0.1 -0.2 Voltage -0.3 -0.4 -0.5 -0.6 -0.7 0 10 20 Iteration 30 40 50 3-4: R = 0.0001 3-3 UNC-Chapel Hill, TR 95-041, July 24, 2006 [Brown92] 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. [Gelb74] [Grewal93] Grewal, Mohinder S., and Angus P. Andrews (1993). Kalman Filtering Theory and Practice. Upper Saddle River, NJ USA, Prentice Hall. [Jacobs93] [Julier96] 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 ...
View Full Document

Ask a homework question - tutors are online