- No category
Orientation Matrices from Sensor Data: NXP Application Note
advertisement
AN5021 Calculation of Orientation Matrices from Sensor Data Rev. 2.0 — 21 June 2016 Application note Document information Info Content Abstract This application note introduces the rotation or orientation matrix and documents the functions in file orientation.c used by the NXP Sensor Fusion Library to compute an orientation matrix from different combinations of sensor measurements. AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data Revision history Document ID Release date Supercedes AN5021 v2.0 20160621 AN5021 v1.0 Modifications: • Minor changes • The format of this document has been redesigned to comply with the new identity guidelines of NXP Semiconductors. Legal texts have been adapted to the new company name where appropriate. AN5021 v1.0 2015 October — Contact information For more information, please visit: http://www.nxp.com AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 2 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 1. Introduction 1.1 Summary This application note introduces the rotation or orientation matrix and documents the functions in file orientation.c used by the NXP Sensor Fusion Library to compute an orientation matrix from different combinations of sensor measurements. 1.2 Terminology Symbol Definition 𝐵𝐵 𝑆𝑆 𝑆𝑆 Geomagnetic field strength 𝑩𝑩𝑐𝑐 Calibrated magnetometer measurements in the sensor frame 𝐵𝐵𝑐𝑐𝑐𝑐 , 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 , 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 ENU 𝑆𝑆 𝑆𝑆 Calibrated accelerometer measurement in the sensor frame � 𝒏𝒏 Normalized rotation axis 𝐺𝐺𝑐𝑐𝑐𝑐 , 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 , 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 NED 𝑹𝑹 x, y and z components of the accelerometer reading in the sensor frame � Components of normalized rotation axis 𝒏𝒏 x=North, y=East, z=Down coordinate system (Aerospace) General orientation matrix 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 Orientation matrix in the Android coordinate system Orientation matrix in the NED/Aerospace coordinate system 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 Orientation matrix in the Windows 8 coordinate system 𝑡𝑡𝑡𝑡(𝑹𝑹) Trace of orientation matrix 𝑹𝑹 𝑅𝑅𝑥𝑥𝑥𝑥 𝛿𝛿 𝜂𝜂 𝜃𝜃 𝜙𝜙 𝜓𝜓 Application note x=East, y=North, z=Up coordinate system (Android and Windows 8) 𝑮𝑮𝑐𝑐 𝑛𝑛�𝑥𝑥 , 𝑛𝑛�𝑦𝑦 , 𝑛𝑛�𝑧𝑧 AN5021 x, y and z components of the calibrated magnetometer reading in the sensor frame Element in row x and column y of orientation matrix 𝑹𝑹 Geomagnetic inclination angle Rotation angle Pitch angle Roll angle Yaw angle All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 3 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 1.3 Software Functions Functions Description Reference Computes the tilt orientation matrix from accelerometer measurements in the Aerospace/NED, Android and Windows 8 coordinate systems. 4 Computes the 2D eCompass yaw orientation matrix from calibrated magnetometer measurements in the Aerospace/NED, Android and Windows 8 coordinate systems. 5 Computes the full 3D orientation matrix from accelerometer and calibrated magnetometer measurements in the Aerospace/NED, Android and Windows 8 coordinate void feCompassAndroid (float fR[][3], float *pfDelta, systems. float *pfsinDelta, float *pfcosDelta, Also returns the float fBc[], float fGc[], geomagnetic inclination float *pfmodBc, float *pfmodGc) angle and the moduli of the accelerometer and void feCompassWin8 magnetometer (float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, measurements. 6 void f3DOFTiltNED (float fR[][3], float fGc[]); void f3DOFTiltAndroid (float fR[][3], float fGc[]); void f3DOFTiltWin8 (float fR[][3], float fGc[]); void f3DOFMagnetometerMatrixNED (float fR[][3], float fBc[]); void f3DOFMagnetometerMatrixAndroid (float fR[][3], float fBc[]); void f3DOFMagnetometerMatrixWin8 (float fR[][3], float fBc[]); void feCompassNED (float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[], float *pfmodBc, float *pfmodGc) float fBc[], float fGc[], float *pfmodBc, float *pfmodGc) 2. General Rotation Matrix 2.1 Introduction The general rotation matrix 𝑹𝑹 which transforms a vector as a result of a rotation of the coordinate system around the normalized axis 𝒏𝒏 � by angle 𝜂𝜂 is: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑹𝑹 = �𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑛𝑛�𝑥𝑥 2 + �1 − 𝑛𝑛�𝑥𝑥 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) − 𝑛𝑛�𝑧𝑧 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 𝑛𝑛� 𝑛𝑛� (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛� 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑥𝑥 𝑧𝑧 𝑦𝑦 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛�𝑧𝑧 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑦𝑦 2 + �1 − 𝑛𝑛�𝑦𝑦 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝜂𝜂) − 𝑛𝑛�𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) − 𝑛𝑛�𝑦𝑦 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛�𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � (1) 𝑛𝑛�𝑧𝑧 2 + �1 − 𝑛𝑛�𝑧𝑧 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 Equation (1) is known as the Rodrigues rotation matrix. The product of the rotation axis 𝒏𝒏 � and rotation angle 𝜂𝜂 is termed the rotation vector 𝒏𝒏 �𝜂𝜂. AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 4 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data If the coordinate system rotates by angle 𝜂𝜂 about the z axis, then a vector rotates by angle −𝜂𝜂 about the z axis, relative to the newly rotated coordinate system. When NXP documents refer to a rotation vector defining a rotation matrix, it always refers to a coordinate system rotation. In addition, when a rotation matrix 𝑹𝑹 models the orientation of the sensors relative to the earth (global) reference frame, the NXP Sensor Fusion Library standard is that the product 𝑹𝑹𝑹𝑹 transforms the vector 𝒗𝒗 from the global to the sensor frame. The inverse rotation 𝑹𝑹−1 𝒗𝒗 = 𝑹𝑹𝑇𝑇 𝒗𝒗 transforms the vector 𝒗𝒗 from the sensor to the global frame. The orientation matrix is always defined relative to an initial orientation in the global frame with the product flat and pointed to magnetic north giving zero roll, pitch and yaw angles. Two special cases of equation (1) occur for rotation angles of i) 0o and ii) 180o about any axis. Substituting into equation (1) gives the 0o rotation matrix 𝑹𝑹(0) and 180o rotation matrix 𝑹𝑹(𝜋𝜋) as: 1 𝑹𝑹(0) = �0 0 2𝑛𝑛�𝑥𝑥 2 − 1 𝑹𝑹(𝜋𝜋) = � 2𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 2𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑧𝑧 0 1 0 0 0� 1 2𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 2 (2) 2𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑧𝑧 2𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 � 2𝑛𝑛�𝑦𝑦 − 1 2𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 In both cases the rotation matrix is symmetric. (3) 2𝑛𝑛�𝑧𝑧 2 − 1 2.2 Eigenvector and Eigenvalue The normalized rotation axis 𝒏𝒏 � is the eigenvector of the rotation matrix product 𝑹𝑹 corresponding to eigenvalue 1. Geometrically, this means that any vector parallel to the rotation axis is unchanged by the rotation. 𝑛𝑛�𝑥𝑥 2 + �1 − 𝑛𝑛�𝑥𝑥 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑹𝑹𝒏𝒏 � = �𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) − 𝑛𝑛�𝑧𝑧 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛�𝑦𝑦 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛�𝑧𝑧 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑦𝑦 2 + �1 − 𝑛𝑛�𝑦𝑦 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) − 𝑛𝑛�𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) − 𝑛𝑛�𝑦𝑦 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑛𝑛�𝑥𝑥 𝑛𝑛�𝑦𝑦 𝑛𝑛�𝑧𝑧 (1 − 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐) + 𝑛𝑛�𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� �𝑛𝑛�𝑦𝑦 � 𝑛𝑛�𝑧𝑧 𝑛𝑛� 2 + �1 − 𝑛𝑛� 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑧𝑧 𝑛𝑛�𝑥𝑥 �𝑛𝑛�𝑥𝑥 2 + 𝑛𝑛�𝑦𝑦 2 + 𝑛𝑛�𝑧𝑧 2 � − 𝑛𝑛�𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐�𝑛𝑛�𝑥𝑥 2 + 𝑛𝑛�𝑦𝑦 2 + 𝑛𝑛�𝑧𝑧 2 − 1� 𝑧𝑧 𝑛𝑛�𝑥𝑥 = � 𝑛𝑛�𝑦𝑦 �𝑛𝑛�𝑥𝑥 2 + 𝑛𝑛�𝑦𝑦 2 + 𝑛𝑛�𝑧𝑧 2 � − 𝑛𝑛�𝑦𝑦 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐�𝑛𝑛�𝑥𝑥 2 +𝑛𝑛�𝑦𝑦 2 + 𝑛𝑛�𝑧𝑧 2 − 1� � = �𝑛𝑛�𝑦𝑦 � 𝑛𝑛�𝑧𝑧 𝑛𝑛� �𝑛𝑛� 2 +𝑛𝑛� 2 + 𝑛𝑛� 2 � − 𝑛𝑛� 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐�𝑛𝑛� 2 + 𝑛𝑛� 2 + 𝑛𝑛� 2 − 1� 𝑧𝑧 𝑥𝑥 𝑦𝑦 𝑧𝑧 𝑧𝑧 𝑥𝑥 𝑦𝑦 (4) (5) 𝑧𝑧 3. Converting Between Rotation Matrix and Rotation Vector 3.1 Rotation Vector from Rotation Matrix Equation (1) allows the rotation angle 𝜂𝜂 to be determined from the trace of the rotation matrix 𝑹𝑹: AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 5 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑡𝑡𝑡𝑡(𝑹𝑹) = 𝑅𝑅𝑥𝑥𝑥𝑥 + 𝑅𝑅𝑦𝑦𝑦𝑦 + 𝑅𝑅𝑧𝑧𝑧𝑧 = 𝑛𝑛�𝑥𝑥 2 + �1 − 𝑛𝑛�𝑥𝑥 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 + 𝑛𝑛�𝑦𝑦 2 + �1 − 𝑛𝑛�𝑦𝑦 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 + 𝑛𝑛�𝑧𝑧 2 + �1 − 𝑛𝑛�𝑧𝑧 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 (6) ⇒ 𝑡𝑡𝑡𝑡(𝑹𝑹) = 1 + 2𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 (8) = �𝑛𝑛�𝑥𝑥 2 + 𝑛𝑛�𝑦𝑦 2 +𝑛𝑛�𝑧𝑧 2 � + 3𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 − �𝑛𝑛�𝑥𝑥 2 + 𝑛𝑛�𝑦𝑦 2 +𝑛𝑛�𝑧𝑧 2 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 ⇒ 𝜂𝜂 = 𝑐𝑐𝑐𝑐𝑐𝑐 −1 � 𝑡𝑡𝑡𝑡(𝑹𝑹) − 1 � 2 (7) (9) The trace of the matrix varies between –1 and +3. A trace of –1 corresponds to a rotation of 180° about any axis and a trace of +3 corresponds to a rotation of 0°. There is no angle ambiguity in equation (9) because the inverse cosine returns an angle between 0° and 180° meaning that the rotation angle 𝜂𝜂 also varies between 0° and 180°. Rotation angles between –180° and 0° are equivalent to rotation by the negated angle −𝜂𝜂 about the negated rotation axis −𝒏𝒏 � and the rotation vector 𝜂𝜂𝒏𝒏 � is unaltered by a double sign change. Differencing elements across the leading diagonal of 𝑹𝑹 gives: 𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 2𝒏𝒏 �𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = � 𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 � 𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑅𝑅𝑦𝑦𝑦𝑦 (10) In the general case where 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ≠ 0 and the rotation matrix is not symmetric, the � equals: rotation axis 𝒏𝒏 𝒏𝒏 �= 𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 � 𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 � 2 2 ��𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 � + (𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 )2 + �𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑅𝑅𝑦𝑦𝑦𝑦 1 (11) As the rotation angle 𝜂𝜂 approaches zero, equation (11) becomes undefined mathematically corresponding to the physical situation that all rotation axes are equivalent at zero rotation. The rotation vector 𝒏𝒏 �𝜂𝜂 is still, however, defined as: 𝒏𝒏 �𝜂𝜂 = 𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 1 � 𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 � 2 𝑅𝑅 − 𝑅𝑅 𝑥𝑥𝑥𝑥 (12) 𝑦𝑦𝑦𝑦 For the case of the rotation angle 𝜂𝜂 near 180°, the rotation matrix 𝑹𝑹 approximates the form of equation (3) with a trace of –1. The elements on the leading diagonal allow the rotation axis 𝒏𝒏 � to be given by: AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 6 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑅𝑅𝑥𝑥𝑥𝑥 + 1 ⎛ ±� 2 ⎞ ⎜ ⎟ 𝑛𝑛�𝑥𝑥 ⎜ ⎟ 𝑅𝑅 + 1⎟ 𝒏𝒏 � = �𝑛𝑛�𝑦𝑦 � = ⎜±� 𝑦𝑦𝑦𝑦 ⎜ 2 ⎟ 𝑛𝑛�𝑧𝑧 ⎜ ⎟ ⎜ 𝑅𝑅𝑧𝑧𝑧𝑧 + 1 ⎟ ±� 2 ⎠ ⎝ (13) The sign ambiguity can be resolved by differencing across the leading diagonal and using the result that 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 is nonnegative. 𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 = 2𝑛𝑛�𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑛𝑛�𝑥𝑥 ) = 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠�𝑅𝑅𝑦𝑦𝑦𝑦 − 𝑅𝑅𝑧𝑧𝑧𝑧 � (14) 𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑅𝑅𝑦𝑦𝑦𝑦 = 2𝑛𝑛�𝑧𝑧 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑛𝑛�𝑧𝑧 ) = 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠�𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑅𝑅𝑦𝑦𝑦𝑦 � (16) 𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 = 2𝑛𝑛�𝑦𝑦 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠�𝑛𝑛�𝑦𝑦 � = 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑅𝑅𝑥𝑥𝑥𝑥 ) (15) Any rotation can be represented by its normalized rotation axis 𝒏𝒏 � and the rotation angle 𝜂𝜂 about that axis, giving a total of three degrees of freedom. The rotation vector 𝜂𝜂𝒏𝒏 � is the most efficient encoding of a rotation but has no useful mathematical properties. The rotation quaternion (four components) and rotation matrix (9 components) are, therefore, used in preference. The output of the gyroscope sensor (in degrees/second) multiplied by the sampling interval is a rotation vector and is therefore immediately converted in software to a rotation quaternion or matrix for orientation integration. 3.2 Rotation Matrix from Rotation Vector Equation (1) defines the Rodrigues rotation matrix directly in terms of the rotation axis 𝒏𝒏 � and rotation angle 𝜂𝜂. These can easily be determined for the case of non-zero rotation vector 𝜂𝜂𝒏𝒏 � using: 𝜂𝜂 = |𝜂𝜂𝒏𝒏 �| 𝒏𝒏 �= (17) 𝜂𝜂𝒏𝒏 � |𝜂𝜂𝒏𝒏 �| (18) If the rotation vector is zero then the rotation matrix is the identity matrix. AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 7 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 4. Accelerometer Tilt Orientation Matrix 4.1 Introduction Accelerometer sensors are sensitive to both linear acceleration and gravity and are insensitive to rotation about the vertical gravity axis. An accelerometer operating without other sensors can therefore detect roll, pitch and tilt angles from horizontal provided there is no linear acceleration but cannot detect compass heading. The mathematics below assumes that the yaw or compass angle is always zero degrees. One consequence of this assumption is that the orientation undergoes a twist of 180° at ±90° pitch (Aerospace / NED coordinate system) and at ±90° roll (in Android and Windows 8 coordinate systems) to ensure that the orientation remains pointing northward. In the Aerospace / NED coordinate system, another way of describing the same phenomenon is to apply a 180° roll (resulting in the circuit board pointed northward but inverted) followed by a 180° yaw rotation (which has no effect because the accelerometer is insensitive to rotation about the vertical gravity axis). The same orientation can also be reached by a 180° pitch rotation. Without the 180° orientation twist at 90° pitch, the same accelerometer reading would lead to two different orientations depending upon the path taken. 4.2 Aerospace / NED This section documents the mathematics underlying the function f3DOFTiltNED in file orientation.c. The Aerospace / NED orientation matrix 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 for zero yaw angle 𝜓𝜓 after rotation from the horizontal by pitch angle 𝜃𝜃 followed by roll angle 𝜙𝜙 is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜓𝜓 = 0) = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑜𝑜𝑠𝑠 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 (19) The Aerospace / NED accelerometer reading 𝑆𝑆𝑮𝑮𝑐𝑐 in the sensor frame at this orientation can be computed by multiplying the accelerometer measurement in the initial flat orientation by 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜓𝜓 = 0): 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 � 𝐺𝐺𝑐𝑐𝑐𝑐 � = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝐺𝐺 𝑆𝑆 𝑐𝑐𝑐𝑐 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑛𝑛𝑛𝑛 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � �0� = � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 1 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 2 𝑆𝑆 𝐺𝐺 ⎛ ⎞ �1 − � 𝑆𝑆 𝑐𝑐𝑐𝑐 � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⎜ ⎟ 𝑆𝑆 � 𝑮𝑮 � 𝑐𝑐 = � 𝑮𝑮𝑐𝑐 � ⎜ ⎟ 2⎟ ⎜ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 �1 − � 𝑆𝑆 � � 𝑮𝑮𝑐𝑐 � ⎠ ⎝ AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (20) (21) © NXP B.V. 2016. All rights reserved. 8 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = �1 − � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = (22) � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑆𝑆 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 ��1 − � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 2 (23) 𝑆𝑆 2 (24) 𝑆𝑆 2 (25) 𝐺𝐺𝑐𝑐𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 ��1 − � 𝐺𝐺𝑐𝑐𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � The positive square root for 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 can always be taken in equation (23) because the pitch angle 𝜃𝜃 in the Aerospace / NED coordinate system varies between –90° and +90° giving a nonnegative value of 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐. Substituting back into equation (19) gives the Aerospace / NED 3DOF orientation matrix directly in terms of the sensor frame accelerometer reading 𝑆𝑆𝑮𝑮𝑐𝑐 as: 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 ⎛ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎜ 𝑆𝑆 ⎜ − 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜓𝜓 = 0) = ⎜ 2 2 𝑆𝑆 𝑆𝑆 𝑆𝑆 ⎜� 𝑮𝑮𝑐𝑐 �� 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ 2 𝑆𝑆 𝑆𝑆 � 𝑆𝑆 ⎝� 𝑮𝑮𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 ⎛ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎜ ⎜ −𝑅𝑅 𝑅𝑅 � 𝑆𝑆𝑮𝑮 � 𝑥𝑥𝑥𝑥 𝑦𝑦𝑦𝑦 𝑐𝑐 ⎜ =⎜ 2 2 𝑆𝑆 � 𝑆𝑆 ⎜ 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ −𝑅𝑅 𝑅𝑅 � 𝑆𝑆𝑮𝑮 � 𝑥𝑥𝑥𝑥 𝑧𝑧𝑧𝑧 𝑐𝑐 ⎜ ⎝ AN5021 Application note 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 2 2 2 2 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 𝑅𝑅𝑧𝑧𝑧𝑧 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 2 2 2 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 All information provided in this document is subject to legal disclaimers. ⎞ � 𝑮𝑮𝑐𝑐 �⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 �⎟ ⎟ ⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎠ 𝐺𝐺𝑐𝑐𝑐𝑐 ⎞ � 𝑮𝑮𝑐𝑐 �⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 �⎟ ⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎠ (26) 𝑆𝑆 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 −𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 0 Rev. 2.0 — 21 June 2016 𝑆𝑆 0 (27) © NXP B.V. 2016. All rights reserved. 9 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data At the gimbal lock orientations with ±90° pitch 𝜃𝜃 rotation, where 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 = 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 = 0 and 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 = � 𝑆𝑆𝑮𝑮𝑐𝑐 �, equations (24) to (27) show that the roll angle 𝜙𝜙 and the rotation matrix 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 are undefined. Since the roll angle 𝜙𝜙 is now about the same rotation axis as the yaw rotation angle 𝜓𝜓 which is assumed to be zero, the simplest solution is to also set the roll angle to zero giving: 0 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜙𝜙 = 𝜓𝜓 = 0) = � 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 1 0 0 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 �=� 0 1 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠( 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ) 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠( 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ) � 0 0 (28) 4.3 Android This section documents the mathematics underlying the function f3DOFTiltAndroid in file orientation.c. The Android rotation matrix 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 for zero yaw angle 𝜓𝜓 after rotation from the horizontal by roll angle 𝜙𝜙 followed by pitch angle 𝜃𝜃 is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜓𝜓 = 0) = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (29) The Android accelerometer reading 𝑆𝑆𝑮𝑮𝑐𝑐 in the sensor frame at this orientation can be computed by multiplying the accelerometer measurement in the initial flat orientation by 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜓𝜓 = 0): 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 � 𝐺𝐺𝑐𝑐𝑐𝑐 � = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝐺𝐺 𝑆𝑆 𝑐𝑐𝑐𝑐 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� � 𝑆𝑆𝑮𝑮𝑐𝑐 � �0� = � 𝑆𝑆𝑮𝑮𝑐𝑐 � �−𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑠𝑠𝑠𝑠𝑠𝑠 𝜃𝜃� 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 1 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 2 𝑆𝑆 ⎛ 𝐺𝐺𝑐𝑐𝑐𝑐 � −𝑠𝑠𝑠𝑠𝑠𝑠 𝜃𝜃 1 − � � ⎜ 𝑆𝑆 𝑆𝑆 � 𝑮𝑮𝑐𝑐 � = � 𝑮𝑮𝑐𝑐 � ⎜ ⎜ 2 ⎜ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 �1 − � 𝑆𝑆 � � 𝑮𝑮𝑐𝑐 � ⎝ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑮𝑮𝑐𝑐 � AN5021 Application note 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 2 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (31) ⎠ (32) 𝑆𝑆 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = �1 − � ⎞ ⎟ ⎟ ⎟ ⎟ (30) (33) © NXP B.V. 2016. All rights reserved. 10 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑠𝑠𝑠𝑠𝑠𝑠 𝜃𝜃 = 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 = − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 ��1 − � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 2 (34) 𝑆𝑆 2 (35) 𝐺𝐺𝑐𝑐𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 ��1 − � 𝐺𝐺𝑐𝑐𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � The positive square root for 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 can always be taken in equation (33) because the roll angle 𝜙𝜙 in the Android coordinate system varies between –90° and +90° giving a nonnegative value of 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐. Substituting into equation (29) gives the 3DOF Android orientation matrix directly in terms of the accelerometer reading as: 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 ⎛ 𝑆𝑆 𝑆𝑆 ⎜ − 𝐺𝐺𝑐𝑐𝑐𝑐 𝐺𝐺𝑐𝑐𝑐𝑐 1 ⎜ ⎜� 𝑆𝑆𝐺𝐺 2 + 𝑆𝑆𝐺𝐺 2 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜓𝜓 = 0) = 𝑆𝑆 𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐 � 𝑮𝑮𝑐𝑐 � ⎜ ⎜ − 𝑆𝑆𝐺𝐺 𝑆𝑆𝐺𝐺 𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐 ⎜ 2 ⎝ 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 ⎛ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎜ ⎜ −𝑅𝑅 𝑅𝑅 � 𝑆𝑆𝑮𝑮 � 𝑥𝑥𝑥𝑥 𝑦𝑦𝑦𝑦 𝑐𝑐 ⎜ =⎜ 2 2 𝑆𝑆 � 𝑆𝑆 ⎜ 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ −𝑅𝑅 𝑅𝑅 � 𝑆𝑆𝑮𝑮 � 𝑥𝑥𝑥𝑥 𝑧𝑧𝑧𝑧 𝑐𝑐 ⎜ ⎝ 2 � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 2 𝑆𝑆 0 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 2 2 2 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 0 𝑅𝑅𝑧𝑧𝑧𝑧 � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 2 2 2 −𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝐺𝐺𝑐𝑐𝑐𝑐 AN5021 Application note 0 1 0 Rev. 2.0 — 21 June 2016 ⎞ ⎟ 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ ⎟ ⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ 𝑆𝑆 (36) ⎠ ⎞ � 𝑮𝑮𝑐𝑐 �⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 �⎟ ⎟ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎠ 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 0 �=� 0 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠( 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ) All information provided in this document is subject to legal disclaimers. 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 In the case of gimbal lock after ±90° roll rotation, when pitch angle 𝜃𝜃 is undefined and can be set to zero: 0 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜃𝜃 = 𝜓𝜓 = 0) = � 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 (37) 𝐺𝐺𝑐𝑐𝑐𝑐 = 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 = 0, the 0 1 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠( 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ) � 0 0 (38) © NXP B.V. 2016. All rights reserved. 11 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 4.4 Windows 8 This section documents the mathematics underlying the function f3DOFTiltWin8 in file orientation.c. The Windows 8 rotation matrix 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 for zero yaw angle 𝜓𝜓, after rotation from the horizontal by pitch angle 𝜃𝜃 followed by roll angle 𝜙𝜙, is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜓𝜓 = 0) = � 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (39) The Windows 8 accelerometer reading 𝑆𝑆𝑮𝑮𝑐𝑐 in the sensor frame at this orientation can be computed by multiplying the accelerometer measurement in the initial flat orientation by 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜓𝜓 = 0): 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 � 𝐺𝐺𝑐𝑐𝑐𝑐 � = � 0 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝐺𝐺 𝑆𝑆 𝑐𝑐𝑐𝑐 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 = −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � � 0 � = � 𝑆𝑆𝑮𝑮𝑐𝑐 � � − 𝑠𝑠𝑠𝑠𝑠𝑠 𝜃𝜃 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 −𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 −1 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⇒ 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 = 𝑠𝑠𝑠𝑠𝑠𝑠 𝜃𝜃 = 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 = − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = 𝑆𝑆 1 �1 + � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝐺𝐺𝑐𝑐𝑐𝑐 �1 + � − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 (41) 2 (42) (43) � 𝑆𝑆𝑮𝑮𝑐𝑐 � − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 2 (40) �1 + � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 2 (44) The positive square root for 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 can always be taken in equation (41) because the roll angle 𝜙𝜙 in the Windows 8 coordinate system varies between –90° and +90° resulting in a nonnegative value of 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐. Substituting into equation (39) gives the Windows 8 3DOF orientation matrix directly in terms of the accelerometer reading as: AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 12 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data ⎛ ⎜ ⎜ ⎜ 1 ⎜ 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜓𝜓 = 0) = 𝑆𝑆 � 𝑮𝑮𝑐𝑐 � ⎜ ⎜ ⎜ ⎜ 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎝ � 𝑆𝑆𝑮𝑮𝑐𝑐 � �1 + � 0 𝑆𝑆 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 ⎛ 2 2 𝑆𝑆 𝑆𝑆 � 𝑆𝑆 ⎜𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ ⎜ =⎜ 0 ⎜ ⎜ − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎜ 2 𝑆𝑆 𝑆𝑆 � 𝑆𝑆 ⎝𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 � 𝑆𝑆 ⎝ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝐺𝐺𝑐𝑐𝑐𝑐 ) 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 2 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝐺𝐺𝑐𝑐𝑐𝑐 �1 + � 𝑆𝑆 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 − 𝐺𝐺𝑐𝑐𝑐𝑐 �1 + � 𝑆𝑆 2 𝐺𝐺𝑐𝑐𝑐𝑐 �1 + � − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑅𝑅𝑦𝑦𝑦𝑦 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 2 � 𝑮𝑮𝑐𝑐 � − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑅𝑅𝑧𝑧𝑧𝑧 2 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝐺𝐺𝑐𝑐𝑐𝑐 )� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 �� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎧ 2 ⎫ 2 2⎬ ⎨ 𝑆𝑆 𝑆𝑆 � 𝑆𝑆 ⎩𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑐𝑐 ⎭ 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 2 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 �� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 ⎞ ⎟ ⎟ ⎟ 𝑆𝑆 − 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ ⎟ 𝑆𝑆 − 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ (45) ⎠ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎞ ⎟ ⎟ 𝑆𝑆 − 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎟ ⎟ − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎠ (46) − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 2 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 2 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑆𝑆𝑮𝑮𝑐𝑐 � −1 � 2 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 �� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆 2 2 2 2 ⎛ 2 2 𝑆𝑆 𝑆𝑆 � 𝑆𝑆 ⎜𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝐺𝐺𝑐𝑐𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 + 𝐺𝐺𝑐𝑐𝑧𝑧 ⎜ ⎜ 0 ⎜ =⎜ ⎜ ⎜ ⎜ 𝑅𝑅𝑥𝑥𝑥𝑥 � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎜ 𝑆𝑆 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 �� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 −𝑅𝑅𝑧𝑧𝑧𝑧 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 2 2 − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝐺𝐺𝑐𝑐𝑐𝑐 �1 + � 𝑆𝑆 � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎞ ⎟ − 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎟ ⎟ ⎟ ⎟ ⎟ 𝑆𝑆 − 𝐺𝐺𝑐𝑐𝑐𝑐 ⎟ � 𝑆𝑆𝑮𝑮𝑐𝑐 � ⎠ (47) In the case of gimbal lock after ±90° pitch rotation, when 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 = 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 = 0, the roll angle 𝜙𝜙 is undefined and can be set to zero: 1 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜙𝜙 = 𝜓𝜓 = 0) = �0 0 AN5021 Application note 0 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � = �0 0 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 � All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 0 0 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 �� (48) 0 © NXP B.V. 2016. All rights reserved. 13 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 5. Magnetometer 2D Compass Orientation Matrix 5.1 Introduction This section documents the 2D, magnetometer-only, eCompass functions that calculate the orientation matrix from calibrated magnetometer measurements with the assumption that the magnetometer is always flat and has zero tilt away from the horizontal. The market application for this arrangement is an automotive compass where the normal accelerometer plus magnetometer tilt-compensated eCompass functions cannot be used because of vehicle acceleration, braking and cornering forces. Use these functions with caution. In the automotive application, the 2D eCompass will be robust against the effects of acceleration, braking and cornering forces because the magnetometer sensor is unaffected by acceleration. However, the compass heading will change when the vehicle tilt angle, whether roll or pitch, deviates from horizontal, violating the assumption that the magnetometer sensor is always flat. 5.2 Aerospace / NED This section documents the mathematics underlying the function f3DOFMagnetometerMatrixNED in file orientation.c. The Aerospace / NED rotation matrix for rotation in yaw angle 𝜓𝜓 only while remaining horizontal is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜓𝜓) = �−𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 0 0� 1 (49) The Aerospace / NED calibrated magnetometer reading at this orientation is: 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 � 𝐵𝐵𝑐𝑐𝑐𝑐 � = �−𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 0 𝐵𝐵𝑐𝑐𝑐𝑐 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 0 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 0� 𝐵𝐵 � 0 � = 𝐵𝐵 �−𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 (50) The horizontal component of the geomagnetic field has the value: 2 𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵 = � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 (51) The yaw 𝜓𝜓 (and compass heading angle) is given by: 𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 = AN5021 Application note 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 = 𝑆𝑆 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝐵𝐵𝑐𝑐𝑐𝑐 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (52) © NXP B.V. 2016. All rights reserved. 14 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = − 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 2 2 2 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 (53) (54) The orientation matrix is independent of the unknown geomagnetic inclination angle 𝛿𝛿 and has the value: 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 (𝜓𝜓) = 𝑆𝑆 1 2 𝑆𝑆 𝑆𝑆 � 𝐵𝐵𝑐𝑐𝑐𝑐 + 𝐵𝐵𝑐𝑐𝑐𝑐 2 − 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 𝑆𝑆 0 𝐵𝐵𝑐𝑐𝑐𝑐 0 0 0� 1 (55) 5.3 Android This section documents the mathematics underlying the function f3DOFMagnetometerMatrixAndroid in file orientation.c. The Android rotation matrix for rotation in yaw angle 𝜓𝜓 only while remaining horizontal is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜓𝜓) = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 0 0� 1 (56) The Android calibrated magnetometer reading at this orientation is: 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 � 𝐵𝐵𝑐𝑐𝑐𝑐 � = � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 0 𝐵𝐵𝑐𝑐𝑐𝑐 𝑆𝑆 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 0 0� 𝐵𝐵 � 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � = 𝐵𝐵 � 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 (57) The horizontal component of the geomagnetic field has the value: 2 𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵𝐵 = � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 (58) The yaw 𝜓𝜓 and compass heading angles are given by: 𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 = AN5021 Application note 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 = 𝑆𝑆 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝐵𝐵𝑐𝑐𝑐𝑐 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (59) © NXP B.V. 2016. All rights reserved. 15 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = − 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 2 2 2 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 (60) (61) The orientation matrix is independent of the unknown geomagnetic inclination angle 𝛿𝛿 and has the value: 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 (𝜓𝜓) = 𝑆𝑆 1 𝐵𝐵𝑐𝑐𝑐𝑐 �− 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 2 𝑆𝑆 𝑆𝑆 � 𝐵𝐵𝑐𝑐𝑐𝑐 + 𝐵𝐵𝑐𝑐𝑐𝑐 0 𝑆𝑆 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 𝐵𝐵𝑐𝑐𝑐𝑐 0 0 0� 1 (62) 5.4 Windows 8 This section documents the mathematics underlying the function f3DOFMagnetometerMatrixWin8 in file orientation.c. The Windows 8 rotation matrix for rotation in yaw angle 𝜓𝜓 only while remaining horizontal is: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜓𝜓) = �− 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 0 0� 1 (63) The Windows 8 calibrated magnetometer reading at this orientation is: 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 � 𝐵𝐵𝑐𝑐𝑐𝑐 � = �− 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 0 𝐵𝐵𝑐𝑐𝑐𝑐 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 0 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 0 0� 𝐵𝐵 � 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � = 𝐵𝐵 �𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑜𝑜𝑠𝑠𝑠𝑠 � −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 (64) The horizontal component of the geomagnetic field has the value: 𝟐𝟐 𝑩𝑩𝑩𝑩𝑩𝑩𝑩𝑩𝑩𝑩 = � 𝑺𝑺𝑩𝑩𝑐𝑐𝑐𝑐 + 𝑺𝑺𝑩𝑩𝑐𝑐𝑐𝑐 𝟐𝟐 (65) The yaw 𝜓𝜓 angle, which is equal to minus the compass heading in the Windows 8 coordinate system, is given by: 𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 = AN5021 Application note 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝐵𝐵𝑐𝑐𝑐𝑐 = 𝑆𝑆 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝐵𝐵𝑐𝑐𝑐𝑐 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (66) © NXP B.V. 2016. All rights reserved. 16 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑆𝑆 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝐵𝐵𝑐𝑐𝑐𝑐 2 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 2 2 2 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 (67) (68) The orientation matrix is independent of the unknown geomagnetic inclination angle 𝛿𝛿 and has the value: 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 (𝜓𝜓) = 𝑆𝑆 1 2 𝑆𝑆 𝑆𝑆 � 𝐵𝐵𝑐𝑐𝑐𝑐 + 𝐵𝐵𝑐𝑐𝑐𝑐 2 𝐵𝐵𝑐𝑐𝑐𝑐 �− 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 0 𝑆𝑆 𝑆𝑆 𝐵𝐵𝑐𝑐𝑐𝑐 0 0� 1 𝐵𝐵𝑐𝑐𝑐𝑐 0 (69) 6. Accelerometer and Magnetometer 3D eCompass Orientation Matrix 6.1 Introduction This section documents the calculation of the orientation matrix from accelerometer and magnetometer measurements. This is the classic tilt-compensated eCompass which provides a complete determination of orientation. The assumption is that there is no linear acceleration so that the accelerometer reading is a function of orientation in the earth's gravitational field only. The orientation matrix can be calculated with an algorithm which is most easily understood from the trivial mathematical identity below: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅 � 𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 1 𝑅𝑅𝑦𝑦𝑦𝑦 � �0 𝑅𝑅𝑧𝑧𝑧𝑧 0 0 1 0 0 0� 1 (70) The three columns of the identity matrix on the right hand side are the three unit vectors in the x, y and z directions aligned with the initial orientation in the global reference frame. In the Aerospace/NED coordinate system, the x axis points North, the y axis points East and the z axis points Down. In the Android and Windows 8 coordinate systems, the x axis points East, the y axis points North and the z axis points Up Separating equation (70) into column vectors clearly shows that the x, y, and z components of the orientation matrix are the original x, y, and z unit vectors rotated from the global reference frame to the sensor frame: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 �𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 AN5021 Application note 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 1 𝑅𝑅𝑦𝑦𝑦𝑦 � �0� 𝑅𝑅𝑧𝑧𝑧𝑧 0 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (71) © NXP B.V. 2016. All rights reserved. 17 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 �𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅 𝑅𝑅 � 𝑦𝑦𝑦𝑦 � = � 𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑦𝑦𝑦𝑦 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑥𝑥𝑥𝑥 0 𝑅𝑅𝑦𝑦𝑦𝑦 � �1� 𝑅𝑅𝑧𝑧𝑧𝑧 0 𝑅𝑅𝑥𝑥𝑥𝑥 0 𝑅𝑅𝑦𝑦𝑦𝑦 � �0� 𝑅𝑅𝑧𝑧𝑧𝑧 1 (72) (73) In the absence of acceleration, the accelerometer measures the gravity vector measured in the sensor frame and, therefore, provides the right-hand column of the orientation matrix. The geomagnetic vector in the global frame points northward and downward (upward) in the northern (southern) hemisphere. The vector product between the rotated gravity and rotated geomagnetic vector is, by definition, at right angles to both and is, therefore, the rotated easterly-pointing vector from the global frame which forms the y (Aerospace / NED coordinate system) or x (Android and Windows 8 coordinate systems) column vector in the orientation matrix. Because the orientation matrix is orthonormal, a final vector product between these two column vectors gives the remaining column vector of the orientation matrix. 6.2 Aerospace / NED This section documents the mathematics underlying the function feCompassNED in file orientation.c. The z axis in the initial NED orientation is aligned downwards and parallel to gravity. The z axis column vector of the orientation matrix is the rotated z axis vector which, on the assumption of zero acceleration, is the normalized accelerometer measurement 𝑆𝑆𝑮𝑮𝑐𝑐 : 𝑅𝑅𝑥𝑥𝑥𝑥 𝑆𝑆 𝑮𝑮𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑮𝑮𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (74) The geomagnetic vector, estimated from the calibrated magnetometer measurement, points northwards in the initial orientation (in the x direction for the NED coordinate system) and downward (upward) in the northern (southern) hemisphere. The normalized vector product of the accelerometer and magnetometer measurements gives a normalized vector orthogonal to both which is, therefore, aligned along the east pointing y axis and equal to the y column of the rotation matrix: 𝑅𝑅𝑥𝑥𝑦𝑦 𝑆𝑆 𝑮𝑮𝑐𝑐 × 𝑆𝑆𝑩𝑩𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑮𝑮𝑐𝑐 × 𝑆𝑆𝑩𝑩𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (75) Finally, since rotation matrices are column (and row) orthonormal, the vector product of the y and z columns of the rotation matrix columns equals the remaining x column AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 18 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 �𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 (76) The geomagnetic inclination angle 𝛿𝛿 is the angle by which the geomagnetic field dips below horizontal in the global frame. Because the scalar product of two vectors is invariant under rotation, the inclination angle can be computed from the scalar product of the normalized accelerometer and calibrated magnetic measurements in the sensor frame giving: 𝑆𝑆 𝑮𝑮𝑐𝑐 ∙ 𝑆𝑆𝑩𝑩𝑐𝑐 = � 𝑆𝑆𝑮𝑮𝑐𝑐 �� 𝑆𝑆𝑩𝑩𝑐𝑐 �𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 �� 𝑆𝑆𝑩𝑩𝑐𝑐 � (77) The orientation matrix resulting from applying equations (74) to (76) can be shown to equal the Aerospace / NED orientation matrix derived in equation (5) of AN5017 “Aerospace, Android and Windows Coordinate Systems”: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 = �𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 (78) The proof is simplified by using the standard result that the vector product of any two rotated vectors equals their rotated vector product: Equation (74) evaluates to: 𝑹𝑹(𝒂𝒂 × 𝒃𝒃) = 𝑹𝑹𝑹𝑹 × 𝑹𝑹𝑹𝑹 (79) 𝑅𝑅𝑥𝑥𝑥𝑥 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑅𝑅 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 �0� = � � = � 𝑦𝑦𝑦𝑦 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑅𝑅𝑧𝑧𝑧𝑧 1 𝑆𝑆 𝑮𝑮𝑐𝑐 (80) Equation (75) evaluates to: 0 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑹𝑹𝑁𝑁𝑁𝑁𝑁𝑁 ��0� × � 0 �� 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑮𝑮𝑐𝑐 × 𝑩𝑩𝑐𝑐 1 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑹𝑹 𝑅𝑅 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = � � = � � = � 1 𝑦𝑦𝑦𝑦 � 𝑁𝑁𝑁𝑁𝑁𝑁 0 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 × 𝑆𝑆𝑩𝑩𝑐𝑐 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅 0 𝑧𝑧𝑧𝑧 ��0� × � 0 �� 1 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑆𝑆 𝑆𝑆 (81) Equation (76) evaluates to: AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 19 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � = � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � × � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 (𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 − (𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −(𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠)𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 2 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 =� � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 2 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + (𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠)𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 = �𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � = �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑐𝑐𝑐𝑐𝑠𝑠𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 (82) (83) (84) 6.3 Android This section documents the mathematics underlying the function feCompassAndroid in file orientation.c. The z axis in the initial Android orientation is aligned upwards and anti-parallel to gravity. The Android standard specifies that the accelerometer is acceleration positive and not gravity positive. The z axis column vector of the orientation matrix is therefore the rotated z axis vector which, on the assumption of zero acceleration, is the normalized accelerometer measurement 𝑮𝑮𝑠𝑠 : 𝑅𝑅𝑥𝑥𝑥𝑥 𝑆𝑆 𝑮𝑮𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑮𝑮𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (85) The geomagnetic vector, estimated from the calibrated magnetometer measurement, points northwards in the initial orientation (in the y direction for the Android coordinate system) and downward (upward) in the northern (southern) hemisphere. The normalized vector product of the magnetometer and accelerometer measurements gives a normalized vector orthogonal to both which is, therefore, aligned along the east pointing x axis: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑆𝑆 𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (86) Finally, since rotation matrices are orthonormal, the vector product of the rotation matrix z and x axes gives the remaining y component of the orientation matrix: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 �𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 (87) The inclination angle 𝛿𝛿 is computed from the scalar product of the accelerometer and calibrated magnetic measurements to give: AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 2016. All rights reserved. 20 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 𝑆𝑆 𝑆𝑆 𝑆𝑆 𝑆𝑆 𝑮𝑮𝑐𝑐 ∙ 𝑩𝑩𝑐𝑐 = −� 𝑮𝑮𝑐𝑐 �� 𝑩𝑩𝑐𝑐 �𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = −� 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 � � 𝑆𝑆𝑮𝑮𝑐𝑐 �� 𝑆𝑆𝑩𝑩𝑐𝑐 � (88) The orientation matrix resulting from applying equations (85) to (87) can be shown to equal the Android orientation matrix derived in equation (30) of AN5017 “Aerospace, Android and Windows Coordinate Systems”: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 = � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝜙𝜙𝜙𝜙𝜙𝜙𝜙𝜙𝜙𝜙 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (89) Equation (85) evaluates to: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 0 𝑅𝑅 = 𝑹𝑹𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 �0� = �− 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � = � 𝑦𝑦𝑦𝑦 � � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑅𝑅𝑧𝑧𝑧𝑧 1 𝑆𝑆 𝑮𝑮𝑐𝑐 (90) Equation (86) evaluates to: 0 0 𝑹𝑹 �� � × � 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 0�� 𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 𝑆𝑆 1 𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 = 𝑹𝑹 = � 0� 𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴 𝑆𝑆 𝑆𝑆 0 0 � 𝑩𝑩𝑐𝑐 × 𝑮𝑮𝑐𝑐 � 0 �� 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � × �0�� −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 1 (91) 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑅𝑅 = � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � = � 𝑦𝑦𝑦𝑦 � − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 (92) Equation (87) evaluates to: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � = �− 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠� × � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(− 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(− 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) =� � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑅𝑅𝑥𝑥𝑥𝑥 −𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 = � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � = �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (93) (94) (95) © NXP B.V. 2016. All rights reserved. 21 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 6.4 Windows 8 This section documents the mathematics underlying the function feCompassWin8 in file orientation.c. The accelerometer reading in the Windows 8 coordinate system in the initial orientation is anti-parallel to the z axis because the Windows 8 coordinate system is ENU with the z axis upward and is gravity (not acceleration) positive. The z axis column vector of the orientation matrix is therefore the rotated z axis vector which, on the assumption of zero acceleration, is the normalized accelerometer measurement 𝑮𝑮𝑠𝑠 : 𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑆𝑆𝑮𝑮𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑮𝑮𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (96) The geomagnetic vector, estimated from the calibrated magnetometer measurement, points northwards in the initial orientation (in the y direction for the Windows 8 coordinate system) and downward (upward) in the northern (southern) hemisphere. The normalized vector product of the magnetometer and negated accelerometer measurements gives a normalized vector orthogonal to both which is, therefore, aligned along the east pointing x axis: 𝑅𝑅𝑥𝑥𝑥𝑥 − 𝑆𝑆𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 �𝑅𝑅𝑦𝑦𝑦𝑦 � = 𝑆𝑆 � 𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑅𝑅𝑧𝑧𝑧𝑧 (97) Finally, since rotation matrices are orthonormal, the vector product of the rotation matrix z and x axes gives the remaining y component of the orientation matrix: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 �𝑅𝑅𝑦𝑦𝑦𝑦 � = �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 (98) The inclination angle 𝛿𝛿 is computed from the scalar product of the accelerometer and calibrated magnetic measurements giving: 𝑆𝑆 𝑮𝑮𝑐𝑐 ∙ 𝑆𝑆𝑩𝑩𝑐𝑐 = � 𝑆𝑆𝑮𝑮𝑐𝑐 �� 𝑆𝑆𝑩𝑩𝑐𝑐 �𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ⇒ 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = 𝑆𝑆 𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 + 𝑆𝑆𝐺𝐺𝑐𝑐𝑐𝑐 𝑆𝑆𝐵𝐵𝑐𝑐𝑐𝑐 � 𝑆𝑆𝑮𝑮𝑐𝑐 �� 𝑆𝑆𝑩𝑩𝑐𝑐 � (99) The orientation matrix resulting from applying equations (96) to (98) can be shown to equal the Windows 8 orientation matrix derived in equation (55) of AN5017 “Aerospace, Android and Windows Coordinate Systems”: 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑹𝑹𝑊𝑊𝑊𝑊𝑊𝑊8 = � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 AN5021 Application note 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (100) © NXP B.V. 2016. All rights reserved. 22 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data Equation (96) evaluates to: 𝑅𝑅𝑥𝑥𝑥𝑥 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 0 𝑅𝑅 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 = −𝑹𝑹 � � = � � = � 0 𝑦𝑦𝑦𝑦 � 𝑊𝑊𝑊𝑊𝑊𝑊8 � 𝑆𝑆𝑮𝑮𝑐𝑐 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑅𝑅𝑧𝑧𝑧𝑧 −1 − 𝑆𝑆𝑮𝑮𝑐𝑐 (101) Equation (97) evaluates to: 0 0 −𝑹𝑹 �� � × � 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 0 �� 𝑊𝑊𝑊𝑊𝑊𝑊8 1 − 𝑆𝑆𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −1 = = 𝑹𝑹 � 0� 𝑊𝑊𝑊𝑊𝑊𝑊8 0 0 � 𝑆𝑆𝑩𝑩𝑐𝑐 × 𝑆𝑆𝑮𝑮𝑐𝑐 � 0 �� 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 � × � 0 �� −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −1 (102) 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 =� � = �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 (103) Equation (98) evaluates to: 𝑅𝑅𝑥𝑥𝑥𝑥 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 �𝑅𝑅𝑦𝑦𝑦𝑦 � × �𝑅𝑅𝑦𝑦𝑦𝑦 � = � �×� � 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 𝑅𝑅𝑧𝑧𝑧𝑧 (104 ) 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 (105 = �𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 (𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 ) 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠(𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠) 𝑅𝑅𝑥𝑥𝑥𝑥 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 + 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐 𝜃𝜃 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 =� � = �𝑅𝑅𝑦𝑦𝑦𝑦 � 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 − 𝑐𝑐𝑐𝑐𝑐𝑐 𝜙𝜙 𝑐𝑐𝑐𝑐𝑐𝑐 𝜓𝜓 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑅𝑅𝑧𝑧𝑧𝑧 AN5021 Application note All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 (106 ) © NXP B.V. 2016. All rights reserved. 23 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 7. Legal information 7.1 Definitions Draft — The document is a draft version only. The content is still under internal review and subject to formal approval, which may result in modifications or additions. NXP Semiconductors does not give any representations or warranties as to the accuracy or completeness of information included herein and shall have no liability for the consequences of use of such information. 7.2 Disclaimers 7.3 Trademarks Information in this document is provided solely to enable system and software implementers to use NXP products. There are no express or implied copyright licenses granted hereunder to design or fabricate any integrated circuits based on the information in this document. NXP reserves the right to make changes without further notice to any products herein. NXP makes no warranty, representation, or guarantee regarding the suitability of its products for any particular purpose, nor does NXP assume any liability arising out of the application or use of any product or circuit, and specifically AN5021 Application note disclaims any and all liability, including without limitation consequential or incidental damages. “Typical” parameters that may be provided in NXP data sheets and/ or specifications can and do vary in different applications, and actual performance may vary over time. All operating parameters, including “typicals,” must be validated for each customer application by customer's technical experts. NXP does not convey any license under its patent rights nor the rights of others. NXP sells products pursuant to standard terms and conditions of sale, which can be found at the following address: nxp.com/salestermsandconditions. Notice: All referenced brands, product names, service names and trademarks are property of their respective owners. NXP, the NXP logo, Freescale, and the Freescale logo are trademarks of NXP B.V. ARM and Cortex are registered trademarks of ARM Limited (or its subsidiaries) in the EU and/or elsewhere. All information provided in this document is subject to legal disclaimers. Rev. 2.0 — 21 June 2016 © NXP B.V. 20164. All rights reserved. 24 of 25 AN5021 NXP Semiconductors Calculation of Orientation Matrices from Sensor Data 8. Contents 1. 1.1 1.2 1.3 2. 2.1 2.2 3. 3.1 3.2 4. 4.1 4.2 4.3 4.4 5. 5.1 5.2 5.3 5.4 6. 6.1 6.2 6.3 6.4 7. 7.1 7.2 7.3 8. Introduction ......................................................... 3 Summary ............................................................ 3 Terminology ....................................................... 3 Software Functions ............................................ 4 General Rotation Matrix ...................................... 4 Introduction ........................................................ 4 Eigenvector and Eigenvalue............................... 5 Converting Between Rotation Matrix and Rotation Vector.................................................... 5 Rotation Vector from Rotation Matrix ................. 5 Rotation Matrix from Rotation Vector ................. 7 Accelerometer Tilt Orientation Matrix ............... 8 Introduction ........................................................ 8 Aerospace / NED ............................................... 8 Android ............................................................. 10 Windows 8........................................................ 12 Magnetometer 2D Compass Orientation Matrix ............................................................................ 14 Introduction ...................................................... 14 Aerospace / NED ............................................. 14 Android ............................................................. 15 Windows 8........................................................ 16 Accelerometer and Magnetometer 3D eCompass Orientation Matrix........................... 17 Introduction ...................................................... 17 Aerospace / NED ............................................. 18 Android ............................................................. 20 Windows 8........................................................ 22 Legal information .............................................. 24 Definitions ........................................................ 24 Disclaimers....................................................... 24 Trademarks ...................................................... 24 Contents ............................................................. 25 Please be aware that important notices concerning this document and the product(s) described herein, have been included in the section 'Legal information'. © NXP B.V. 2016. All rights reserved. For more information, visit: http://www.nxp.com Date of release: 21 June 2016 Document identifier: AN5021
0
advertisement
Download
advertisement
Add this document to collection(s)
You can add this document to your study collection(s)
Sign in Available only to authorized usersAdd this document to saved
You can add this document to your saved list
Sign in Available only to authorized users