# Accelerometer, gyro, and magnetometer sensor fusion in 2d

I have not yet built this so this is basically a theoretical question. I am still wrestling with some C code to manage i2c communication etc.

When I originally said "I have not built this", I meant that the robot is in what could be called a "design phase". For the sake of my question lets assume for a moment that the whole robot consists of just one IMU sensor. It moves magically (no motors that create a lot of noise in the sensor measurements). With theoretical I mean I am interested in the mathematics and algorithms involved in solving this problem. What I call IMU sensor provides raw accelerometer, gyro, and magnetometer measurements.

Lets say our tiny robot travels on a snooker table (3569 mm x 1778 mm). I believe this is sufficiently small to call it 2d. **Now, sensor fusion should be much easier (faster, consume less resources) than in 3d, right? **

I would like to measure the velocity and if possible the position. With velocity I mean at a given point of time I need to know the current velocity of the robot moving over the snooker table. Velocity is in the range of 0 - 5 m/s. With position I mean at a given point of time I need to know the current position of the robot on the snooker table (x, y, heading).

I hope this will be possible since the robot should be able to identify some landmarks and use this information to reduce position errors.

When I originally said "I hope this will be possible" I meant to express that I already am aware of the fact that real sensor data is noisy and errors accumulate quickly. Using landmarks I will / or will not be able to manage to reduce the error in the position estimates. But this is NOT part of my question.

I am about to improve my linear algebra knowledge. So I am confident to manage some matrix multiplications, inversions and such.

My question is for some ideas, references on measuring velocity and position in 2d using an IMU sensor like this one.

P.S. A little side question: I just figured that this question is probably too theoretical for robotics.SE. If you know any forum that is more focused on mathematical / algorithmic side of robotics please let me know.

• If you're just using one sensor (the IMU), there is no fusion -- you just use the values that the IMU gives you. An IMU just integrates the values from an accelerometer to get velocity, and then integrates that to get position (with errors accumulating in each). – Ian Apr 29 '14 at 14:22
• the kind of IMU I am talking about provides only raw accelerometer, magnetometer and gyro data (pololu.com/product/2468). I assumed that other IMUs would do the same. I am convinced that fusion for the raw sensor data is very application specific (given that you have different applications). – tired_of_nitpickers Apr 29 '14 at 17:49
• OK, I've updated the question to better reflect what you're after. You're right that it can be very application specific. – Ian Apr 30 '14 at 18:25
• @Ian - I'm sure there is fusion going on here. You have the gyro + compass ( + wheel odometry ?) fusing to give a measure of the robot's heading. There's also the accelerometer + position sensing ( + wheel odometry ?) to give a measure of the robot's position. – Rocketmagnet Apr 30 '14 at 19:47
• Mark, when you say "I would like to measure the velocity and if possible the position." what do you mean? Do you mean that you'd like to have some other sensor(s) apart from the IMU which can measure the velocity and position of the robot. Or do you mean you'd like to infer the velocity and position entirely from the IMU data? – Rocketmagnet Apr 30 '14 at 19:49

The gyrometer gives you angular velocity about each axis. You simply integrate these values to get the roll, pitch and yaw of the robot. Since this is 2D, all you care about is yaw, and you'll integrate one value. Of course, there are many different ways of integrating the value you read from the gyrometer. The easiest way is to sample the gyro, timestamp the sample, and assume this value holds until the next sample. This is similar to a Riemann's sum approximation of the integral, and will be more accurate the higher your sampling rate. You can use more advanced forms of interpolation between sets of samples like bilinear interpolation, but it's up to you to decide how far you'd like to go.

The accelerometer gives you linear acceleration along each axis. Since this is 2D, we only care about the acceleration along x & y (I assume x is pointing in the direction of positive velocity of the vehicle, y is pointing to the left, and z is the normal of the table). Again, we simply integrate these values to get velocity, and if you so choose integrate a second time to obtain position. Since you're integrating twice, you'll accumulate a significant amount of error. Error in the integration accumulates as a random walk (see: http://www.mit.edu/~kardar/teaching/projects/chemotaxis(AndreaSchmidt)/random.htm). This means error grows with time on the order of sqrt(t). Both the first and the second integration will have this random walk, which will compound your error further.

The magnetometer is there to provide a constant reference (magnetic north) which compensates for the drift of the gyroscope.

Essentially, you use the magnetometer and gyroscope to calculate your heading in the local frame (a static frame that has (0,0) located at the position where you started the robot). The accelerometer gives you changes in position in the vehicle frame (dynamic frame whose (0,0) origin is anchored somewhere on the robot). You can use the updated heading and changes in position in the vehicle frame to compute your location in the local frame.

Here's an article I think you will find very useful: https://forum.sparkfun.com/viewtopic.php?f=14&t=24226

Edit: I should mention that to do this, all you need are encoders on the wheels to compute position and velocity and a one axis gyro to measure heading. Then you simply use trigonometry with the change in distance computed from the encoders and the angle from the gyro to get the change in x & y. The best gyrometers can be used for hours upon hours without a significant amount of drift. Adding the magnetometer is a big help as well. Ideally you'd use the IMU as an "Attitude Heading & Reference System" and encoders to compute distance travelled and as another source of velocity. If you wanted the most accurate velocity, you'd fuse the data from the encoders with the integral of acceleration by adding the data together after weighting each data point by 1/variance of the data.

Using an IMU you can only measure: acceleration, rate of rotation, and direction of magnetic field. You cannot measure velocity, you can only integrate the acceleration to infer velocity. As you can imagine, this leads to velocity drift, which in turn leads to a lot of unbounded position drift.

There are three parts to your problem:

1. Infer the robot's bearing from gyro and compass. If the robot is quite slow, you may be able to ignore the gyro, and just use the compass.
2. Infer the robots velocity and position from the accelerometer

The mathematics for 2. is very simple. if dT is the time between samples, then:

accelerationInWorldCoordinates = accelerationInRobotCoodinates.rotate(-heading)
velocity += accelerationInWorldCoordinates * dT
position += velocity * dT


The mathematics for 1. is a little more involved, because we're fusing together the gyro and compass. Here's something to try:

k = 0.95