Accelerometer, gyro, and magnetometer sensor fusion in 2d

Question

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.

Answer 1

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.

Answer 2

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
3. Adjust your position estimate from 'landmarks'

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
headingDegrees += gyroReadingDegreesPerSecond * dT
headingDegrees  = (headingDegrees*k) + (compassReadingDegrees*(1.0-k))

What's happening here is that we're mainly using the gyro to update the heading, but mixing in a little of the compass reading to help keep it stable in the long term.

The mathematics for 3. (and possibly 1) is harder. What you are looking for is a Kalman Filter. This filter can take information from several sources and produce an optimal estimate of the state of the robot, along with a measure of its confidence in that estimate. It's not unlike our code for 1. In fact, you'll see the k and (1-k) in the Kalman filter too.

The problem with Kalman filters is that the only documentation you'll find on the subject tends to be heavy on the maths, and very low on practical advice for people actually implementing it.

The way I learned was by very slowly and carefully picking my way through Kalman Filter Tank Filling from Cornell University. I recommend implementing the examples given and trying to make them work. What's annoying about this paper (and most others), is that they often don't tell you the size and shape of the various matrices used in the algorithm, so you'll have to work these out by hand. They also don't tell you much about the meaning of the covariance matrix, or how to calculate it.

I have this dream that one day I'll get round to writing a genuinely helpful article on Kalman filters on my blog, but don't hold your breath.