Rao-Blackwellized Particle Filtering for 6-DOF Estimation of Attitude and Position via GPS and Inertial Sensors GRASP Laboratory University of Pennsylvania June 6, 06
Outline Motivation Motivation 3
Problem Statement: 6-DOF Vehicle Pose Estimation Calculate (online) probability distribution of 6-DOF vehicle pose given diverse sensor measurements Sensors: GPS (position, velocity), wheel encoders (odometry), inertial unit (accelerometers, rate gyroscopes, possibly magnetometer) Requirements: robustness to sensor outage, cope with noisy sensors, real-time performance
Challenging Issues Multimodal pose distributions Manifold structure of SO(3) Representing uncertainty over SO(3) Multimodality
Particle Filtering for Orientation Estimation p(θ z t ) = i w i δ(θ θ i ) Sampled orientations dynamics importance weighting Particle ltering alleviates most problems with orientation estimation
Process, Measurement Models Rate gyros (ω g t ) specify dynamics update (Gaussian ωɛ t ): p(θ t θ t ) = f (θ t, ω g t + ω ɛ t, t ) Accelerometers (and/or magnetometers) provide body-relative measurements of eld vector g f : p(z t θ (i) t ) = N (z t ; Rot fb {θ (i) t, g f }, )
Adding Translational States The curse of dimensionality Naive: sample translational states Better: combine particle lter with Kalman lter Solution: Rao-Blackwellization (Doucet 00)
Rao-Blackwellization Samples with associated Gaussians predicted measurement distribution p(θ, x z t ) = i w i δ(θ θ i )p(x θ i, z t ) actual measurement Mixture of particle ltering and exact inference Importance weights Sampled orientations, Gaussians over translations Free linearization Easily-calculated importance weights
Translational Process, Measurement Models Translational state (xed frame): p = Process (velocity integrator): ( ) I I T ṗ = p 0 I ( ) x ẋ Measurement (odometry): ( ) z od = 0 R fb {θ (i) t } p Measurement (GPS): z gps = ( ) I 0 p 0 I
Algorithm Summary Inertial measurements GPS measurements weight weight Measurements aect particle dynamics, Gaussian statistics, importance weights
Simulation Error (degrees) Error (meters) Dead Reckoning Attitude Error 60 7 Dead Reckoning Position Error 70 0 0 0.5 Error (degrees) Error (meters) 60 All Sensors Attitude Error 7 70 0 0 0.5 All Sensors Position Error Error (degrees) Error (meters) No GPS Position Attitude Error 60 7 Error (degrees) No GPS Position Position Error 70 0 0 0.5 Error (meters) No GPS Vel./Accel. Attitude Error 60 7 No GPS Vel./Accel. Position Error 70 0 0 0.5 0. 0. 0. 0. Filter converges to true mean in spite of large sensor noise, unavailability of specic sensors
Field Experiments The DARPA LAGR robot All sensors enabled GPS velocity disabled
Summary, Future Work Sampling orientations alleviates representational issues for inference Free linearization of process/measurement models (when nonlinearity is caused by orientation) Resulting lter is robust to signicant sensor noise/outage, fairly simple to implement Future/related work: map integration, SLAM-like aspects