Welch & Bishop, [Kalman60] [Maybeck79] [Sorenson70] [Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93] x R n x k = Ax k 1 + Bu k 1 + w

Size: px

Start display at page:

Download "Welch & Bishop, [Kalman60] [Maybeck79] [Sorenson70] [Gelb74, Grewal93, Maybeck79, Lewis86, Brown92, Jacobs93] x R n x k = Ax k 1 + Bu k 1 + w"

评严速
7 years ago
Views:

1 Greg Welch 1 and Gary Bishop 2 TR Department of Computer Science University of North Carolina at Chapel Hill 3 Chapel Hill, NC : welch@cs.unc.edu, welch 2 gb@cs.unc.edu, gb 3 1

2 Welch & Bishop, [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 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

3 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 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

4 Welch & Bishop, ˆ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 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

5 Welch & Bishop, 5 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 : 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 z k

6 Welch & Bishop, 6 2 [Brown92] : R R Q x k Q

7 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)

8 Welch & Bishop, 8 z k = h( x k, 0), (2.4) x k k [Julier96] Julier et al 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 ˆx k k 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),

9 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 ẽ 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 ẽ zk 2.9 ẽ xk ê k 2.7 ˆx k = x k + ê k. (2.11) 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)

10 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 K k ˆ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

11 Welch & Bishop, : K k H k h z k H k z k h(ˆx k, 0) z k h 3 Andrew Straw KalmanFiltering Python/SciPy Root-

12 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

13 Welch & Bishop, 13 x = x ˆ 50 z k R = (0.1) 2 = x = noisy measurements a posteri estimate truth value -0.2 Voltage Iteration 3-1: R = (0.1) 2 = 0.01 x = P 0 P P k

14 Welch & Bishop, (Voltage) Iteration 3-2: 50 P k Q R R R = 1

15 Welch & Bishop, noisy measurements a posteri estimate truth value -0.2 Voltage Iteration 3-3: R = R =

16 Welch & Bishop, noisy measurements a posteri estimate truth value -0.2 Voltage Iteration 3-4: R =

17 [Brown92] [Gelb74] [Grewal93] [Jacobs93] [Julier96] Brown, R. G. and P. Y. C. Hwang Introduction to Random Signals and Applied Kalman Filtering, Second Edition, John Wiley & Sons, Inc. Gelb, A 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 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 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: Available from siju/work/publications/ ACC95 pr.zip. Also see Simon Julier s home page at siju/. [Kalman60] Kalman, R. E A New Approach to Linear Filtering and Prediction Problems, Transaction of the ASME Journal of Basic Engineering, pp (March 1960). [Lewis86] Lewis, Richard Optimal Estimation with an Introduction to Stochastic Control Theory, John Wiley & Sons, Inc. [Maybeck79] Maybeck, Peter S Stochastic Models, Estimation, and Control, Volume 1, Academic Press, Inc. [Sorenson70] Sorenson, H. W Least-Squares estimation: from Gauss to Kalman, IEEE Spectrum, vol. 7, pp , July

: 29 : n ( ),,. T, T +,. y ij i =, 2,, n, j =, 2,, T, y ij y ij = β + jβ 2 + α i + ɛ ij i =, 2,, n, j =, 2,, T, (.) β, β 2,. jβ 2,. β, β 2, α i i, ɛ i

: 29 : n ( ),,. T, T +,. y ij i =, 2,, n, j =, 2,, T, y ij y ij = β + jβ 2 + α i + ɛ ij i =, 2,, n, j =, 2,, T, (.) β, β 2,. jβ 2,. β, β 2, α i i, ɛ i 2009 6 Chinese Journal of Applied Probability and Statistics Vol.25 No.3 Jun. 2009 (,, 20024;,, 54004).,,., P,. :,,. : O22... (Credibility Theory) 20 20, 80. ( []).,.,,,.,,,,.,. Buhlmann Buhlmann-Straub