MATLAB Examples

Estimate Orientation and Height Using IMU, Magnetometer, and Altimeter

This example shows how to fuse data from a 3-axis accelerometer, 3-axis gyroscope, 3-axis magnetometer (together commonly referred to as a MARG sensor for Magnetic, Angular Rate and Gravity) and 1-axis altimeter to estimate orientation and height.


Simulation Setup

This simulation processes sensor data at multiple rates. The IMU (accelerometer and gyroscope) typically runs at the highest rate. Often the magnetometer runs at a lower rate than the IMU and the altimeter runs at the lowest rate. Changing the sample rates causes parts of the fusion algorithm to run more frequently and can affect the performance.

% Set the sampling rate for IMU sensors, magnetometer, and altimeter.
imuFs = 100;
altFs = 10;
magFs = 25;
imuSamplesPerAlt = fix(imuFs/altFs);
imuSamplesPerMag = fix(imuFs/magFs);

% Set the number of samples to simulate.
N = 1000;

% Construct object for other helper functions.
hfunc = fusiondemo.Helper10AxisFusion;

Define Trajectory

The sensor body rotates about all three axes while oscillating in position up and down vertically. The oscillations increase in magnitude as the simulation continues.

% Define the initial state of the sensor body
initPos = [0, 0, 0];       % initial position (m)
initVel = [0, 0, -1];      % initial linear velocity (m/s)
initOrient = ones(1, 'quaternion');

% Define the constant angular velocity for rotating the sensor body
% (rad/s).
angVel = [0.34 0.2 0.045];

% Define the acceleration required for simple oscillating motion of the
% sensor body.
fc = 0.2;
t = 0:1/imuFs:(N-1)/imuFs;
a = 1;
oscMotionAcc = sin(2*pi*fc*t);
oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);

% Construct the trajectory object
traj = kinematicTrajectory('SampleRate', imuFs, ...
    'Velocity', initVel, ...
    'Position', initPos, ...
    'Orientation', initOrient);

Sensor Configuration

The accelerometer, gyroscope and magnetometer are simulated using imuSensor. The altimeter is modeled by simply adding white noise to the true altitude. The values used in the sensor configurations correspond to real MEMS sensor values.

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);
% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.ConstantBias = deg2rad(3.125);
imu.Gyroscope.AxesMisalignment = 1.5;
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/ sqrt(50) ;

% altimeter
altimeterNoise = 0.24; % variance in m^2

Fusion Filter

The insfilter can be used to create a simple 10-axis fusion filter. The insfilter is intended to be used in conjunction with a GPS, but that is not required as long as there is some alternate sensor measurement of position. Altimeter readings represent a direct measurement of the height of the device, and are used in the correct method of the insfilter as an alternative to GPS readings. Though an application-specific filter could be more efficient than the insfilter by using fewer states, the insfilter does a capable job in this case with little overhead.

Define where on the Earth this simulated scenario takes place using the latitude, longitude and altitude.

refloc = [42.2825 -72.3430 53.0352];

Construct a filter with the insfilter function and configure.

fusionfilt = insfilter;
fusionfilt.IMUSampleRate = imuFs;
fusionfilt.ReferenceLocation = refloc;

Set initial values for the fusion filter.

initstate = zeros(22,1);
initstate(1:4) = compact(initOrient);
initstate(5:7) = initPos;
initstate(8:10) = initVel;
initstate(11:13) =  imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) =  imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) =  imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;

Initialize the state covariance matrix of the fusion filter. The ground truth is used for initial states so there should be little error in the estimates.

icv = 1e-6*eye(22);
fusionfilt.StateCovariance = icv;

Magnetometer measurement noise is the observation noise associated with the magnetometer used by the internal Kalman filter in the fusion filter. This value would normally come from a sensor datasheet.

magNoise =  2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs;

Filter process noises are used to tune the filter to desired performance.

fusionfilt.AccelerometerNoise = [1e-1 1e-1 1e-4];
fusionfilt.AccelerometerBiasNoise = 1e-8;
fusionfilt.GeomagneticVectorNoise = 1e-12;
fusionfilt.MagnetometerBiasNoise = 1e-12;
fusionfilt.GyroscopeNoise = 1e-12;

Additional Simulation Option - Motion Constraints

The insfilter tracks the X, Y, and Z positions. However, only a Z position measurement is available from an altimeter. If it is known that the X and Y positions are constrained (for example, in this simulation they are constrained to be X = 0 meters, Y = 0 meters), then the constraints can be applied to improve position estimation. The default for this simulation does not apply the constraint that X and Y are 0, so the position estimates from the filter drift rapidly. To use the constraint that X=0 and Y=0, set the applyXYConstraint variable to true.

applyXYConstraint = false;

Additional Simulation Option - Viewer

By default, this simulation plots the estimation errors at the end of the simulation. To view both the estimated position and orientation along with the ground truth as the simulation runs, set the usePoseViewer variable to true.

usePoseViewer = false;

Simulation Loop

q = initOrient;
firstTime = true;

actQ = zeros(N,1, 'quaternion');
expQ = zeros(N,1, 'quaternion');
actP = zeros(N,3);
expP = zeros(N,3);

for ii = 1: N
    % Generate a new set of samples from the trajectory generator
    accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]);
    omgBody = rotateframe(q, angVel);
    [pos, q, vel, acc] = traj(accBody, omgBody);

    % Feed the current position and orientation to the imuSensor object
    [accel, gyro, mag] = imu(acc, omgBody, q);
    fusionfilt.predict(accel, gyro);

    % Fuse magnetometer samples at the magnetometer sample rate
    if ~mod(ii,imuSamplesPerMag)
        fusemag(fusionfilt, mag, magNoise);

    % Sample and fuse the altimeter output at the altimeter sample rate
    if ~mod(ii,imuSamplesPerAlt)
        altHeight = hfunc.altimeter(pos(3), altimeterNoise);

        % The altimeter output represents a direct measurement of one of
        % the state variables. Use the correct method to update the
        % fusion filter with the altimeter output.
        correct(fusionfilt, 7, altHeight, altimeterNoise);

    % Log the actual orientation and position
    [actP(ii,:), actQ(ii)] = pose(fusionfilt);

    % Log the expected orientation and position
    expQ(ii) = q;
    expP(ii,:) = pos;

    if usePoseViewer
        hfunc.view(actP(ii,:), actQ(ii),expP(ii,:), expQ(ii)); %#ok<*UNRCH>

    if applyXYConstraint
        % If applyXYContraint is true that means the X and Y positions
        % are known with full confidence. Therefore, the measurement noise
        % can be set to 0 in the correct function.
        correct(fusionfilt, 5:6, [0 0], 0);

Plot Filter Performance

Plot the performance of the filter. The display shows the error in the orientation using quaternion distance and position error. The Z-position error should be low because there exists a direct measurement of the position in the altimeter output.

When applyXYConstraint is false the X and Y positions have large, increasing errors because they are purely a double integration of the accelerometer output. There is no sensor which corrects the X and Y position measurements in this scenario. This illustrates why it is generally not advisable to use an accelerometer alone to determine position.

When applyXYConstraint is set to true the X and Y positions have small errors.

hfunc.plotErrs(actP, actQ, expP, expQ, applyXYConstraint);


This example shows how to repurpose the insfilter to perform 10-axis sensor fusion for height and orientation, without requiring a GPS. The example also illustrates the perils of determining position simply by double integrating an accelerometer output.