Foundations of state Estimation Topics: Bayes Filters Kalman filters Hidden markov models 。 Additional readins G. Welch and G. Bishop. An Introduction to the Kalman Filter ". University of North Carolina at Chapel Hill, DepartmentofComputersCience.Tr95-041.http://www.cs.unc.edu/-welch/kalman/kalmanintro.html JJ. Leonard and H. F. Durrant-Whyte. " Mobile robot localization by tracking geometric beacons. "IEEE Trans Robotics and Automation, 7(3): 376-382, June 1991 L.R. Rabiner, "A tutorial on hidden Markov models, " Proceedings of the IEEE, vol. 77, pp. 257-286, 1989 Issues Statement Given a set of observations of the world. what is the current state of the world? Alternatives Given a set of observations of the world what is the state of the world at time t? Inputs Sequence of observations, or perhaps actions and observations Outputs from different algorithms Most likely current state x Probability distribution over possible current states p(xi) Most likely sequence of states over time: X,, 2, X3, x4, .X, Sequence of probability distributions over time p(,), p(x2), p(x3).p(x,) Choices compute the estimate x, or p(xi) present p(x)
Foundations of State Estimation Topics: Hidden Markov Models Ɣ Additional reading: Ɣ Ɣ J. J. Leonard and H. F. Durrant-Whyte. “ IEEE Trans. Ɣ Issues Ɣ Statement: – world? Ɣ – time t? Ɣ Inputs: – Model – Ɣ Outputs from different algorithms: – Most likely current state xi – i ) – Most likely sequence of states over time: x1, x2, x3, x4, ... xt – 1), p(x2), p(x3) ... p(xt ) Ɣ Choices: – How to compute the estimate xi or p(xi ) – i ) Bayes Filters Kalman Filters G. Welch and G. Bishop. “An Introduction to the Kalman Filter”. University of North Carolina at Chapel Hill, Department of Computer Science. TR 95-041. http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html Mobile robot localization by tracking geometric beacons.” Robotics and Automation, 7(3):376-382, June 1991 L.R. Rabiner, “A tutorial on hidden Markov models," Proceedings of the IEEE, vol. 77, pp. 257-286, 1989. Given a set of observations of the world, what is the current state of the Alternatives: Given a set of observations of the world, what is the state of the world at Sequence of observations, or perhaps actions and observations Probability distribution over possible current states p(x Sequence of probability distributions over time p(x How to represent p(x
Graphical models, bayes' rule and the markov Assumption actions Beliefs T(xi a Observations (Z, Observable O(z /x) states Bayes rule: P(x y) p(y)p p(y) Markov:p(x1|x1,a1,a0,z0,a1,21,…,z)=p(x1|x1,a1) The Bayes' Filter p, (x)=p(x ao, zo, a,, z, a ,,z, According to Bayes rule: p(z|x,a0,z0,a1,z1,a2,2…,a,)p(xao,20,a1,2,a a.) According to the markov assumption p(z,)p(x Introducing an auxiliary variable p(z,1x)P(x|a, x')P(x ao, Zo, a, -,,a-1, 2-1) And with recursion =p(z, 1x)p(xla, x')pi-I(x)
Graphical Models, Bayes’ Rule and the Markov Assumption States x1 x2 T(xj |ai , xi ) Z2 b Beliefs 1 Observations Z1 a Actions 1 O(zj |xi ) b2 Z2 Hidden Observable ( ) ( | ) ( ) ( | ) p y p y x p x Bayes rule : p x y ( | , , , , , , , ) ( | , ) t t 1 t 0 0 1 1 t 1 t t 1 at p x x a a z a z z p x x Markov : The Bayes’ Filter ( | ) ( | , ') ( ) ( | ) ( | , ') ( '| , , , ,..., , ) ( | ) ( | , , , , , ,..., ) ( | , , , , , , ,..., ) ( | , , , , , ,..., ) ( ) ( | , , , , , ,..., , ) 1 ' 0 0 1 1 1 1 ' 0 0 1 1 2 2 0 0 1 1 2 2 0 0 1 1 2 2 0 0 1 1 2 2 p z x p x a x p x p z x p x a x p x a z a z a z p z x p x a z a z a z a p z x a z a z a z a p x a z a z a z a p x p x a z a z a z a z t X t t t t X t t t t t t t t t t ³ ³ And with recursion: Introduc ing an auxiliary variable : Accordi ng to the Markov assumption : Accordi ng toBayes rule :
Bayes Filter An action is taken 意/ 二2 Initial belief Posterior belief Posterior belief after an action after sensing The Kalman filter Kalman/960 Linear process and measurement models Gaussian noise Gaussian state estimate Image adapted from Maybeck Process model is Ax,+ B qn Measurement model is
Posterior belief after an action An action is taken Posterior belief after sensing State Space Initial belief Ɣ Linear process and measurement models Ɣ Gaussian noise Ɣ Gaussian state estimate Kalman, 1960 fx(t1)|z(t1)(x|z1) fx(t2)|z(t2)(x|z2) fx(t2)|z(t1 2) |z1 2) Z1 X x1 Z1 Z2 Z1 Z2 x σ σ σ µ xt zt t . Bayes Filter The Kalman Filter ),z(t (x ,z x2 Process model is = Axt-1 + But-1 + qt-1 Measurement model is = Hxt-1 + r Image adapted from Maybeck
Linear Kalman filter Only need first two moments E(x,) E[(x2-元,)(x1-元,)] Process model ax,+ Bu C=ACIA+o Measurement model K,=C, H(HC H+R)- x,=x,+K,(z1-BE) C1=(-K,H)C The Extended Kalman Filter Process model is now f(x-1,u-1,q1) Measurement model is h(x,, r Linearising we get f(x x≈x1+A(x-1-x1)+Wq1 h(x1,0)+H(x-x)+V Where A.H. w and v are the jacobians …100a…「aa , of afr
Ɣ Only need first two moments: Ɣ Process model: Ɣ Measurement model: [( ˆ )( ˆ ) ] ˆ ( ) T t t t t t t t C E x x x x x E x C AC A Q x Bu T t t t t t 1 1 1 ˆ ˆ t t t t t t t t T t T t t C I K H C x x K z K C H HC H R ( ) ˆ ˆ ( ˆ ) ( ) 1 Ɣ Process model is now Ɣ Measurement model is Ɣ Linearising, we get Ɣ where A, H, W and V are the Jacobians: ( , , ) t t1 t1 t1 x u q ( , ) t t t z r t t t t t t t t t t t t t z H x x Vr x x A x x Wq x u | | ) ~ ) ( ~( ( ˆ ) ~ ( , ) ~ 1 1 1 1 1 » » » » » ¼ º « « « « « ¬ ª w w w w w w w w n n n n x f x f x f x f A 1 1 1 1 » » » » » ¼ º « « « « « ¬ ª w w w w w w w w n m m n x h x h x h x h H 1 1 1 1 » » » » » ¼ º « « « « « ¬ ª w w w w w w w w n n n n q f q f q f q f W 1 1 1 1 » » » » » ¼ º « « « « « ¬ ª w w w w w w w w m m m m v h v h v h v h V 1 1 1 1 Linear Kalman Filter A x H x The Extended Kalman Filter f x h x h x f x , 0 , 0
Updates for the eKF Still using only first two moments Process model x, =fo CI=ACIA +w,2w Measurement model T K C H,(H,C H +VR V) x,=x,+K,(2,-h(x1,0) (-K,H,)C Leonard and Durrant-White 1991 Robot navigation Initial belief Action Measurement x+△ t cos e+q△ t cos e △b f(5,u,q)=y+△sinb+q△tsin b+△b+q△6 h(,v) nge bearing tan 6+v
Updates for the EKF Ɣ Still using only first two moments. Ɣ Process model: Ɣ Measurement model: T t t t T t t t t t t t C A x f x u 1 1 1 ˆ (ˆ , ) t t t t t t t t t T t t t T t t t T t t t C I C x x K z K C H H ( ) ˆ ˆ ( (ˆ ( ) 1 Robot Navigation Initial belief Action Measurement » » » ¼ º « « « ¬ ª T [ y x » » » ¼ º « « « ¬ ª ' ' ' T T T T T T T [ q y t q t x t q t f sin sin cos cos ( , , ) » ¼ º « ¬ ª ' ' T t u » ¼ º « ¬ ª b r z » » » ¼ º « « « ¬ ª ¸ ¸ ¹ · ¨ ¨ © § » ¼ º « ¬ ª T T O O O O [ v x y x y v h v x y x y r 1 2 2 tan ( , ) bearing range [ t1 [t [ t » ¼ º « ¬ ª y x O O O A C W Q W , 0 K H h x H C V R V , 0 )) ' ' ' u q Leonard and Durrant-White, 1991