In a couple previous posts we have been discussing HMCSPIlibrary, which is responsible for communication with the HMC5983
magnetometer from Honeywell installed on my board. If you were checking the
code and followed on my explanation you might have noticed that HMCSPI_Local.c
code file includes several constant vectors:
// MAG Hard Iron correction vector
Vector _HMC_HardIron = {-22.85, -112.55, -38.64};
// MAG Soft Iron correction matrix
Vector _HMC_SoftIron_X = { 1.0000, -0.0183, 0.0305};
Vector _HMC_SoftIron_Y =
{-0.0183, 1.0269, 0.0009};
Vector _HMC_SoftIron_Z = { 0.0305,
0.0009, 1.0585};
These vectors are applied to the raw HMC5983 measurements in
function _HMC_NormHMCData(…) defined
in the header file HMCSPI_Inlines.h.
So what are these vectors and where their values come from and what does the
function _HMC_NormHMCData(…) do?
These vectors represent the hard-iron correction vector and soft-iron
correction matrix calculated specifically for the sensor installed on the board
and _HMC_NormHMCData(…) applies
these corrections to the magnetometer measurements. In this post we will see
how these values are calculated and how we can apply them to improve precision
of our measurements.
First, let’s define the problem. When a sensor installed on
the board it becomes a subject to interference from the nearby components,
board traces (even if we follow manufacturer’s recommendation on how to lay out
the traces), some physical stress on the package resulting from soldering, etc.
Despite the datasheet statement about factory calibration of all the sensors, I
still haven’t seen a single magnetometer that would not require in-place
calibration!
When we rotate magnetometer in Earth magnetic field we would
expect that the measurement vector would follow a sphere centered at the origin
of the coordinate system. In practice most of the time the measurement vector
would follow ellipsoid with the center at some offset from the origin of
coordinate system. The “hard-iron” correction centers measurements around the
origin of coordinate system and “soft-iron” correction converts ellipsoid back
to a circle. Application notes AN4246
and AN4248
from Freescale WEB site provide a
wealth of details about the effects of distortion on magnetic measurements and
how to correct them. The algorithm discussed in this post is based upon the
information provided in these notes. AN4246
also provides a picture illustrating measurements subject to hard- and
soft-iron distortion and results of correction, which I copied for this post:
The pink ellipsoid with the red dots represent uncorrected
measurements achieved while rotating magnetometer while the bluish sphere with
blue dots represent the same measurements after the hard- and soft-iron
correction is applied.
The idea of the hard- and soft-iron correction of
magnetometer measurements (slightly simplified notation as compared to AN4246)
is that the corrected vector M’ can
be obtained from the raw vector of measurements M by applying the following linear transformation:
M’ = W*(M – V), (1)
where V is the
hard-iron correction vector and W is
a 3x3 symmetric matrix transforming ellipsoid into a sphere, which is the soft-iron correction. So the problem is
reduced to identifying these vector V and
matrix W. The application notes
referred above discuss analytic methods of identifying these elements; I
resorted to Excel and Solver to calculate vector V and matrix W based
upon the results of multiple measurements, which I will discuss below. To
collect these measurements I programmed the board with the code generated from
project 13-HMCSPI-Async,
which we briefly discussed in one of the previous posts. Obviously for this
experiment I was interested in uncorrected measurements so initially the hard-iron
correction vector V was set to a zero-vector
and soft-iron correction matrix W was set to be an identity matrix in the HMCSPI_Local.c
code file.
After programming the board with the firmware from this
project, I started the board and collected about 25,000 measurements while rotating
the board in all 3 dimensions. All collected measurements I imported into
Excel. The following picture provides the snapshot of the spreadsheet preloaded
with the samples data:
Columns headed Mx,
My, and Mz represent respective components of the magnetic vector as
reported by the sensor. For each of these columns I calculated the minimum and
maximum value (rows Min and Max) and their mid-point (average
between respective Min and Max) in the row Vinit. The formula for Vinit(X)
is shown on the snapshot above. Now for each sample we should calculate the
strength of the magnetic field derived from the respective measurement as
illustrated in the following picture:
The strength of the magnetic field is the size of the
measurement vector; we calculate it as a square root of the dot product of the
measurement vector to itself as illustrated in the picture above. Just remember
that this is an “array formula” so after it is built in in the formula bar to
commit it to the spreadsheet you need to press Ctrl+Shift+Enter keys simultaneously. After you enter the formula
in the first cell you may extend it down to calculate magnetic field strength
for each of the measurements. After I did that I calculated average strength of
the magnetic field across all measurements.
In the ideal case all these measurements should be on the
surface of a sphere; to evaluate how close our measurements match the sphere we
can calculate the variance in the size of the magnetic field across all
measurements. The formula for this calculation is presented on the picture
below – just remember that this is again is an “array formula”:
Unfortunately both the variance
at 3,774.96 and the standard deviation at 61.5 are quite large indicating that
the measurements are quite far from being on the sphere so there is a need for
hard- and soft-iron correction. Next to the columns containing measurement data
we will allocate columns for corrected values as well as the hard-iron
correction vector V and soft-iron
correction matrix W. We will copy
values of Vinit onto vector V as they represent a good initial
estimate and set matrix W to be the
identity matrix. As we know matrix W
to be symmetric, in the cells under main diagonal (highlighted in blue) instead
of actual 0 values I entered the formula making their values equal to the respective
values above the diagonal as illustrated on the picture below:
To simplify entering further formulas on the spreadsheet it
is convenient to assign names to the range of cell representing vector V – naturally I gave it a name “V”. Similarly
the range of cells representing matrix W
was named “W”. Now that we have the original measurements and vector V and matrix W, we may enter the formula for calculating corrected measurement vector
M’ in accordance with formula (1)
above.
Excel snapshot above illustrates this formula for the first
row of corrected values – from the vector of original measurements B2:D2 we subtract vector V, transpose the result to make it into
a column vector, which we then left-multiply by the matrix W using Excel MMULT
function. Result of multiplication is again transposed to make it into a row
and this formula is entered as Excel array formula into cells H2:J2 as the first row of corrected
measurements. Now this formula could be extended down to calculate corrected
measurements for each of the source measurements.
Same as we did before for the original measurements, we
should calculate the strength of magnetic field for each of the corrected
measurement, the average strength of magnetic field, its variance and standard
deviation. Results are presented on the picture below:
As you may see from the picture above, by applying very
simple hard-iron correction using Vinit
vector the variance of the results and standard deviation have been reduced
quite significantly! Let’s see what we can achieve by optimizing hard- and
soft-iron correction values – our tool to achieve this will be Excel’s Solver. Our variables will be the
elements of vector V and highlighted
in red elements of matrix W. Using
these variables we will instruct Solver to find a solution that minimizes the
value of the variance. There is one caveat to this approach – we have to force one
of the diagonal elements of W to be
1.0; otherwise Solver will just bring both vector V and matrix W to 0
resulting in a very trivial solution. The limitation of this approach is that
we will not be able to identify exactly
the strength of magnetic field, but it will be proportional to the value we
calculate while being a few percentage points off. When configuring Solver just
remember to uncheck “Make Unconstrained
Variables Non-Negative” as shown in the following screenshot:
Now we may run the Solver! On my Core i-7 machine Solver
took about 10 minutes to reach the optimal solution – your results could vary. After accepting Solver’s solution, I got the following results:
From comparing original samples with corrected ones we may
see that applying hard- and soft-iron correction to magnetometer measurements
decreased variance of the measurements more than 100-fold and brought standard
deviation down to about 1%. The evaluated strength of Earth magnetic field
achieved after correction is around 579 mGa, which is consistent with the
expectations for my location. The only thing left is to copy the values of
vector V and matrix W into the code as the constants specific to my sensor on
the board, which now explains the constants you saw in HMCSPI_Local.c
code file J.
I do not have tools to create nice 3-D diagrams like those
provided by Freescale, so I resort to a simple chart tracing the strength of
magnetic field (the length of the magnetic measurement vector) across original
and corrected measurements, which is presented below:
The corrected one looks much more reliable to use for
estimating orientation based upon the magnetic vector – at least to me.
Having reliable sensor data is quite important for achieving stability in the control algorithm. Looks like we addressed this issue for the magnetometer, so in the next post we will look at another calibration –
applying temperature compensation to MPU-6050 measurements.
