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