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.
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;
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);
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
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;
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); end % 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); end % 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> end 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); end end
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.