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 affected 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.

The state estimator requires the GPS and IMU messages to be correctly timestamped with respect to the system clock. The timesync procedure for the physical AutoRally platform can be found in the AutoRally Operating Procedures document. Before starting the state estimator on a physical platform verify proper timesync by inspecting timing information in ChronyStatus, IMU, and GPS diagnostic messages using the OCS. In general, the messages should be green if the synchronization is working correctly. If timesync is not working correctly, refer to the Operating Procedures for further debugging information.

In simulation, you must change the following parameters in the launch file to the values given:

• InvertY = false
• InvertZ = false

and set a parameter on the parameter server:

rosparam set /gps_imu/FixedInitialPose true

Launch the stateEstimator while the platform is stationary at your track origin with the following command:

roslaunch autorally_core stateEstimator.launch

This launch file is configured to read 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 data from the vehicle with accelerations for the estimate to converge. Accelerate forward, brake, and corner to 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 will likely drift. To get the estimate to reconverge you will have to manually accelerate, brake, and turn the vehicle just like you did when the state estimator was started.

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.