Rao-Blackwellized Particle Filtering for 6-DOF Estimation of Attitude and Position via GPS and Inertial Sensors

Transcription

1 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

2 Outline Motivation Motivation 3

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

4 Challenging Issues Multimodal pose distributions Manifold structure of SO(3) Representing uncertainty over SO(3) Multimodality

5 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

6 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 }, )

7 Adding Translational States The curse of dimensionality Naive: sample translational states Better: combine particle lter with Kalman lter Solution: Rao-Blackwellization (Doucet 00)

8 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

9 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

10 Algorithm Summary Inertial measurements GPS measurements weight weight Measurements aect particle dynamics, Gaussian statistics, importance weights

11 Simulation Error (degrees) Error (meters) Dead Reckoning Attitude Error 60 7 Dead Reckoning Position Error Error (degrees) Error (meters) 60 All Sensors Attitude Error All Sensors Position Error Error (degrees) Error (meters) No GPS Position Attitude Error 60 7 Error (degrees) No GPS Position Position Error Error (meters) No GPS Vel./Accel. Attitude Error 60 7 No GPS Vel./Accel. Position Error Filter converges to true mean in spite of large sensor noise, unavailability of specic sensors

12 Field Experiments The DARPA LAGR robot All sensors enabled GPS velocity disabled

13 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