Before the first run the accelerometer has to be calibrated. The easiest way is to measure maximum and minimum for each axis, but with only gravity applied (no dynamic acceleration). Then gain and offset are then calculated for each axis. ADXL335’s has by less than 1% over years and harsh treatment. With the offset it is a bit worse (after vibration tests and switch to a new system) …
Corrected acceleration will be calculated as
With magnetic field, situation is a bit more complicated, especially for 3 axes. It is similar to the accelerometer but besides gain (and offset – depends on a sampling method) the measured magnetic field has to be compensated for hard and soft iron distortion. For some more information, you can read for example diydrones, memsblog or others.
Hard iron causes additional offset, so center of the sphere (envelope of ideal magnetometer’s measured Earth magnetic field) is moved. Soft iron causes some other and more severe deformations. Essentially it changes gain for each axis and rotates the original sphere. As a result you get an ellipsoid, which is moved and rotated. Something like in the pictures below.
Now, elements of soft iron correction Matrix GRPY and hard iron offset vector ORPY have to be found. But this is just a beginning. The calibration has to be calculated in the embedded system, with data which are burdened by noise, in three axes and real-time.
The algorithm was originally targeted for the quadrocopter, so it means for limited processing power (of one 32bit Coldfire V2 MCU at 64MHz) and very limited memory. For the reasons stated above (weighted) least squares method was selected. It enables correction calculation with reasonable dataset where sphere/ellipse does not have to be covered completely, just fundamental axes and areas. This speeds up the whole process.
Advantages are already known, the disadvantage is it has problems to calculate parameters for magnetic field heavily distorted by soft iron, but only for rotation components of the matrix (no amount of axis gain is a problem) and when the measurement is noisy. In case of the quadrocopter the most significant error was gain for each axis, followed by the offset. Rotation part was small. The calibration thus worked perfectly.
Here is some real data:
The magnetometer data was measured in the quadrocopter with all engines running. In the picture above you can see measured and uncompensated Earth magnetic field compared to the compensated. Red line represents Earth magnetic field amplitude. The lower part, compensated magnetic field, is normalized because of the nature of the algorithm. So, actual gain has to be calculated from the measured amplitude (by weighted mean (if sensor’s gain is 1) or from Earth magnetic field model) and put to the GRPY matrix and ORPY vector.
The picture below shows how few points is needed to calibrate the magnetometer when there is no strong rotation component present. The picture is for the same measured data as the picture above.
Here is a simulation of noisy magnetometer measurements with strong offset and “asymmetrical” gain components. The video shows in real-time how the algorithm works. Black points represent measurement, red calibrated data.
Least Squares Algorithm
Go to download section to see m-file with the algorithm (simplified example).
The algorithm is based on a simple ellipsoid (not rotated) equation.
Two magnetic vector readings are subtracted to make the equation more suitable for least squares. And GR is considered 1.
Now, we can define vector B to estimate O and G coefficients. Roll axis is considered output Y.
Then LS can be calculated as
So as GR is 1, the calibration does not keep correct magnetic vector length. But this is not important as magnetic vector is normalized before entering INS anyway.
Before entering calibration calculation input magnetometer data have to be filtered out (as a replacement of weight matrix W in weighted least squares), this step is important to prevent having many points close to one location when other locations are empty. This would reduce algorithm accuracy. There are many ways how to implement the filter. One way is shown in the example m-file.
Corrected magnetic filed vector can be then calculated as
The leasts squares algorithm has one big advantage, the matrices GG = GTG and GY = GTY can be updated every time a magnetic vector is read in a way that their size remains 5×5, instead of creating big G and Y with all measurements and then calculating. This makes the algorithm suitable for systems with limited memory and processing power like Arduino.
Then, for every step
GG(j)= GG(j-1) + GT(j)G(j)
GY(j) = GY(j-1) + GT(j)Y(j)
B = (GG)-1GY