ahrs10filter

Height and orientation from MARG and altimeter readings

expand all in page

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

Open Live Script

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

Figure contains an axes object. The axes object with xlabel Time (s), ylabel Height (m) contains 2 objects of type line. These objects represent Ground Truth, Estimated Height.


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

See Also

ahrsfilter | insfilter