Estimation of Poses with Particle Filters

Transcription

1 Esimaion of Poses wih Paricle Filers Dr.-Ing. Bernd Ludwig Chair for Arificial Inelligence Deparmen of Compuer Science Friedrich-Alexander-Universiä Erlangen-Nürnberg 12/05/2008 Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

2 Overview 1 Probabiliy Disribuions in he Bayesian Filer 2 Velociy Moion Model 3 Sampling New Poses 4 Odomery Based Model 5 A Nonparameric Approximaion of he Bayesian Filer: he Paricle Filer 6 Weighing and Resampling 7 The Paricle Filer Algorihm Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

3 The Paricle Filer Algorihm ˆX = X = for m = 1 o M do ( ) sample s [m] p s u, s [m] 1 ( ) w [m] = p z s [m] ˆX = ˆX +, w [m] endfor s [m] for m = 1 o M do draw i wih probabiliy proporional o w [i] X = X + s [i] endfor reurn X Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

4 The Bayesian Filer Revisied (1) Bayesian Filer p(s, Θ z, u, n ) = η p(z s, Θ, n ) Z p(s s 1, u ) p(s 1, Θ z 1, u 1, n 1 )ds 1 Moion Model p(s u, s 1 ) Noisy Moion ˆv ˆω «= v ω «ɛα1 v +α + 2 ω ɛ α3 v +α 4 ω (ɛ b is a zero-mean random variable for he noise wih sandard deviaion b.) «Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

5 The Bayesian Filer Revisied (2) Sensor Model The correlaion beween sensing and moion is capured in he probabilisic sensor model p(z s, n, Θ) This disribuion changes when Roberino moves around. Recursive Sae Esimaion The SLAM poserior capures he relaionship beween moion and sensing. I is compued by recursively updaing he poserior disribuion wih a recursive filer. Fur his purpose we mus approximae he disribuions for sensor model and moion model. Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

6 Finding he Nex Pose Overview (x, y ) θ + ω ω Roberino s curren pose is (x, y, θ). In he inerval we assume r ω α (x c, y c ) r γ β (x, y) θ consan ranslaional velociy v and consan roaional velociy ω. Roberino moves on a circle wih radius r and cener (x c, y c). Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

7 Finding he Nex Pose Compuaion (1) Kinemaics: r = v ω Compuaion of he angles α, β, γ: β = π π 2 π + θ = θ π 2 γ = π 2 β = π θ α = π 2 γ = θ π 2 Cener of he circle ( x c = x r cos θ π ) ( π ) = x r cos 2 2 θ = x v ω sin θ ( y c = y r sin θ π ) ( π ) = y + r sin 2 2 θ = y + v ω cos θ Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

8 Finding he Nex Pose Compuaion (2) Compuaion of α, β, γ : α = θ + ω π 2 β = θ + ω π 2 γ = π θ ω Compuaion of (x, y, θ ) x y θ = x c + v ω sin(θ + ω ) y c v ω cos(θ + ω ) ω (Solve equaions for x c and y c for x and y and subsiue θ + ω for θ!) Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

9 Noise In our model, noise is a linear combinaion of he ranslaional velociy v and he roaional velociy ω. In oher words: he error in he moion model has wo componens: error during ranslaion error during roaion The expeced (mean value of he) noise is 0: non-noisy daa! The variance of he noise depends on paricular parameers for Roberino moving in a given environmen. Therefore he variance of he noise can be se o: Noise for Moion ˆv = α 1 v + α 2 ω ˆω = α 3 v + α 4 ω Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

10 Approximaive Compuaion of he Nex Pose (1) Beyond he values for mean and variance, we have o assume a analyical disribuion for he noise. We assume noise in moion o be disribued according o a Gaussian. Under his assumpion, we can compue a new pose x (given x 1, v, and ω ). For ha purpose, we compued he nex (non-noisy) pose (see earlier slides) and add noise (using he approximaion below). We call his drawing a sample from he moion model p(x u, x 1 ). Approximaive Normal Disribuion sample normal disribuion(sandard deviaion b) reurn i=1 rand( b, b) Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

11 Approximaive Compuaion of he Nex Pose (2) The algorihm below also perurbes he final orienaion by he random variable ˆγ. Sampling from he Velociy Model sample moion model velociy(u, x 1 ) reurn x ˆv = v + sample normal disribuion(α 1 v + α 2 ω ) ˆω = ω + sample normal disribuion(α 3 v + α 4 ω ) ˆγ = sample normal disribuion(α 5 v + α 6 ω ) x = x sin(θ + ˆω ) ˆvˆω y = y + cos(θ + ˆω ) ˆvˆω θ = θ + ˆω + ˆγ x = (x, y, θ ) T Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

12 Noisy Odomery Daa In he absence of noise, we can reconsruc moion from odomery measuremens as follows: δ ro1 = aan2(y y, x x) θ δ rans = (x x ) 2 + (y y ) 2 δ ro2 = θ θ δ ro1 As wih he velociy based models, noise is conrolled by a random variable wih zero mean and a sandard deviaion ha depends on he hardware. Again, we assume ha he error may be modelled as a linear dependence of he noise free conrol variables. The srengh of he linear dependence is conrolled by four parameers α 1, α 2, α 3 and α 4. Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

13 Approximaive Compuaion of he Nex Pose (1) The noisy conrols are: ˆδ ro1 = δ ro1 N (0, α 1 δ ro1 + α 2 δ rans ) ˆδ rans = δ rans N (0, α 3 δ rans + α 4 δ ro1 + δ ro2 ) ˆδ ro2 = δ ro2 N (0, α 5 δ ro2 + α 6 δ rans ) Wih hese, values we can compue he nex pose from he curren one: x x ˆδ rans cos(θ + ˆδ ro1 ) y = y + ˆδ rans sin(θ + ˆδ ro1 ). θ θ θ + ˆδ ro1 + ˆδ ro2 Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

14 Approximaive Compuaion of he Nex Pose (2) Sampling from he Odomery Model sample moion model odomery(u,x 1 ) δ ro1 = aan2(y y, x x) θ q δ rans = (x x ) 2 + (y y ) 2 δ ro2 = θ θ δ ro1 ˆδ ro1 = δ ro1 sample normal disribuion(α 1 δ ro1 + α 2 δ rans ) ˆδ rans = δ rans sample normal disribuion(α 3 δ rans + α 4 δ ro1 + δ ro2 ) ˆδ ro2 = δ ro2 sample normal disribuion(α 1 δ ro2 + α 2 δ rans ) x = ˆδ rans cos(θ + ˆδ ro1 ) y = ˆδ rans sin(θ + ˆδ ro1 ) θ = θ + ˆδ ro1 + ˆδ ro2 reurn x = (x, y, θ ) T Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

15 Ideas Underlying he Paricle Approximaion The poserior of a map can hardly be represened using a parameric approximaion. A differen idea for approximaion is o have a represenaive sample of he acual disribuion. The elemens of such a sample are called paricles. A sample of M paricles is denoed as X = {s [1], s [2],..., s [M] } In case of a recursive filer, here is a sample for each ime : X = {s [1], s [2],..., s [M] } In our case, each paricle is a map (i.e a hypohesis of how Roberino s environmen could look like). Paricles may occur repeaedly in he sample reflecing he fac ha some hyoheses are more plausible han ohers. Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

16 Inegraion of Moion and Sensing (1) Obviously, he larger M is, he more represenaive is he sample for he acual disribuion p(s z, u ). Each ime Roberino is execuing u i will updae is belief (pose updae) according o p(s s 1, u ) A represenaive sample conains M paricles from he sample for 1 disribued proporional o he moion model. This reflecs he Markov assumpion: The probabiliy for ending up in s saring from s 1 wih he noisy conrol u depends on he probabiliy of s 1 and he probabiliy of he ransiion s 1 s. Drawing M paricles, we ge a new sample ˆX from X 1. For a complee updae, we mus sill inegrae he measuremen. Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

17 Inegraion of Moion and Sensing (2) Each ime Roberino is sensing z i will updae is belief of he environmen: The poserior will change. Therefore, he sample mus be modified o approximae he new disribuion. The derivaion of he Bayesian filer shows ha he modificaion amouns o p(z s ) Again, his reflecs he Markov assumpion: he probabiliy of he noisy measuremen z is condiioned on he noisy hypohesis abou he sae s ha generaes z. To approximae he disribuion, for each paricle s [m] hax we compue is so called weigh or imporance facor: w [m] = p(z s ) Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

18 Imporance Facor (1) When building a sample of size M, we use he imporance facor o know how probable each paricle s [m] ˆX is. In order o build he paricle se X from X 1 ha inegraes moion and sensing a ime, we draw paricles from ˆX wih replacemen. w [m] ells how probable i is o draw he m h paricle. This sep is called resampling. Afer moving and sensing, Roberino mus know he new SLAM poserior disribuion. Pose updae however, jus compues a disribuion ha mus sill be shifed according o he measuremen. Le f (x) be he probabiliy densiy for X, and g(x) be he densiy for ˆX. Afer sensing, he expecaion for Roberino o be locaed a s = ŝ is: E f (I(s = ŝ)) = f (s )I(s = ŝ)ds Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

19 Imporance Facor (2) Bu wihou resampling, ˆX is disribued according o g(x). Therefore we adjus he inegrand wih a surprising rick: f (s ) f (s )I(s = ŝ)ds = g(s ) g(s )I(s = ŝ)ds Now we se and ge he resul Resampling w(x) = f (x) g(x) f (s ) g(s ) g(s )I(s = ŝ)ds = w(s )g(s )I(s = ŝ)ds = E g (I(s = ŝ)) w(x) ells o wha amoun measuremens influence he esimaion of he curren pose. Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

20 Mahemaical Correcness of he Approach (1) To show ha paricle filers really work, we non jus approximae he SLAM poserior for he las pose s, bu for he whole pah s. In analogy o he derivaion of he Bayesian filer, we can show p(s z, u ) = (Bayes) = ηp(z s, z 1, u )p(s z 1, u ) (Markov) = ηp(z s )p(s z 1, u ) (Bayes) = ηp(z s )p(s s 1 z 1 u )p(s 1 z 1 u ) (Markov) = ηp(z s )p(s s 1 u )p(s 1 z 1 u 1 ) To argue,ha he paricle filer approximaes jus his disribuion, e use inducion on he pah lengh. For a pah of lengh 0, we build X 0 by sampling he prior disribuion p(x 0 ). Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

21 Mahemaical Correcness of he Approach (2) For laer seps, he inducion hypohesis is X 1 is disribued according o p(s 1 z 1, u 1 ) Drawing a paricle s [m] amouns o sampling from X 1 according o p(s [m] s [m] 1, u ) p(s s 1, u )p(s 1 z 1, u 1 ) For he imporance facor, we have p(s z, u ) = η p(s s 1, u )p(s 1 z 1, u 1 ) w [m] This is rue as we know o approximae he pah poserior. = η p(z s )p(s s 1, u )p(s 1 z 1, u 1 ) p(s s 1, u )p(s 1 z 1, u 1 ) = ηp(z s ) Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22

22 Mahemaical Correcness of he Approach (3) We can now conclude ha resampling ˆX by drawing M paricles wih probabiliies according o w [i] resuls in a sample X ha approximaes p(s z, u ). Finally: as he samples for he complee pah are disribued according o p(s z, u ), s [m] samples p(s z, u ). Dr.-Ing. Bernd Ludwig (FAU ER) Esimaion of Poses wih Paricle Filers 12/05/ / 22