1. State Estimation for Autonomous Vehicles
Stergios I. Roumeliotis
Computer Science & Engineering Department
University of Minnesota

2. Outline Sensing & Estimation Estimator Requirements
Indirect Kalman filter formulation
SC-KF
Preliminary results
Ongoing & related work
Challenges & unresolved issues
Extensions & future work

3. Sensors Proprioceptive: Directly measure the motion of the vehicle
IMU (accels. & gyros)
Doppler radar
Noise Integration
Exteroceptive: Measure “identities” of the environment, or, “relation” of vehicle with the environment. Used to infer absolute and/or relative position and orientation (pose displacement)
Compass, Sun Sensor
GPS
Cameras (single, stereo, omni, FLIR)
Laser scanner, MW radar, Sonar
Wheel Encoders …
Uncertainty & Noise

4. State Estimation Propagation Update Techniques
Bayesian estimation
Kalman filter
Particle filter
Unscented filter …
Goal: Estimate & Control
State of vehicle (position, orientation, velocity, direction of motion, …)
State of environment (detect obstacles, position of objects of interest, area identities, mapping, …)
Propagation
Update

5. Estimator Requirements
Portable (independent of vehicle)
Adaptable (number & type of sensors)
Modular (robustness against single point sensor failures)
Time flexible (able to process synchronous & asynchronous sensor measurements)
Expandable to multi-robot systems

6. Sensor &Vehicle Independence
Adaptability & Portability:
Estimator considers any vehicle as a static network of sensors at known configuration

7. Indirect Kalman filter – Sensor Modeling
State Propagation: Integrate sensor measurements from these of the sensors that measure highest order derivatives of motion
When IMU part of sensor payload, integrates
Advantages, compared to
Dynamic Modeling
Difficult to derive precise vehicle/environment dynamics
Vehicle modifications require new derivation
CPU cost (large state vector to capture dynamics)
Statistical Modeling (commonly used for target tracking)
Motion statistics unknown/uncertain
State Update: Asynchronously when new measurements available.

8. Indirect Kalman filter - Formulation
Quantities of interest
State vector
Estimated State vector
Error state vector
Propagation
Continuous time error state propagation
Covariance propagation

9. Indirect Kalman filter – Update (1 time instant)
Measurement is a function of state vector at a certain time instant
Position (GPS, UHF link, DSN)
Orientation (Compass, sun sensor)
Linear Velocity (Doppler radar)
Observer
(Estimator)
Controller
H MATRICES FOR CATEGORY 1

10. Indirect Kalman filter – Update (2 time instants)
Measurement is a function of state vector at more than one time instant
Estimated Rotational & Translational Displacement (Relative State Measurement)
Visual odometry (mono, stereo)
Laser scan matching
Kinematics-based vehicle odometry

11. Example: Weighted Laser Scan Matching
Relative position and orientation measurement inferred by correlating sensor measurements recorded at 2 separate locations.

12. State Estimation & Relative Pose Measurements
Propagation
Sensor Model
Proprioceptive
Measurements
(“continuously”)
Update
Sensor Models
Exteroceptive
Measurements
(intermittently)

13. Previous Approaches I
1. Approximate as higher order derivatives

14. Previous Approaches II
2. Approx. as absolute state pseudo-measurement
[Hoffman, Baumgartner, Huntsberger, Shenker ’99]
3. Estimate relative states instead (2 estimators)
FILTER

15. Stochastic Cloning –Kalman Filter (SC-KF)
Relative State Measurement
Relative State Measurement Error
Augmented State Vector

16. SC-KF Propagation Equations
State Propagation
Augmented Error State & Covariance
Augmented Error State & Covariance Propagation

17. SC-KF Update Equations
Residual Covariance

18. Estimation Block Diagram (Helicopter)
Estimators
Sensors
Camera
IMU
3 accelerometers
&
3 gyroscopes
Laser
Altimeter
SC- KF
Inertial
Sensor
Integrator
Kalman
filter
Visual Feature
Tracking
pixel images
distance to features

19. Preliminary Results – Experimental Setup
Average absolute errors in p = [x y z]:
IMU alone [ ] mm
(not shown on Fig. due to errors magnitude)
VISION alone [ ] mm
KF: IMU & VISION [ ] mm
simulated
planetary
surface
helicopter E-Box

20. Preliminary Results - W/out sensor fusion

21. Preliminary Results - Altitude & Bias Estimates

22. Experiments w/ Mobile Robot I
Wheel Odometry and Weighted Laser Scan Matching

23. Experiments w/ Mobile Robot I
Total Distance: mm
Average Distance Errors
Odometry: 258 mm
WLSM: mm
SC-KF: mm

24. State Covariance - Simulation

25. Ongoing & Related Work
Treat time delays of vision algorithms (e.g. visual odometry)
SC2-KF (3 copies of the state)
Detect kinematics-based odometry errors
Slippage Estimation
Smoother – Trajectory Reconstruction
Attitude estimation between consecutive stops of the rover

26. Ongoing Work - *Unresolved Issues*
Sensor Alignment
Determine 3D transformation between pairs of sensors
Must be accurate to correlate sensor measurements w/out errors
Tedious & time consuming process when done manually
Active Sensor Alignment
Determine motions that excite all d.o.f. and allow sensor network on the vehicle body to self-configure

27. Extensions & Future Work
Extension to Simultaneous Localization And Mapping (SLAM)
Incorporate, update, and enhance previous maps of area
Satellite imagery, EDL
Challenges: Computational complexity O(N2)
Proposed solution: FWPT compression of covariance matrix P
Fault detection and identification
Structural damages
Sensor failures
Distributed state estimation
Reconfigurable, mobile networks of robots & sensors

28. Acknowledgements DARPA, Tactical Mobile Robot Program (JPL)
Cog: Robert Hogg, PI: Larry Matthies
NASA Ames, IS program (JPL)
“Safe & Precise Landing,” Cog: Jim Montgomery, PI: Larry Matthies
NASA Mars Technology Program, (JPL)
“Navigation on Slopes,” Cog: Dan Helmick, PI: Larry Matthies
“CLARAty”, PI: Issa Nesnas
University of Minnesota (UMN), GIA program
PI: Stergios Roumeliotis
NSF, ITR program (UMN)
PI: Nikos Papanikolopoulos
NSF, Ind./Univ. Cooperative Research Center (UMN)
PI: Richard Voyles