Robotics Stack Exchange is a question and answer site for professional robotic engineers, hobbyists, researchers and students. It only takes a minute to sign up.
Sign up to join this community
I'm working with a differential-drive robot that has odometry measurements from wheel shaft encoders and heading information from an IMU (I'm using BNO055 in IMU mode to get Euler angles, primarily the heading angle).
I'd like to use the IMU header angle to augment the odometry which is prone to slipping and other errors, but the IMU lags the odometry by up to 100 ms.
How can I combine these measurements to get the best estimate of the robot pose?
Thanks for any word on this.
Combining two sensor outputs that should theoretically give you the same information is called "sensor fusion".
The easiest way to go about this is with a tool known as a complimentary filter. The complimentary filter uses a "blend" value, between 0 and 1, to produce the output:
filterOutput = blend*sensor1 + (1-blend)*sensor2;
Typically you have the sensor that is more reliable short term weighted much more than the sensor that is more reliable long term. Blend values around 0.95 are common. If you think both sensors are equally reliable, then set blend to 0.5. This simply averages the two.
In your case you know odometry is unreliable long term because of wheel slip and IMU short term because of delays, so I would weight odometry 0.95 and IMU 0.05 to start. See how that works and adjust as necessary.
You're robot must be moving pretty fast for this delay to cause problems.
Use a circular buffer to store the odometry readings, so you'll have a record of the last 100ms of odometry readings.
Use the IMU and odom combined at time $T_{-100ms}$ to estimate your last known 'accurate' state. Integrate the odom forward from $T_{-100ms}$ to $T_{0}$ to get your current state. Repeat this at every time step.
If your control can handle the delay, don't bother with integrating the odom. Just use your accurate state 100ms ago.
As the other answers suggest, 100ms of delay in control may not be significant to your application. It would be prudent to first solve your data-fusion problem and then see if the delay is an issue to your controller. I would first record some data of a closed-loop path and do some offline filtering to see if the results are good. For instance, make your robot drive a square and record that data.
Your first problem is then to synchronise the data streams; since your IMU is lagging by 100ms, then simply operate on the newest IMU data you have, with the odometry from the corresponding time.
The data-fusion could be done using a Kalman filter for the 2D case (X,Y,heading). The heading is updated by the IMU and the velocity (and heading, depending on the model) is updated by the odometry. See the system model in this paper, which uses odometry. A good solution would involve a nonlinear KF such as EKF, but I suspect for low speeds and high sensor rates you can get away with a linear KF. I think the states would be $\mathbf{[x, y, \phi, x', y', \phi']}^\top$, where your process model would be the same as the first paper I linked, and the measurement model is simply $\mathbf{y}= \mathbf{u} + v$, where v is Gaussian white noise with known standard deviation. Maybe see this kalman filter framework.
After you have some working results with your offline data, you can worry about implementing it online. The implementation will greatly depend on your choice of data-fusion algorithm.
See also: particle filter methods and a full ROS implementation.
