sixD,The AHRS as is, doesn’t do 3D correction, adding Merlins code and Mag Cal correction data supposedly (before anyone jumps on me) makes the ellipsoid a circle and centres it in the 3 planes, BUT l haven’t found this to be the case. I asked Merlin to confirm if the coding in that example on the under water blog was correct but he never answered. My Guru, Jack Edwards has gone through and checked the way l was implementing the correction formula and it is correct in the example given. But we both found that it doesn’t make the compass heading as tilt compensated as we all hoped and there is no use in using it.
Much later l found Merlins coding correction l was after all along was already on Pololu web site (ironicly, no one new that’s what it did) as the Pololu Open IMU, but as l say (please test it) it drifts rapidly and Mike Baker hasn’t answered a question re this(l have tried changing everything l could in the code without success).
I was after a < ± 2 dynamic accuracy to bring it inline with the bought sensors. I believe the only way to get this is with a gimbal, in Sydney you don’t have as bad mag field tilt as down here and hence you will get better results than l. I am pessimistic of how well the commercial units work and would love to have one and thought l would have to pay the $A700-1200(l did buy one, see later comment) to get the accuracy l thought l needed, but V3 and gimbal is giving me the accuracy l am after. But a Auto Pilot heading sensor (which is what l am after) just really needs to be accurate, in a worst case, swinging ± 30 from a set heading with as little tilt error, as in rough water the boat is jumping around a lot anyway.
If l was after the best heading sensor without paying the big bucks, l believe you won’t get a better unit than the one from Kris l posted.
I bought a commercial unit from a AP builder/supplier that they said would be <± 1, when l tested it, it was ±8 (this was static testing let alone dynamic). They then retested there units with the same result. The R+D manger is not the manager anymore. They gave me a refund. Hence my pessimism about commercial units accuracy.
So what sort of accuracy are you after, the gyro is what gives a stable heading but if it is turning when you start the IMU it computes the ROT into the start cal and then the heading can be off, right from the start. Your unit doesn’t have that but it won’t be as good a heading, when you add velocity into the equation, then they use the gyro as the main input and the accel and mag as corrections to the gyro.
It is a interesting subject that l have researched now for 18 months, l might have basic coding skills but have looked into a lot about IMU’s, and you get what you pay for, $35 chip against $1200 like Airmar H2183 which all others are graded against.
The issue with Merlins approach is that you are correcting the Mag and you need to do the same for the accel (which l did) but the chip is the inaccurate part, you can cal it all you like, but it has inbuilt inaccuracy hence why if you want accuracy you have to buy a better quality chip like military grade that you can’t get anyway at $50K
http://www.vectornav.com/support/library/imu-and-ins have a read re accuracy and you see what we are up against.
Below is an example of the code you are after (change all variables to your Magneto data). The screen print you have uploaded is the correct way to implement Merlins corrections (also look at Pololu Open IMU)
#include <Wire.h>
#include <LSM303.h>
LSM303 compass;
const float alpha = 0.15;//default 0.15, higher number less filtering=faster response
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
}
void loop()
{
compass.read();
float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;
// Accelerometer calibration
//AP/all data/data on trip/16166
Xa_off = compass.a.x + 295;//pitch
Ya_off = compass.a.y + 287;//roll
Za_off = compass.a.z - 717;
Xa_cal = 1.001328Xa_off + 0.000286Ya_off + 0.002095 Za_off;
Ya_cal = 0.000286Xa_off + 0.984450Ya_off + 0.000302Za_off;
Za_cal = 0.002095Xa_off + 0.000302Ya_off + 0.985394*Za_off;
//Xa_off = Xa_off0.33;
//Ya_off = Ya_off0.33;
//Za_off = Za_off0.33;
// Magnetometer calibration
//bris river magneto , 3205
Xm_off = compass.m.x + 216;// correct should be default(- bias)= - (bias) -216 = +216 // magneto b = -216 //(100000.0/1600.0) - 8397.862881;
Ym_off = compass.m.y + 208; //(100000.0/1600.0) - 3307.507492;
Zm_off = compass.m.z - 334; //(100000.0/1600.0 ) + 2718.831179;
Xm_cal = 1.046360Xm_off + 0.078575Ym_off + -0.027728Zm_off;
Ym_cal = 0.078575Xm_off + 1.080417Ym_off + -0.005989Zm_off;
Zm_cal = -0.027728Xm_off + -0.005989Ym_off + 1.108273Zm_off;
// scale mag axis
//Xm_cal = Xm_cal1.45;
//Ym_cal = Ym_cal1.3;
//Zm_cal = Zm_cal*0.5;
// Low-Pass filter accelerometer
fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
fZa = Za_cal * alpha + (fZa * (1.0 - alpha));
// Low-Pass filter magnetometer
fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));
// Pitch and roll
roll = atan2(fYa, sqrt(fXafXa + fZafZa));
pitch = atan2(fXa, sqrt(fYafYa + fZafZa));
roll_print = roll180.0/M_PI;
pitch_print = pitch180.0/M_PI;
// Tilt compensated magnetic sensor measurements
fXm_comp = fXmcos(pitch)+fZmsin(pitch);
fYm_comp = fXmsin(roll)sin(pitch)+fYmcos(roll)-fZmsin(roll)*cos(pitch);
// Arctangent of y/x
Heading = (atan2(fYm_comp,fXm_comp)*180.0)/M_PI;
if (Heading < 0)
Heading += 360;
//Serial.print(“Xa_off”); Serial.print(Xa_off); Serial.print(" “);
//Serial.print(“Ya_off”); Serial.print(Ya_off); Serial.print(” “);
//Serial.print(“Za_off”); Serial.print(Za_off); Serial.print(” ");
//Serial.print("compass aX “); Serial.print(compass.a.x); Serial.print(” ");
//Serial.print("compass aY “); Serial.print(compass.a.y); Serial.print(” ");
//Serial.print("compass aZ “); Serial.print(compass.a.z); Serial.print(” ");
//Serial.print("Xa_cal “); Serial.print(Xa_cal); Serial.print(” ");
//Serial.print("Ya_cal “); Serial.print(Ya_cal); Serial.print(” ");
//Serial.print("Za_cal “); Serial.print(Za_cal); Serial.println(” ");
//Serial.print(“Xm_off”); Serial.print(Xm_off); Serial.print(" “);
//Serial.print(“Ym_off”); Serial.print(Ym_off); Serial.print(” “);
//Serial.print(“Zm_off”); Serial.print(Zm_off); Serial.print(” ");
//Serial.print("compass mX “); Serial.print(compass.m.x); Serial.print(” ");
//Serial.print("compass mY “); Serial.print(compass.m.y); Serial.print(” ");
//Serial.print("compass mZ “); Serial.print(compass.m.z); Serial.print(” ");
//Serial.print("Xm_cal “); Serial.print(Xm_cal); Serial.print(” ");
//Serial.print("Ym_cal “); Serial.print(Ym_cal); Serial.print(” ");
//Serial.print("Zm_cal “); Serial.print(Zm_cal); Serial.println(” ");
Serial.print("Pitch (X): “); Serial.print(pitch_print,0); Serial.print(” ");
Serial.print("Roll (Y): “); Serial.print(roll_print,0); Serial.print(” ");
Serial.print("Heading: "); Serial.println(Heading,1);
delay(100);//default 250
