ecompass

Orientation from magnetometer and accelerometer readings

Syntax

orientation = ecompass(accelerometerReading,magnetometerReading)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF)

Description

orientation = ecompass(accelerometerReading,magnetometerReading) returns a quaternion that can rotate quantities from a parent (NED) frame to a child (sensor) frame.

example

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat) specifies the orientation format as quaternion or rotation matrix.

example

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF) also allows you to specify the reference frame RF of the orientation output.

Examples

collapse all

Determine Declination of Boston

Open Live Script

Use the known magnetic field strength and proper acceleration of a device pointed true north in Boston to determine the magnetic declination of Boston.

Define the known acceleration and magnetic field strength in Boston.

magneticFieldStrength = [19.535 -5.109 47.930];
properAcceleration = [0 0 9.8];

Pass the magnetic field strength and acceleration to the ecompass function. The ecompass function returns a quaternion rotation operator. Convert the quaternion to Euler angles in degrees.

q = ecompass(properAcceleration,magneticFieldStrength);
e = eulerd(q,'ZYX','frame');

The angle, e, represents the angle between true north and magnetic north in Boston. By convention, magnetic declination is negative when magnetic north is west of true north. Negate the angle to determine the magnetic declination.

magneticDeclinationOfBoston = -e(1)

magneticDeclinationOfBoston = 
-14.6563

Return Rotation Matrix

Open Live Script

The ecompass function fuses magnetometer and accelerometer data to return a quaternion that, when used within a quaternion rotation operator, can rotate quantities from a parent (NED) frame to a child frame. The ecompass function can also return rotation matrices that perform equivalent rotations as the quaternion operator.

Define a rotation that can take a parent frame pointing to magnetic north to a child frame pointing to geographic north. Define the rotation as both a quaternion and a rotation matrix. Then, convert the quaternion and rotation matrix to Euler angles in degrees for comparison.

Define the magnetic field strength in microteslas in Boston, MA, when pointed true north.

m = [19.535 -5.109 47.930];
a = [0 0 9.8];

Determine the quaternion and rotation matrix that is capable of rotating a frame from magnetic north to true north. Display the results for comparison.

q = ecompass(a,m);
quaterionEulerAngles = eulerd(q,'ZYX','frame')

quaterionEulerAngles = 1×3

   14.6563         0         0

r = ecompass(a,m,'rotmat');
theta = -asin(r(1,3));
psi = atan2(r(2,3)/cos(theta),r(3,3)/cos(theta));
rho = atan2(r(1,2)/cos(theta),r(1,1)/cos(theta));
rotmatEulerAngles = rad2deg([rho,theta,psi])

rotmatEulerAngles = 1×3

   14.6563         0         0

Determine Gravity Vector

Open Live Script

Use ecompass to determine the gravity vector based on data from a rotating IMU.

Load the inertial measurement unit (IMU) data.

load 'rpy_9axis.mat' sensorData Fs

Determine the orientation of the sensor body relative to the local NED frame over time.

orientation = ecompass(sensorData.Acceleration,sensorData.MagneticField);

To estimate the gravity vector, first rotate the accelerometer readings from the sensor body frame to the NED frame using the orientation quaternion vector.

gravityVectors = rotatepoint(orientation,sensorData.Acceleration);

Determine the gravity vector as an average of the recovered gravity vectors over time.

gravityVectorEstimate = mean(gravityVectors,1)

gravityVectorEstimate = 1×3

    0.0000   -0.0000   10.2102

Track Spinning Platform

Open Live Script

Fuse modeled accelerometer and gyroscope data to track a spinning platform using both idealized and realistic data.

Generate Ground-Truth Trajectory

Describe the ground-truth orientation of the platform over time. Use the kinematicTrajectory System object™ to create a trajectory for a platform that has no translation and spins about its z-axis.

duration = 12;
fs = 100;
numSamples = fs*duration;

accelerationBody = zeros(numSamples,3);

angularVelocityBody = zeros(numSamples,3);
zAxisAngularVelocity = [linspace(0,4*pi,4*fs),4*pi*ones(1,4*fs),linspace(4*pi,0,4*fs)]';
angularVelocityBody(:,3) = zAxisAngularVelocity;

trajectory = kinematicTrajectory('SampleRate',fs);

[~,orientationNED,~,accelerationNED,angularVelocityNED] = trajectory(accelerationBody,angularVelocityBody);

Model Receiving IMU Data

Use an imuSensor System object to mimic data received from an IMU that contains an ideal magnetometer and an ideal accelerometer.

IMU = imuSensor('accel-mag','SampleRate',fs);
[accelerometerData,magnetometerData] = IMU(accelerationNED, ...
                                           angularVelocityNED, ...
                                           orientationNED);

Fuse IMU Data to Estimate Orientation

Pass the accelerometer data and magnetometer data to the ecompass function to estimate orientation over time. Convert the orientation to Euler angles in degrees and plot the result.

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

timeVector = (0:numSamples-1).'/fs;

figure(1)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Ideal IMU')

Figure contains an axes object. The axes object with title Orientation from Ideal IMU, xlabel Time (s), ylabel Rotation (degrees) contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

Repeat Experiment with Realistic IMU Sensor Model

Modify parameters of the IMU System object to approximate realistic IMU sensor data. Reset the IMU and then call it with the same ground-truth acceleration, angular velocity, and orientation. Use ecompass to fuse the IMU data and plot the results.

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',20, ...
    'Resolution',0.0006, ...
    'ConstantBias',0.5, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.004, ...
    'BiasInstability',0.5);
IMU.Magnetometer = magparams( ...
    'MeasurementRange',200, ...
    'Resolution',0.01);
reset(IMU)

[accelerometerData,magnetometerData] = IMU(accelerationNED,angularVelocityNED,orientationNED);

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

figure(2)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Realistic IMU')

Figure contains an axes object. The axes object with title Orientation from Realistic IMU, xlabel Time (s), ylabel Rotation (degrees) contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

Input Arguments

collapse all

`accelerometerReading` — Accelerometer readings in sensor body coordinate system (m/s²)
N-by-3 matrix

Accelerometer readings in sensor body coordinate system in m/s², specified as an N-by-3 matrix. The columns of the matrix correspond to the x-, y-, and z-axes of the sensor body. The rows in the matrix, N, correspond to individual samples. The accelerometer readings are normalized before use in the function.

Data Types: single | double

`magnetometerReading` — Magnetometer readings in sensor body coordinate system (µT)
N-by-3 matrix

Magnetometer readings in sensor body coordinate system in µT, specified as an N-by-3 matrix. The columns of the matrix correspond to the x-, y-, and z-axes of the sensor body. The rows in the matrix, N, correspond to individual samples. The magnetometer readings are normalized before use in the function.

Data Types: single | double

`orientationFormat` — Format used to describe orientation
`'quaternion'` (default) | `'rotmat'`

Format used to describe orientation, specified as 'quaternion' or 'rotmat'.

Data Types: char | string

`RF` — Reference frame of the orientation output
`'NED'` (default) | `'ENU'`

Reference frame of the orientation output. Specify RF as 'NED' (North-East-Down) or 'ENU' (East-North-Up). The default value is 'NED'.

Data Types: char | string

Output Arguments

collapse all

`orientation` — Orientation that rotates quantities from global coordinate system to sensor body coordinate system
N-by-1 vector of quaternions (default) | 3-by-3-by-N array

Orientation that can rotate quantities from a global coordinate system to a body coordinate system, returned as a vector of quaternions or an array. The size and type of the orientation depends on the format used to describe orientation:

'quaternion' –– N-by-1 vector of quaternions with the same underlying data type as the input
'rotmat' –– 3-by-3-by-N array the same data type as the input

Data Types: quaternion | single | double

Algorithms

The ecompass function returns a quaternion or rotation matrix that can rotate quantities from a parent (NED for example) frame to a child (sensor) frame. For both output orientation formats, the rotation operator is determined by computing the rotation matrix.

The rotation matrix is first calculated with an intermediary:

$R = [\begin{matrix} (a \times m) \times a & a \times m & a \end{matrix}]$

and then normalized column-wise. a and m are the accelerometerReading input and the magnetometerReading input, respectively.

To understand the rotation matrix calculation, consider an arbitrary point on the Earth and its corresponding local NED frame. Assume a sensor body frame, [x,y,z], with the same origin.

NED Frame

Recall that orientation of a sensor body is defined as the rotation operator (rotation matrix or quaternion) required to rotate a quantity from a parent (NED) frame to a child (sensor body) frame:

$[\begin{matrix} R \end{matrix}] [\begin{matrix} p_{parent} \end{matrix}] = [\begin{matrix} p_{child} \end{matrix}]$

where

R is a 3-by-3 rotation matrix, which can be interpreted as the orientation of the child frame.
p_parent is a 3-by-1 vector in the parent frame.
p_child is a 3-by-1 vector in the child frame.

For a stable sensor body, an accelerometer returns the acceleration due to gravity. If the sensor body is perfectly aligned with the NED coordinate system, all acceleration due to gravity is along the z-axis, and the accelerometer reads [0 0 1]. Consider the rotation matrix required to rotate a quantity from the NED coordinate system to a quantity indicated by the accelerometer.

Express Gravitational Vector in Different Frames

$[\begin{matrix} r_{11} & r_{21} & r_{31} \\ r_{12} & r_{22} & r_{32} \\ r_{13} & r_{23} & r_{33} \end{matrix}] [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}]$

The third column of the rotation matrix corresponds to the accelerometer reading:

$[\begin{matrix} r_{31} \\ r_{32} \\ r_{33} \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}]$

A magnetometer reading points toward magnetic north and is in the N-D plane. Again, consider a sensor body frame aligned with the NED coordinate system.

Magnetometer Direction

By definition, the E-axis is perpendicular to the N-D plane, therefore D ⨯ N = E, within some amplitude scaling. If the sensor body frame is aligned with NED, both the acceleration vector from the accelerometer and the magnetic field vector from the magnetometer lie in the N-D plane. Therefore a ⨯ m = y, again with some amplitude scaling.

Consider the rotation matrix required to rotate NED to the child frame, [x y z].

Express a crosses m

$[\begin{matrix} r_{11} & r_{21} & r_{31} \\ r_{12} & r_{22} & r_{32} \\ r_{13} & r_{23} & r_{33} \end{matrix}] [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] \times [\begin{matrix} m_{1} \\ m_{2} \\ m_{3} \end{matrix}]$

The second column of the rotation matrix corresponds to the cross product of the accelerometer reading and the magnetometer reading:

$[\begin{matrix} r_{21} \\ r_{22} \\ r_{23} \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] \times [\begin{matrix} m_{1} \\ m_{2} \\ m_{3} \end{matrix}]$

By definition of a rotation matrix, column 1 is the cross product of columns 2 and 3:

$\begin{array}{l} [\begin{matrix} r_{11} \\ r_{12} \\ r_{13} \end{matrix}] = [\begin{matrix} r_{21} \\ r_{22} \\ r_{23} \end{matrix}] \times [\begin{matrix} r_{31} \\ r_{32} \\ r_{33} \end{matrix}] \\ = (a \times m) \times a \end{array}$

Finally, the rotation matrix is normalized column-wise:

$R_{i j} = \frac{R_{i j}}{\sqrt{\sum_{i = 1}^{3} R_{i j}^{2}}}, \forall j$

Note

The ecompass algorithm uses magnetic north, not true north, for the NED coordinate system.

References

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2018b

ecompass

Syntax

Description

Examples

Determine Declination of Boston

Return Rotation Matrix

Determine Gravity Vector

Track Spinning Platform

Input Arguments

accelerometerReading — Accelerometer readings in sensor body coordinate system (m/s2) N-by-3 matrix

magnetometerReading — Magnetometer readings in sensor body coordinate system (µT) N-by-3 matrix

orientationFormat — Format used to describe orientation 'quaternion' (default) | 'rotmat'

RF — Reference frame of the orientation output 'NED' (default) | 'ENU'

Output Arguments

orientation — Orientation that rotates quantities from global coordinate system to sensor body coordinate system N-by-1 vector of quaternions (default) | 3-by-3-by-N array

Algorithms

References

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using MATLAB® Coder™.

Version History

See Also

`accelerometerReading` — Accelerometer readings in sensor body coordinate system (m/s²)
N-by-3 matrix

`magnetometerReading` — Magnetometer readings in sensor body coordinate system (µT)
N-by-3 matrix

`orientationFormat` — Format used to describe orientation
`'quaternion'` (default) | `'rotmat'`

`RF` — Reference frame of the orientation output
`'NED'` (default) | `'ENU'`

`orientation` — Orientation that rotates quantities from global coordinate system to sensor body coordinate system
N-by-1 vector of quaternions (default) | 3-by-3-by-N array

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.