Universal orientation, gravity and motion acceleration sensor

We have seen a number of fusion possibilities for 2 sensors.

• We have seen accelerometer supported by gyroscope so that gravity and motion acceleration may be reliably separated. See this Droidcon Tunis presentation for more details.
• We have seen accelerometer supported by the compass to achieve the same goal. There were some limitations that made this solution less flexible than the one based on accelerometer-gyroscope fusion.
• We have seen the compass supported by the accelerometer to compensate for the tilt of the device.

Time has come to put all of these together and to take a step toward the Holy Grail of sensor processing. We want a sensor that measures gravity, motion acceleration and orientation reliably, no matter what the circumstances are. If we achieve this, then we have a good basis for the much researched in-door navigation problem because the linear acceleration may be used for step counting while the orientation can be used to calculate the vector of movement. Errors will accumulate but if we have occasional reference points like WiFi beacons, reasonably good results can be achieved.


So what prevents us realizing this vision? We already obtained a pretty reliable motion acceleration estimation from the accelerometer-gyroscope fusion. The orientation is the thing we don't have. Using the compass-accelerometer fusion we can eliminate the tilt problem. If we throw in the gravity calculation from the accelerometer-gyroscope fusion and use the gravity estimation instead of the raw accelerometer data for the compass tilt compensation then we have also compensated the motion acceleration. The only thing we don't have is the effect of external magnetic fields which may fool the compass and can be pretty nasty in-door in the presence of household appliances, large transformators or other electric machinery. That we will compensate with a compass-gyroscope fusion.


The idea of the compensation of the external magnetic field using the gyroscope is very similar to the compensation of the motion acceleration. If we assume that no external magnetic field is present (beside the Earth's magnetic field), we just use the compass data (and then post-compensate against tilt by the gravity vector which needs accelerometer-gyroscope fusion). If we suspect that there may be external magnetic field present, we use the gyroscope rotation data to rotate a previous, trusted compass vector (that is supposed to measure only the Earth's magnetic field) and use that as compass estimation. The difference between this compass estimation and the measured compass value is the estimation for the external magnetic field.


The trick is the detection whether an external magnetic field may be present. In this prototype the same trick was used as with the accelerometer: if the length of the compass vector is too long or too short compared to the reference measurement, then we have an external magnetic field to compensate.

Here is the summary of the sensor fusions used in this prototype:

• Accelerometer-gyroscope fusion provides gravity and motion acceleration.
• Compass-gravity fusion provides compass tilt compensation. The gravity is already a derived measurement coming from the accelerometer-gyroscope fusion.
• Compass-gyroscope fusion provides orientation and external magnetic field.

Click here to download the example program.

The example program starts with a complex stationary and dynamic calibration process. When stationary, it measures the reference gravity and magnetic field values. In this stage it is very important that the device is not subject to motion acceleration or external magnetic field (beside the Earth's magnetic field). In the dynamic calibration phase we calculate the bias of the compass. We do this by instructing the user to rotate the device and measuring compass vectors in each special position. These are supposed to be points on a sphere. Once we collected the measurement points, we calculate the center of the sphere by solving a set of linear equations. The Jama package was incorporated into the example program for matrix manipulations.

Once the calibration is finished, the algorithm outlined above is executed. The measurement data is visualized in a 2D coordinate system - the z coordinate is not drawn. This is just my laziness, I did not take the effort to implement a 3D visualization engine. It is important to note, however, that the motion acceleration (yellow) and the external magnetic field (cyan) vectors are really 3D, they are just not drawn as such. The white vector is the final orientation compensated against tilt, motion acceleration and external magnetic field.

The prototype does have limitations. While it survives pretty nicely short-term disturbances from external magnetic fields (try with a small magnet), in longer term (e.g. after 5-10 minutes) it gets properly lost. For example when riding the Budapest underground there are strong and varying magnetic fields generated by the train's traction motors during the entire journey. If the compensation algorithm picks up a wrong reference vector, it may get lost for the rest of the journey until it is able to pick up the Earth's undisturbed magnetic field.