State Estimator
This tutorial describes how to run the state estimator released for the AutoRally platform.
The stateEstimator node subscribes to GPS location and IMU measurements (linear acceleration and angular velocity) and produces an accurate, fast state estimate that includes vehicle position and velocity. This estimate is then published, in real time, to the /pose_estimate topic for use by other processes and controllers on the vehicle.
Conceptually, this combination is similar to a Kalman filter in an INS. The IMU provides very high rate information about the change in velocity and orientation which must be integrated over time. Used in isolation for state estimation, an IMU-based estimate will quickly drift from reality because the sensors are effected by noise and a time-varying bias component. GPS provides long-term, drift-free position information, but is slow to update and provides no orientation information.
In simulation, launch the stateEstimator with the following command:
roslaunch autorally_core stateEstimator.launch
This file is configured to read simulated GPS and IMU messages and publish a state estimate. Position and velocity estimate are published to the topic /pose_estimate as nav_msgs/Odometry messages.
The state estimator creates a local Cartesian coordinate system centered at the GPS location of the car when the estimator is started. The origin of this coordinate system (East North Up convention Wikipedia) is the GPS point first recorded when the state estimator is run.
After starting, the estimator needs the vehicle to accelerate for the estimator to converge. Accelerating forward, braking, and cornering give the estimator information about global orientation that cannot be obtained while sitting still or driving at a constant speed.
If the vehicle is stationary for a long period of time, the heading is likely to drift, and you will have to manually accelerate, brake, and turn the vehicle for the estimate to reconverge.
Use Rviz to visualize the nav_msgs/Odometry output from the stateEstimator on the /pose_estimate topic. In Rviz, set the global reference frame for RViz to odom (currently the frame published in the odometry message).
During normal operation, you should see small arrows graphed, following the track you are driving with the vehicle, and point in the direction your vehicle is facing. Sometimes the estimator finds an incorrect local minimum, or diverges for some other reason. In this case, the estimator should be restarted.
When running on the AutoRally hardware, the state estimator requires the GPS and IMU messages to be correctly timestamped with respect to the system clock. This is accomplished through a combination of gpsd and chrony for the system clock and hardware pps input into the IMU. The OCS shows a diagnostic message related to time synchronization, and this message should stay green if the synchronization is working correctly. If this is not working correctly, refer to the operating instructions for further information.