There are a variety of sensor fusion algorithms out there, but the two most common in small embedded systems are the Mahony and Madgwick filters.
Mahony is more appropriate for very small processors, whereas Madgwick can be more accurate with 9DOF systems at the cost of requiring extra processing power (it isn't appropriate for 6DOF systems where no magnetometer is present, for example).
We will use the Mahony fusion algorithm in this example since it is the most relevant to a wide variety of devices, but it's easy to switch to Madgwick if you prefer to test another algorithm.
MahonyAHRS and MadgwickAHRS Libraries
For best results, use the fork of MahonyAHRS and MadgwickAHRS from PJRC (based on the original Arduino libraries) available here:
You will need to download these as a zip file and then install them in your libraries folder, or git clone them in the Arduino libraries folder (ex. 'git clone https://github.com/PaulStoffregen/MahonyAHRS.git').
For help installing libraries, see our Arduino Libraries Learning Guide.
Enter Calibration Data into ahrs_mahony
To get started, open the ahrs_mahony example from the Adafruit_AHRS folder.
You will need to enter the magnetometer calibration values calculated earlier in this guide in the appropriate field, as shown in the screenshot below:
These values are based on the following calibration data:
Run the Sketch
Next, compiled and run your sketch.
Open the Serial Monitor, and you should see a millisecond timestamp, followed by the output of the sensor fusion algorithm, which will give you Euler Angles for Heading, Pitch and Roll in that order:
Tuning the Filter
One particularity of fusion algorithms (and most DSP algorithms) is that they are sensitive to timing. Make a note of the millisecond timestamp before the individual samples, can count approximately how many samples per second your device is outputting.
You need to tell the fusion algorithm how many samples per second we are generating to more accurately estimate positions in 3D space based on the accel, gyro and mag data.
In the filters setup() function, find the following lines and adjust this value to match the number of samples per second your system is outputting:
// Filter expects 50 samples per second filter.begin(50);
Run the filter again and you should get slightly more accurate results.
Switching to Madgwick
To switch to the Madgwick filter, simply comment the appropriate class instance at the top of the sketch and run the code again. The two filters have identical APIs and are easily interchangeable.
Change the code from this ...
// Mahony is lighter weight as a filter and should be used // on slower systems Mahony filter; //Madgwick filter;
... to this:
//Mahony filter; Madgwick filter;
A Note on Orientation Values
Please note that at present all orientation values are relative and not absolute. What this means is that the orientation values will be relative to the starting position of the device, not absolute magnetic north or south, etc.
Before starting, put your device in a known, repeatable position if you require reasonably repeatable, consistent output.
Page last edited March 08, 2024
Text editor powered by tinymce.
