ahrs10filter

Height and orientation from MARG and altimeter readings

Description

The ahrs10filter object fuses MARG and altimeter sensor data to estimate device height and orientation. MARG (magnetic, angular rate, gravity) data is typically derived from magnetometer, gyroscope, and accelerometer sensors. The filter uses an 18-element state vector to track the orientation quaternion, vertical velocity, vertical position, MARG sensor biases, and geomagnetic vector. The ahrs10filter object uses an extended Kalman filter to estimate these quantities.

Creation

Syntax

FUSE = ahrs10filter

FUSE = ahrs10filter('ReferenceFrame',RF)

FUSE = ahrs10filter(___,Name=Value)

Description

FUSE = ahrs10filter returns an extended Kalman filter object, FUSE, for sensor fusion of MARG and altimeter readings to estimate device height and orientation.

FUSE = ahrs10filter('ReferenceFrame',RF) returns an extended Kalman filter object that estimates device height and orientation relative to the reference frame RF.

FUSE = ahrs10filter(___,Name=Value) sets one or more properties using name-value arguments in addition to any of the previous input arguments.

Input Arguments

RF — Local navigation coordinate system

'NED' (default) | 'ENU'

Local navigation coordinate system, specified as either 'NED' (North-East-Down) or 'ENU' (East-North-Up).

Data Types: char | string

Properties

IMUSampleRate — Sample rate of the IMU (Hz)

100 (default) | positive scalar

Sample rate of the IMU in Hz, specified as a positive scalar.

Data Types: single | double

GyroscopeNoise — Multiplicative process noise variance from gyroscope ((rad/s)²)

[1e-9,1e-9,1e-9] (default) | scalar | three-element row vector

Multiplicative process noise variance from the gyroscope in (rad/s)², specified as positive real finite numbers.

Data Types: single | double

AccelerometerNoise — Multiplicative process noise variance from accelerometer ((m/s²)²)

[1e-4,1e-4,1e-4] (default) | scalar | three-element row vector

Multiplicative process noise variance from the accelerometer in (m/s²)², specified as positive real finite numbers.

Data Types: single | double

GyroscopeBiasNoise — Multiplicative process noise variance from gyroscope bias ((rad/s²)²)

[1e-10,1e-10,1e-10] (default) | scalar | three-element row vector

Multiplicative process noise variance from the gyroscope bias in (rad/s²)², specified as positive real finite numbers.

Data Types: single | double

AccelerometerBiasNoise — Multiplicative process noise variance from accelerometer bias ((m/s²)²)

[1e-4,1e-4,1e-4] (default) | scalar | three-element row vector

Multiplicative process noise variance from the accelerometer bias in (m/s²)², specified as positive real finite numbers.

Data Types: single | double

GeomagneticVectorNoise — Additive process noise for geomagnetic vector (μT²)

[1e-6,1e-6,1e-6] (default) | scalar | three-element row vector

Additive process noise for geomagnetic vector in μT², specified as positive real finite numbers.

Data Types: single | double

MagnetometerBiasNoise — Additive process noise for magnetometer bias (μT²)

[0.1,0.1,0.1] (default) | scalar | three-element row vector

Additive process noise for magnetometer bias in μT², specified as positive real finite numbers.

Data Types: single | double

State — State vector of extended Kalman filter

18-element column vector

State vector of the extended Kalman filter. The state values represent:

State	Units	Index
Orientation (quaternion parts)	N/A	1:4
Altitude (NED or ENU)	m	5
Vertical Velocity (NED or ENU)	m/s	6
Delta Angle Bias (XYZ)	rad/s	7:9
Delta Velocity Bias (XYZ)	m/s	10:12
Geomagnetic Field Vector (NED or ENU)	μT	13:15
Magnetometer Bias (XYZ)	μT	16:18

The default initial state corresponds to an object at rest located at [0 0 0] in geodetic LLA coordinates.

Data Types: single | double

StateCovariance — State error covariance for extended Kalman filter

eye(18)*1e-6 (default) | 18-by-18 matrix

State error covariance for the Kalman filter, specified as an 18-by-18-element matrix of real numbers.

Data Types: single | double

Object Functions

predict	Update states using accelerometer and gyroscope data for ahrs10filter
fusemag	Correct states using magnetometer data for ahrs10filter
fusealtimeter	Correct states using altimeter data for ahrs10filter
correct	Correct states using direct state measurements for ahrs10filter
residual	Residuals and residual covariances from direct state measurements for ahrs10filter
residualmag	Residuals and residual covariance from magnetometer measurements for ahrs10filter
residualaltimeter	Residuals and residual covariance from altimeter measurements for ahrs10filter
pose	Current orientation and position estimate for ahrs10filter
reset	Reset internal states for ahrs10filter
stateinfo	Display state vector information for ahrs10filter
tune	Tune ahrs10filter parameters to reduce estimation error
copy	Create copy of ahrs10filter

Examples

Estimate Pose of UAV

Load logged sensor data, ground truth pose, and initial state and initial state covariance. Calculate the number of IMU samples per altimeter sample and the number of IMU samples per magnetometer sample.

load('fuse10exampledata.mat', ...
     'imuFs','accelData','gyroData', ...
     'magnetometerFs','magData', ...
     'altimeterFs','altData', ...
     'expectedHeight','expectedOrient', ...
     'initstate','initcov');

imuSamplesPerAlt = fix(imuFs/altimeterFs);
imuSamplesPerMag = fix(imuFs/magnetometerFs);

Create an AHRS filter that fuses MARG and altimeter readings to estimate height and orientation. Set the sampling rate and measurement noises of the sensors. The values were determined from datasheets and experimentation.

filt = ahrs10filter('IMUSampleRate',imuFs, ...
                    'AccelerometerNoise',0.1, ...
                    'State',initstate, ...
                    'StateCovariance',initcov);

Ralt = 0.24;
Rmag = 0.9;

Preallocate variables to log height and orientation.

numIMUSamples = size(accelData,1);
estHeight = zeros(numIMUSamples,1);
estOrient = zeros(numIMUSamples,1,'quaternion');

Fuse accelerometer, gyroscope, magnetometer and altimeter data. The outer loop predicts the filter forward at the fastest sample rate (the IMU sample rate).

for ii = 1:numIMUSamples
    
    % Use predict to estimate the filter state based on the accelometer and
    % gyroscope data.
    predict(filt,accelData(ii,:),gyroData(ii,:));
    
    % Magnetometer data is collected at a lower rate than IMU data. Fuse
    % magnetometer data at the lower rate.
    if ~mod(ii,imuSamplesPerMag)
        fusemag(filt,magData(ii,:),Rmag);
    end
    
    % Altimeter data is collected at a lower rate than IMU data. Fuse
    % altimeter data at the lower rate.
    if ~mod(ii, imuSamplesPerAlt)
        fusealtimeter(filt,altData(ii),Ralt);
    end
    
    % Log the current height and orientation estimate.
    [estHeight(ii),estOrient(ii)] = pose(filt);
end

Calculate the RMS errors between the known true height and orientation and the output from the AHRS filter.

pErr = expectedHeight - estHeight;
qErr = rad2deg(dist(expectedOrient,estOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Altitude RMS Error\n');

Altitude RMS Error

fprintf('\t%.2f (meters)\n\n',pRMS);

	0.38 (meters)

Visualize the true and estimated height over time.

t = (0:(numIMUSamples-1))/imuFs;
plot(t,expectedHeight);hold on
plot(t,estHeight);hold off
legend('Ground Truth','Estimated Height','location','best')
ylabel('Height (m)')
xlabel('Time (s)')
grid on

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n',qRMS);

	2.93 (degrees)

Extended Capabilities

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

Version History

Introduced in R2019a