On achievable accuracy for range finder localization How precise - PowerPoint PPT Presentation

On achievable accuracy for range finder localization How precise can a localization method be? Given a sensor, there is an hard limit: the Cramér–Rao bound. Summary: 1. Definition of the Cramér–Rao bound. 2. Application to range-finder localization. 3. A model for unstructured environments. 4. Experiments: predicting the error of the ICP . 1

The Cramér–Rao Bound • A localization algorithm A is an estimator of x given function A from the sensor data z : ˆ x = A ( z ) • The estimator is unbiased if E { A ( z ) } = x . Cramér–Rao inequality: For any unbiased estimator ˆ x , x ) ≥ ( I ( x )) − 1 Cov (ˆ The n × n symmetric matrix I ( x ) , called Fisher’s information matrix , is defined as � T � ∂ log p ( z , x ) ∂ log p ( z , x ) I ( x ) = E z ∂ x T ∂ x T 2

Fisher, Mahalanobis, Rao, Cramér • Sir Ronald Ayimer Fisher (1890-1962), England • Prasanta Chandra Mahalanobis (1893-1972), India • Harald Cramér (1893-1985), Sweden • Calyampudi Radhakrishna Rao (1920-), India 3

The Cramér–Rao lower bound • The CRB is valid only for unbiased estimators. – Consider the localization algorithm A ( z ) = (42 , 42 , 42 ◦ ) It has a covariance of 0 . – Biased estimators might have a lower Mean Square Error: MSE A ( x ) = var A ( x ) + bias 2 A ( x ) • The CRB It is not tight for non-Gaussian problems. – Localization is not a problematic problem. – Non-linear bounds are very very hard to derive. 4

CRB for localization The likelihood function is � ρ i − r ( t , θ + ϕ i ) , σ 2 ) p ( z | x ) = N (˜ i r ( p , ψ ) is the “raytracing function”. 5

CRB for localization Fisher’s information matrix is   v ( α i ) v ( α i ) T tan β i r i v ( α i ) I ( x ) = 1 cos 2 β i � cos β i     σ 2 i tan 2 β i r 2 i ∗ α is the surface direction, β � α i − ( θ + ϕ i ) 6

Results in a square environment var (ˆ det( cov (ˆ det( cov (ˆ x )) t )) θ ) Orientation 7

Kernel of I ( x ) θ y x In under-constrained situations, the kernel of I ( x ) gives the direction of uncertainty. 8

A model for unstructured environments Consider the environment (defined through the raytracing function) r ( 0 , φ ) = ρ + f ( φρ ) | f | ≪ ρ 9

A model for unstructured environments For this model, the following approximation holds: θ ) ≥ ( σ/ρ ) 2 1 var (ˆ C n ≥ 2 σ 2 1 � ˆ � λ min cov t 1 + C n � f ′ 2 � where C � E f . • Localization is easier with features (high values of C ). • For a circle: C = 0 . The uncertainty on θ is infinite, while the uncertainty for t is still finite. • The accuracy for θ depends on the “normalized” noise σ/ρ (invariance to scale) � ˆ � • cov does not depend on ρ . t 10

Experiments The CRB predicts very well the uncertainty of the ICP in localization, while it is a weak bound for scan matching. Residual error − x, θ Residual error − x, y Residual error − θ , y 4 0.06 4 Loc. cov. 3 C.−R. bound 0.04 S.M. cov. 2 2 0.02 1 y (mm) 0 θ (deg) y (mm) 0 0 −1 −2 −0.02 −2 −4 −0.04 −3 −4 −0.06 −6 −2 0 2 4 −0.1 −0.05 0 0.05 0.1 −4 −2 0 2 4 6 θ (deg) x (mm) x (mm) Asymmetric situation: square environment ( 5 m side), FOV = 180 ◦ , x = ( − 2 m, 2 m, 30 ◦ ) 11

Future work • Localization is a finite-dimensional problem, while SLAM is infinite-dimensional. • First explore mapping: the key is to choose a good representation for the map. – Polygonal environment: very easy to obtain the CRB for the map. – Occupancy grids: inference is tricky. – Splines. – Gaussian processes. 12

Predicting localization accuracy in a filter Update equation of a Bayesian filter: p ( x t | t ) ∝ p ( z t | x t ) ∫ p ( x t | x t − 1 , u t ) p ( x t − 1 | t − 1) d x t − 1 � �� � � �� � likelihood p ( x t | t − 1) If everything is Gaussian: � � − 1 � − 1 � Σ − 1 Σ x t | t = z t | x t + Σ x t − 1 | t − 1 + Σ u If your localization algorithm is efficient: � I ( x t ) + ( Σ t − 1 + Σ u ) − 1 � − 1 Σ x t | t ≃ 13