dm00286302

DT0059
Design Tip
Ellipsoid or sphere fitting for sensor calibration
By Andrea Vitali
Main components*
LSM303AGR
Ultra compact high-performance e-compass: ultra-low-power
3D accelerometer and 3D magnetometer
LSM6DS3
iNEMO inertial module: 3D accelerometer and 3D gyroscope
Purpose and benefits
This design tip explains how to compute offsets, gains, and cross-axis gains for a 3-axis
sensor by performing a sphere (ellipsoid) fitting. The technique is typically used to calibrate
and compensate magnetometers, but it can also be used with other sensors, such as
accelerometers.
Benefits:

Added functionality with respect to calibration provided by the MotionFX library which
only provides offsets for the Magnetometer.

Short and essential implementation, which enables easy customization and
enhancement by the end-user (osxMotionFX is available only in binary format, not as
source code)

Easy to use on every microcontroller (osxMotionFX can only be run on the STM32 and
only when the proper license has been issued by Open.MEMS license server).
Algorithm description
Measurements are taken on a number of positions (N) and combined to find the unknowns
(offsets, gains and cross-axis gains).
For 6-tumble calibration, positioning the sensor accurately is required. However, for the
ellipsoid fitting described here, there is no need to know the true stimulus of the sensor, as
the only requirement is that the modulus of the true stimulus be constant (square root of
sum of squares of X, Y, and Z).

April 2016
For the case of the magnetometer: in order to measure only the earth magnetic
field, any other spurious (and often time varying) magnetic anomalies must be
absent; the modulus of the true stimulus is then the modulus of the earth magnetic
field
DT0059 Rev 1
1/5
www.st.com

For the case of the accelerometer: in order to measure only the gravity, the sensor
must not be subject to any other acceleration; the modulus of the true stimulus is
then the modulus of the gravity
In the most general case, the following equation has 9 unknowns,
T
v = [ a, b, c, d, e, f, g, h, i ] with the data points being on a rotated ellipsoid. If the ellipsoid
is not rotated, the axis will be aligned with X, Y and Z, where the corresponding equation
T
has only 6 unknowns v = [ a, b, c, g, h, i ] . If the axes are all the same length, then it is a
T
sphere, and the corresponding equation has only 4 unknowns v = [ a+b+c, g, h, i ] . The
general equation is the following:
2
2
2
a X + b Y + c Z + d 2XY + e 2XZ + f 2YZ + g 2X + h 2Y + i 2Z = 1
The set of N data points is used to build a data matrix D where the data points must not be
co-planar:

Rotated ellipsoid: line of D = [X , Y , Z , 2XY,2XZ, 2YZ, 2X, 2Y, 2Z], where D is
[Nx9]. At least 9 data points are needed to compute offsets, gains and cross-axis
gains

Non-rotated ellipsoid: line of D = [X , Y , Z , 2X, 2Y, 2Z], where D is [Nx6], At least
6 data points are needed to compute offsets and gains

Sphere: line of D = [X +Y +Z , 2X, 2Y, 2Z], where D is [Nx4]. At least 4 data points
are needed to compute offsets
2
2
2
2
2
2
2
2
2
Rotated ellipsoid fitting
Now, the least-square error approximation can be computed for the unknowns in v by using
the pseudo-inverse of the non-square matrix. First, both sides are multiplied by the
T
T
transpose D . Second, both sides are multiplied by the inverse of the square matrix D D .
There can be 9, 6 or 4 unknowns, depending on the aforementioned constraints. For the
most general case:
T
T
D[Nx9] v[9x1] = 1 [Nx1] → D [9xN] D[Nx9] v[9x1] = D [9xN] 1[Nx1] →
T
T
T
T
(D D)[9x9] v[9x1] = (D 1)[9x1] → v[9x1] = inv(D D)[9x9] (D 1)[9x1]
Next, the auxiliary matrix A4[4x4] and A3[3x3], and the auxiliary vector vghi[3x1] are built
using the unknowns v[9x1]:
T
T
v = [ a, b, c, d, e, f, g, h, i ] ,
vghi = [g h i]
A4 = [ a d e g; d b f h; e f c i; g h i -1 ],
A3 = [ a d e; d b f; e f c]
Offsets o = (ox, oy, oz) can be computed as follows:
A3[3x3] o[3x1] = -vghi[3x1] → o[3x1] = -inv(A3)[3x3] vghi[3x1]
Once the offsets are known, another auxiliary matrix B4[4x4] is computed, which
represents the ellipsoid translated into the origin:
April 2016
DT0059 Rev 1
2/5
www.st.com
T
T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; ox oy oz 1 ] → B4[4x4] = T[4x4] A[4x4] T [4x4],
B4[4x4] = [ b11 b12 b13 b14; b21 b22 b23 b24; b31 b32 b33 b34; b41 b42 b43 b44 ],
B3[3x3] = [ b11 b12 b13; b21 b22 b23; b31 b32 b33 ] / -b44
Gains and cross-axis gains can be computed from eigenvalues and eigenvectors
respectively of B3[3x3].

Ellipsoid radii are the square root of the inverse of the 3 eigenvalues; these are the
T
axis gains g = [gx, gy, gz]

Ellipsoid rotation matrix R[3x3] is obtained by juxtaposition of the 3 eigenvectors;
gains and cross-axis gains are obtained by multiplying the 3x3 matrix where the
diagonal contains the gains
T
Compensation of offsets, gains and cross-axis gains to map the data point p = [x, y, z] on
the unit sphere can then be done in 3 steps:
T
1. Subtraction of the offsets, p’ = p - o = [x-ox, y-oy, z-oz] = [ x’, y’, z’ ]
T
2. Multiplication by the inverse of the rotation matrix, p” = p’ inv(R) = [ x”, y”, z” ]
T
3. Division by the gains, p’”= [ x”/gx, y”/gy, z”/gz ] = [x’”, y’”, z’” ]
T
T
Non-rotated ellipsoid fitting
In this case, the data matrix D[Nx6] has only 6 columns and there are only 6 unknowns to
be computed v = [ a, b, c, g, h, i]:
T
T
D[Nx6] v[6x1] = 1 [Nx1] → D [6xN] D[Nx6] v[6x1] = D [6xN] 1[Nx1] →
T
T
T
T
(D D)[6x6] v[6x1] = (D 1)[6x1] → v[6x1] = inv(D D)[6x6] (D 1)[6x1]
Offsets o = (oX, oY, oZ) can be computed as follows:
o = [ a/g, b/h, c/i ]
T
T
Gains g = [ gx, gy, gz ] can be computed as follows:
2
2
2
G = 1 + g /a + h /b + i /c → g = [ sqrt(a/G) sqrt(b/G) sqrt(c/G) ]
T
Sphere fitting
In this case, the data matrix D[Nx4] has only 4 columns and there are only 4 unknowns to
T
T
be computed v = [a+b+c, g, h, i ] = [ a”, g, h, i] :
T
T
D[Nx4] v[4x1] = 1 [Nx1] → D [4xN] D[Nx4] v[4x1] = D [4xN] 1[Nx1] →
T
T
T
T
(D D)[4x4] v[4x1] = (D 1)[4x1] → v[4x1] = inv(D D)[4x4] (D 1)[4x1]
April 2016
DT0059 Rev 1
3/5
www.st.com
Offsets o = (oX, oY, oZ) can be computed as follows:
o = [ a”/g, a”/h, a”/i ]
T
T
Gains g = [ gx, gy, gz ] can be computed as follows:
2
2
2
G = 1 + g /a + h /a + i /a → g = [ sqrt(a/G) sqrt(a/G) sqrt(a/G) ]
T
Notes
Hints for a compact real-time implementation on a microcontroller:

Only the product DT[MxN] D[NxM] needs to be maintained in memory, this is a MxM
matrix, M=9, 6 or 4; worst case is that 9x9=81 elements are to be maintained in
memory

Only the product DT[MxN] 1[Nx1] needs to be maintained in memory, this is a Mx1
vector, M=9, 6, or 4; worst case is that 9 elements are to be maintained in memory

Gaussian elimination can be implemented to compute the inverse of the
aforementioned MxM matrix when enough data points (at least M) have been
collected

For the case of the rotated ellipsoid fitting, eigenvalues and eigenvectors of a 3x3
matrix can be computed by using closed formulas

For the case of a rotated ellipsoid when there is no or little rotation, the system
does not easily converge to the correct solution; this is especially true if data points
are affected by noise. If little or no rotation is expected (matrix R has small values
out of the diagonal) and/or if data points are affected by a significant noise, the
following alternate equation system is suggested:
2
2
2
2
2
2
2
2
D[Nx9] = [ X +Y -2Z , X -2Y +Z , 4XY,2XZ, 2YZ, 2X, 2Y, 2Z, 1 ]
2
E[Nx1] = [ X +Y +Z ]
T
T
D[Nx9] u[9x1] = E[Nx1] → D [9xN] D[Nx9] u[9x1] = D [9xN] E[Nx1] →
T
T
T
T
(D D)[9x9] u[9x1] = (D 1)[9x1] → u[9x1] = inv(D D)[9x9] (D E)[9x1]
S’[3x3] = [ 3, 1, 1; 3, 1, -2; 3, -2, 1 ]
S[10x10] = [ S’[3x3], 0[3x7]; 0[7x3] eye[7x7] ] then set s44 = 2
v’ = S[10x10] [ -1/3; u[9x1] ] = [ a’, b’, c’, d’, e’, f’, g’, h’, i’, j’ ]
T
T
v = - [ a’, b’, c’, d’, e’, f’, g’, h’, i' ] / j’ = [ a, b, c, d, e, f, g, h, i ]
T
Then, the computation proceeds as before for the case of a rotated ellipsoid.
April 2016
DT0059 Rev 1
4/5
www.st.com
Support material
Related design support material
BlueMicrosystem1, Bluetooth low energy and sensors software expansion for STM32Cube
Open.MEMS, MotionFX, Real-time motion-sensor data fusion software expansion for STM32Cube
Documentation
Application note, AN4508, Parameters and calibration of a low-g 3-axis accelerometer
Application note, AN4615, Fusion and compass calibration APIs for the STM32 Nucleo with
the X-NUCLEO-IKS01A1 sensors expansion board
Desing tip, DTxxxx, 6-point tumble sensor calibration
Revision history
Date
11-Apr-2016
Version
1
Changes
Initial release
IMPORTANT NOTICE – PLEASE READ CAREFULLY
STMicroelectronics NV and its subsidiaries (“ST”) reserve the right to make changes, corrections, enhancements,
modifications, and improvements to ST products and/or to this document at any time without notice. Purchasers should
obtain the latest relevant information on ST products before placing orders. ST products are sold pursuant to ST’s terms and
conditions of sale in place at the time of order acknowledgement.
Purchasers are solely responsible for the choice, selection, and use of ST products and ST assumes no liability for
application assistance or the design of Purchasers’ products.
No license, express or implied, to any intellectual property right is granted by ST herein.
Resale of ST products with provisions different from the information set forth herein shall void any warranty granted by ST for
such product.
ST and the ST logo are trademarks of ST. All other product or service names are the property of their respective owners.
Information in this document supersedes and replaces information previously supplied in any prior versions of this document.
© 2016 STMicroelectronics – All rights reserved
April 2016
DT0059 Rev 1
5/5
www.st.com
Similar pages