Now we’ve looked at some of the theory behind calibrating the magnetometer outputs, let’s see how we can actually do it. In the last section I said that we had
- n vector equations of the form mi = M ( Ri eb + B ), where M is a matrix that represents both the stretch and the rotation.
And that we need to find the fixed values of M, eb and B that make all these equations most nearly correct at the same time. We can do this with a least squares optimization method, using
- J (M,eb, B) = Σ ( M ( Ri eb + B ) - mi )2
as the cost function and optimizing over all 15 individual parameters in M, eb and B (there are 9 in the matrix M and 3 each in eb and B.) This corresponds to minimizing a negative log likelihood function (I haven’t given it here) under the assumption that all samples have the same standard deviation.
At some point I want to return to this to consider how the errors contribute: the mi measurements can be considered as subject to a white Gaussian noise process, and the sequence of orientation matrices Ri is subject to error which increases in time as gyro drift (which is uncorrected in yaw, during the calibration period) takes it’s toll. Therefore we might expect to get a more accurate answer if the early samples are weighted more heavily than the later ones.
Another option would be to consider the uncorrected gyro drift as a reorientation of the Ri in space over the duration of the sampling, and which appears as a “wobble” in R. We should replace Ri with Ω-1(ti) Ri , where Ω(t) represents the rotation by which the reference frame has drifted by time t and ti is the time of the i’th sample. A fair assumption for the structure of this drift is that each of the body gyro rates has an independent fixed null offset. It would be convenient to be able to linearize as Ωi = I + (i/n).Δt.Ω where Δt is the duration of the calibration period and Ω is the matrix form of a differential rotational element whose nine elements row-wise are [(0, -ω2, ω3), (ω2, 0, -ω1), (-ω3, ω1, 0)] , that is, to assume that the samples are equally spaced in time and that to first order the drift in R is a continuous rotation at a fixed rate about an axis fixed in space. However this would not match the assumption of a fixed gyro drift rate: the aggregate rotation from a drifting gyro is strongly dependent on the history of the orientation of that gyro. (Eg: a drifting “yaw” gyro shows up as a pitch error if the roll angle is 90 degrees.)
In the mean time let’s go on without considering the errors in the samples, and see what kind of results we get. If we get a decent convergence to an answer then as a check we can determine from the fitted value for eb the angle of magnetic dip (the angle at which the Earth field lines go into the ground) for the location where the calibration data was collected. And we can compare that with a value from a dip meter, or else look up a value from the world geomagnetic model, via one of a number of websites like the British Geological Survey. We’ll then include the values for M and B in the AHRS code as part of the calibration parameters. For each magnetic measurement thereafter we can calculate the Earth field vector in the Earth frame. Wait, you say, shouldn’t that be a constant? Surely the Earth field doesn’t move? Why measure something that’s constant? Because, to the extent that our measurement appears to move, it indicates an error in our heading angle. Suppose I have a heading of 032. I measure “north” and come up with an answer that points two degrees to the right of my x-earth axis (i.e the angle is 2 degrees instead of zero). I determine from this that my earth-x axis needs to be rotated to the left by two degrees to align it correctly. In actual fact I’m going to insert the angle of the measured north relative to my earth-x axis directly into the Kalman filter as the residual of the heading angle state variable. The Kalman filter will rotate the direction cosine matrix to make the residual go to zero, and that means that the earth-x axis will be tied to magnetic north instead of drifting freely due to gyro errors.
To perform the least squares fit I’ve been using a multi-platform package called R – which bills itself as “a free software environment for statistical computing and graphics”. R gives us a command line environment from which we can load and graph data, and also run least squares optimization queries without having to write any code. Fabulous. So I have written a script for R, that loads a file of data output by the AHRS, and runs through a least squares optimisation procedure to give values of M, eb and B. It also draws some 3D graphs so you can see what the data looks like. It makes the whole ellipsoid thing much easier to imagine when you can see it in front of you! Also the script takes the best values for M, eb and B and works out a posteriori how far each of the individual magnetic measurements (k) would have predicted ebk to be from the overall best value eb - that is, the angular deviation in the z-plane from the mean. Which is an indication of how consistently we would have determined a fixed direction for north (noting, however, that consistency isn’t the same thing as accuracy.)
Let’s take a run-through. Firstly I need to record some data. I set the output mode of the AHRS for the magnetic calibration mode, and make sure to turn off the magnetic tracking (I can’t calibrate the compass if the DCM is already being rotated to match some other inaccurate magnetic calibration).
#define PRINT_JSON 0
#define PRINT_LEVIL 0
#define PRINT_LEVILBARO 0
#define PRINT_POWER 0
#define CALIBRATE_MAG 1
#define CALIBRATE_GYROACCEL 1
#define REORIENT 0
#define IGNORE_MAG 1
Just like for the gyro calibration I use telnet or nc to log the output, which I’m going to call data set A:
Somewhere away from localized magnetic disturbances (i.e. not directly above or near some furniture with iron or steel parts) I rotate the AHRS slowly through a variety of orientations and around a variety of axes, for about 30 seconds, while logging. I copy and paste the data into a text file in a directory next to the script and run it through R.
> # Perform optimisation
> nls.out<- nls.lm(par = pStart, fn = residFun, m = magM, r = R, control = nls.lm.control(nprint = 1, maxiter = 50))
It. 0, RSS = 1.22339e+13, Par. = 1 0 0 0 1 0 0 0 1 -9.72222 -262.489 -308.181 1 0 0
It. 1, RSS = 1.22094e+13, Par. = -0.0744783 0.456607 0.162882 -0.134675 1.45067 0.376037 0.041931 0.13083 0.468529 -9.83206 -262.458 -308.02 0.74496 -0.0085705 -0.283312
It. 2, RSS = 1.2177e+13, Par. = -0.539431 0.965673 0.245232 -0.271866 1.20153 0.827138 0.0667927 0.305385 0.85613 -9.98764 -262.413 -307.788 1.14094 0.0620342 0.0159279
It. 3, RSS = 1.20435e+13, Par. = -2.43952 1.76911 0.61229 -0.54322 1.99737 1.49328 0.137573 0.525754 -0.121397 -10.2024 -262.349 -307.465 1.6774 0.399639 0.0936926.
It. 46, RSS = 5.73868e+07, Par. = -35.271 1.07938 -0.0110288 -0.386153 38.6756 -0.168991 1.43572 -1.80098 -33.6084 -62.3397 -247.288 -232.752 3.79514 -1.75578 12.3681
It. 47, RSS = 5.73868e+07, Par. = -35.271 1.07938 -0.0110153 -0.386153 38.6756 -0.169001 1.43571 -1.80098 -33.6084 -62.3397 -247.288 -232.752 3.79521 -1.75585 12.3681
It. 48, RSS = 5.73868e+07, Par. = -35.271 1.07939 -0.0110145 -0.386158 38.6756 -0.168995 1.43567 -1.80097 -33.6084 -62.3397 -247.288 -232.753 3.79518 -1.75581 12.3681
It. 49, RSS = 5.73868e+07, Par. = -35.271 1.07939 -0.0110058 -0.386159 38.6756 -0.168999 1.43564 -1.80097 -33.6084 -62.3396 -247.288 -232.753 3.7952 -1.75584 12.3681
It. 50, RSS = nan, Par. = -35.271 1.07939 -0.0110058 -0.386159 38.6756 -0.168999 1.43564 -1.80097 -33.6084 -62.3396 -247.288 -232.753 3.7952 -1.75584 12.3681
In nls.lm(par = pStart, fn = residFun, m = magM, r = R, control = nls.lm.control(nprint = 1, :
lmdif: info = -1. Number of iterations has reached `maxiter' == 50.
Estimate Std. Error t value Pr(>|t|)
a -35.27102 382.61185 -0.092 0.927
b 1.07939 731.24259 0.001 0.999
c -0.01101 1125.04689 0.000 1.000
d -0.38616 550.44043 -0.001 0.999
e 38.67563 2.99421 12.917 < 2e-16 ***
f -0.16900 4700.28057 0.000 1.000
g 1.43564 160.15873 0.009 0.993
h -1.80097 587.09787 -0.003 0.998
i -33.60840 20.84056 -1.613 0.108
B_x -62.33962 8.77291 -7.106 1.19e-11 ***
B_y -247.28845 268.12997 -0.922 0.357
B_z -232.75322 4.80571 -48.433 < 2e-16 ***
eb_x 3.79520 1868.76346 0.002 0.998
eb_y -1.75584 2065.83362 -0.001 0.999
eb_z 12.36808 643.52944 0.019 0.985
Signif. codes: 0 ‘***’ 0.001 ‘**’ 0.01 ‘*’ 0.05 ‘.’ 0.1 ‘ ’ 1
Residual standard error: 474.4 on 255 degrees of freedom
Number of iterations to termination: 50
Reason for termination: Number of iterations has reached `maxiter' == 50.
Parameters a to i are the 9 entries of the matrix M, and b0x, b0y, b0z and B0x, B0y, Boz are the three entries each of eb and B. For this data set the optimization doesn’t quite converge, within 50 iterations. But the residual sum of squares (RSS) is hardly moving by the end; I could either try again with a higher limit to the number of steps, or press on and see what kind of results I get. If this were an academic exercise I’d be studying the convergence conditions and using different algorithms. But for now I’m just interested in the answers: do they work?
> # RESULTS:
> # This is the mag offset:
B_x B_y B_z
-62.33962 -247.28845 -232.75322
> # This is the rotation matrix from magRaw to body-frame calibrated B-field measurement:
> solve(M) / solve(M)[1,1]
[,1] [,2] [,3]
[1,] 1.00000000 -0.02791751 -0.0001870903
[2,] 0.01016883 -0.91176870 0.0045814641
[3,] 0.04217190 0.04766630 1.0489039771
> # And this is the magnetic field vector at the calibration site,
> # relative to the startup datum for the calibration run:
> eb <- eb / sqrt(sum(eb^2))
eb_x eb_y eb_z
0.2906893 -0.1344868 0.9473188
> # the magnetic declination at your calibration site was
> 57.1 * asin(eb)
The script rescales M so that the top left entry is identically 1.0, and rescales b0,e to be of unit length. This is permissible because I don’t care about the absolute magnitude of the Earth field anywhere; I only want to know its direction. To check the angle of dip I note that the z-component of b0,e is the sin of the dip angle. In this case the script has measured the angle as 71.32º. According to the BGS website the world geomagnetic model predicts an inclincation of 70.147º, so I’m off by a little more than 1 degree. That’s not too shabby. Here’s the 3d plot of the original data, lying on an ellipsoid:
Raw magnetometer measurements. They lie closely on an ellipsoid (which appears close to spherical as a result of the scaling). The green dot is the centre of mass, used as an intiai value for the centre of the ellipsoid in the least squares fit.
If you use R then you can rotate the axes of the plot by dragging the mouse pointer within the window, and you get a much better idea of the shape of the data as it rotates in front of your eyes. Here’s the plot of the same data after scaling to the sphere and rotating. If the least-squares fit was successful then the blue dots now represent the Earth field vector in the sensor frame-of-reference, each measurement. The dots move around as the sensor rotates. The green sphere is centred on the origin and has radius equal to the mean distance of the blue dots. So the blue dots should lie on it. The red dots are the same measurements of the Earth field presented in the Earth frame-of-reference (by using the orientation data that accompanies each measurement). Ideally all the red dots should lie at one place, representing the Earth field.
The same data scaled to the sphere, and then reoriented to show the best estimate for the Earth field vector. The absolute size of each measurement and of the sphere is unimportant.
For the purposes of stabilizing the heading output of the AHRS we are interested entirely in the angular variation of the measurement of the Earth field when projected into the earth-xy plane, since this gives us an indication of how stable the compass heading will be. (If we used the Earth field measurement to stabilize in pitch and roll too then we would be interested in the z component too.) The next graph shows this angular deviation from the mean, in degrees. The horizontal axis is the measurement index:
Angular deviation from the mean direction of measured magnetic north vs. index number
The plot shows that most measurements lie with ±5º. There is some structure visible in the graph (we would hope for pure white noise) which indicates drift or inaccuracy in the direction cosine matrix trending with time. I can remove the noisy component with smoothing (which is what the Kalman filter on the heading variable does). This dataset was obtained after careful calibration of all three axis gyroscopes to better than 0.1%. The next graph shows the angular deviation in a data set created with one of the gyroscopes miscalibrated by 1% (only):
Angular deviation with x- gyroscope miscalibrated by 1%
The results show larger errors and, significantly, errors which are structured in time. They would appear in the output since they can’t be filtered. The time structure results from incorrect angular rates from the miscalibrated gyroscope being integrated into the direction cosine matrix. Roll and pitch errors will be corrected by the gravitational correction but yaw errors persist through the calibration period. Here are the results of another data set (call it B) with the gyro measurement corrected. This time I moved the AHRS more slowly than before during the calibration period:
The results of another calibration period
Angular deviations for the calibration above
The data doesn’t appear to fit the sphere as well as the previous set, but the angular deviations are smaller. The least squares algorithm reached convergence in 32 iterations. Also this time the angle of dip was assessed as 70.52º, very close to the “book” value of 70.147º.