# How to track robot position?

I'm a software researcher, who in my spare time mentors a robotics team, helping on the software side of things. For years, I keep coming back to the same question: How to determine the robots position, and heading during our competitions? We have tried a number of things with varying degrees of success/failure. Encoders on the drive wheels, accelerometers, gyroscopes, etc. I recently bought an IMU with a 3 axis accelerometer, 3 axis gyro, and 3 axis magnetometer, all preprocessed by an Arduino, and outputting the value to a serial port. I thought surely there must be a way to take all these measurements and get a composite view of position and heading. We are using Mecanum wheels on this particular robot, so wheel encoders are not particularly useful. I've looked around and there's a lot of talk about orientation using quaternion with sensor fusion using similar boards, but it very unclear to me how to take the quaternion and the estimation and come up with x,y distance from the starting position? Now, my time window for these measurements is small, ~15 seconds, but I need it to be pretty accurate within that window. I'm about ready to abandon using the IMU, and try something else. One idea is to use a USB ball mouse to try and track robot motion but I'm certain that the mouse is going to get banged around way too much leading to noise and invalid results. As a side note: The robot is about 2ft x 3ft base weighing in at 120 lbs. Any thoughts or suggestions appreciated.

• Have you tried a kalman filter?
– Paul
Feb 23, 2015 at 22:06
• I've read about Kalman filters, but have never written one. Feb 23, 2015 at 23:36
• If you have ~\$1600 to spend, Analog Devices has a IMU with an embedded Extended Kalman Filter (I do not work for AD). link Feb 24, 2015 at 1:26
• I think that any mouse would measure the same kind of motion a ball mouse would. Is there any particular reason it should be a ball mouse? One with a red LED might get less noise due to physical jostling. Both would require physical contact (or near to it) with the road's surface. Mice might be good at linear translation, but they would likely miss out on rotational movements. Feb 26, 2015 at 17:56

How to estimate a robot's position depends on how well you'd like to estimate it. If you just need a rough guess, try odometry, it works OK. For better results, you have to incorporate more sensors. That's an incremental process that involves a lot of sensor fusion, and suddenly, you've built an Extended Kalman Filter.

The best way, in my opinion, is to use each sensor to form it's own estimate. Then, take a weighted average of the resulting estimates. The weights correspond to the certainty associated with each estimate. This, in essence is the Kalman Filter. What is missing is how to estimate the certainties. This is the hard part of the KF. Try some ad hoc values as a start. You'd be surprised how well this can work.

This is addressed as a first-principals question in any major robotics textbook and even here, on this site.

On this site, we have addressed many questions related to this problem.

and many more I've missed.

But honestly, these are heavy-handed approaches. You must understand the basics (estimating position from odometry, for example) to understand the rest.

• At a high level, I do understand the issues involved. And have tried this with various robots. We've had encoders, and calculated speed, and distance traveled but found that slippage, and pushing from other robots distorted things. We've done the double integration of an accelerometer, and found the compounded error pretty quickly makes the value useless. We've had a gyro and found the drift of the relatively cheap components we used to be an issue. So I bought something a little more expensive hoping that it might be better. Thanks for the links, I'll take a look Feb 24, 2015 at 23:08
• Hi, I didn't mean to imply you didn't. Just that each sensor gives an independent estimate of position, and surprisingly, by fusing them, you can get a combined estimate which is better than the best. This is the basic concept that needs to be used if no individual sensor is good enough. Feb 24, 2015 at 23:10
• Agreed, and was the genesis of my question. How to fuse them, and get a better result. Bottomline is that how to derive the motion model, and covariance matrix is what I don't fully understand and why I've never gone down the path of implementing a Kalman filter for our robots. Even just grabbing some code isn't going to help if I don't understand how to tune the gains on the filter. Having an IMU that has an integrated Kalman filter might be a better solution. I did see some other alternatives to Kalman, Madgwick's IMU/AHRS algo but that didn't look like it was what I wanted Feb 24, 2015 at 23:40
• The best way, in my opinion, is to use each sensor to form it's own estimate. Then, take a weighted average of the resulting estimates. The weights correspond to the certainty associated with each estimate. This, in essence is the Kalman Filter. What is missing is how to estimate the certainties. This is the hard part of the KF. Try some ad hoc values as a start. You'd be suprised how well this can work. Feb 24, 2015 at 23:45

I am not allowed to comment, so I have to add a reply. By position, do you mean the location in space (so X, Y coordinates), or orientation (tilt, etc)?

If position, you can use the accelerometer values and integrate acceleration to get distance traveled, though this is fairly inaccurate. We have tried to do this for a quadcopter, and the drift due to error is quite large.

You can use an accelerometer with a gyroscope together, and a Kalman filter to get a better idea for how far the robot has moved in each direction. Here is a previous discussion on this topic.

The tilt you can calculate directly from accelerometer values (but filter them, even an FIR filter would work ok).

Heading you can get from the gyroscope and/or the magnetometer. The gyroscope detects rotation, so make sure it is somewhere near the center of your robot (or whereever the axis of rotation is).

I hope that helps. I have to go teach, but I'll come back in an hour and add some more info.

• I have looked at various documents on Kalman filters and this seems to be the general answer for reducing the noise in the accelerometer, but I've never written code for such a filter, and I'm not sure where to begin on integrating the other sensors. It's not hard to see that just integrating the accelerometer twice will be pretty awful in the noisy, vibration filled environment of our robot. Similarly, I would think that taking the other sensors into account would reduce the error, assuming proper calibration at the start. Primarily we just need x,y coordinates, and a directional heading. Feb 23, 2015 at 23:36
• @MichaelCoss: There is a lot of software available that implement kalman filters for you. The main difficulty for the uninitiated novice is developing an appropriate motion model and characterizing the covariance in the sensor model.
– Paul
Feb 24, 2015 at 3:14
• @MichaelCoss, there is Arduino code available online for Kalman. Here is a nice discussion of tuning a Kalman., and Arduino code for gyro+accelerometer and guide here. You might have more luck if you look at Kalman filters for quadcopter code - it's pretty popular.
– Mewa
Feb 24, 2015 at 4:24
• @MichaelCoss, also for calculating heading with magnetometers, however I think motors & tilt in your robot will affect the accuracy of this measurement. In that case you might want to use an accelerometer to measure whether the robot is level (calibrate it to some level), and then adjust your magnetometer readings to that.
– Mewa
Feb 24, 2015 at 4:30