Main Content

tune

Tune ahrs10filter parameters to reduce estimation error

Since R2021a

collapse all in page

Syntax

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)
tunedMeasureNoise = tune(___,config)

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) adjusts the properties of the ahrs10filter filter object, filter, and measurement noises to reduce the root-mean-squared (RMS) state estimation error between the fused sensor data and the ground truth. The function also returns the tuned measurement noise, tunedMeasureNoise. The function uses the property values in the filter and the measurement noise provided in the measureNoise structure as the initial estimate for the optimization algorithm.

tunedMeasureNoise = tune(___,config) specifies the tuning configuration based on a tunerconfig object, config.

example

Examples

collapse all

Open Live Script

Load the recorded sensor data and ground truth data.

load('ahrs10filterTuneData.mat');

Create tables for the sensor data and the truth data.

sensorData = table(Accelerometer,Gyroscope,...
    Magnetometer,Altimeter);
groundTruth = table(Orientation, Altitude);

Create an ahrs10filter filter object.

filter = ahrs10filter('State', initialState, ...
    'StateCovariance', initialStateCovariance);

Create a tuner configuration object for the filter. Set the maximum iterations to ten and set the objective limit to 0.001.

cfg = tunerconfig('ahrs10filter','MaxIterations',10,...
          'ObjectiveLimit',1e-3);

Use the tuner noise function to obtain a set of initial sensor noises used in the filter.

measNoise = tunernoise('ahrs10filter')
measNoise = struct with fields:
    MagnetometerNoise: 1
       AltimeterNoise: 1

Tune the filter and obtain the tuned parameters.

tunedNoise = tune(filter, measNoise, sensorData, ...
    groundTruth, cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        0.0526
    1            GyroscopeNoise            0.0526
    1            MagnetometerNoise         0.0523
    1            AltimeterNoise            0.0515
    1            AccelerometerBiasNoise    0.0510
    1            GyroscopeBiasNoise        0.0510
    1            GeomagneticVectorNoise    0.0510
    1            MagnetometerBiasNoise     0.0508
    2            AccelerometerNoise        0.0508
    2            GyroscopeNoise            0.0508
    2            MagnetometerNoise         0.0504
    2            AltimeterNoise            0.0494
    2            AccelerometerBiasNoise    0.0490
    2            GyroscopeBiasNoise        0.0490
    2            GeomagneticVectorNoise    0.0490
    2            MagnetometerBiasNoise     0.0487
    3            AccelerometerNoise        0.0487
    3            GyroscopeNoise            0.0487
    3            MagnetometerNoise         0.0482
    3            AltimeterNoise            0.0472
    3            AccelerometerBiasNoise    0.0467
    3            GyroscopeBiasNoise        0.0467
    3            GeomagneticVectorNoise    0.0467
    3            MagnetometerBiasNoise     0.0463
    4            AccelerometerNoise        0.0463
    4            GyroscopeNoise            0.0463
    4            MagnetometerNoise         0.0456
    4            AltimeterNoise            0.0446
    4            AccelerometerBiasNoise    0.0442
    4            GyroscopeBiasNoise        0.0442
    4            GeomagneticVectorNoise    0.0442
    4            MagnetometerBiasNoise     0.0437
    5            AccelerometerNoise        0.0437
    5            GyroscopeNoise            0.0437
    5            MagnetometerNoise         0.0428
    5            AltimeterNoise            0.0417
    5            AccelerometerBiasNoise    0.0413
    5            GyroscopeBiasNoise        0.0413
    5            GeomagneticVectorNoise    0.0413
    5            MagnetometerBiasNoise     0.0408
    6            AccelerometerNoise        0.0408
    6            GyroscopeNoise            0.0408
    6            MagnetometerNoise         0.0397
    6            AltimeterNoise            0.0385
    6            AccelerometerBiasNoise    0.0381
    6            GyroscopeBiasNoise        0.0381
    6            GeomagneticVectorNoise    0.0381
    6            MagnetometerBiasNoise     0.0375
    7            AccelerometerNoise        0.0375
    7            GyroscopeNoise            0.0375
    7            MagnetometerNoise         0.0363
    7            AltimeterNoise            0.0351
    7            AccelerometerBiasNoise    0.0347
    7            GyroscopeBiasNoise        0.0347
    7            GeomagneticVectorNoise    0.0347
    7            MagnetometerBiasNoise     0.0342
    8            AccelerometerNoise        0.0342
    8            GyroscopeNoise            0.0342
    8            MagnetometerNoise         0.0331
    8            AltimeterNoise            0.0319
    8            AccelerometerBiasNoise    0.0316
    8            GyroscopeBiasNoise        0.0316
    8            GeomagneticVectorNoise    0.0316
    8            MagnetometerBiasNoise     0.0313
    9            AccelerometerNoise        0.0313
    9            GyroscopeNoise            0.0313
    9            MagnetometerNoise         0.0313
    9            AltimeterNoise            0.0301
    9            AccelerometerBiasNoise    0.0298
    9            GyroscopeBiasNoise        0.0298
    9            GeomagneticVectorNoise    0.0298
    9            MagnetometerBiasNoise     0.0296
    10           AccelerometerNoise        0.0296
    10           GyroscopeNoise            0.0296
    10           MagnetometerNoise         0.0296
    10           AltimeterNoise            0.0285
    10           AccelerometerBiasNoise    0.0283
    10           GyroscopeBiasNoise        0.0283
    10           GeomagneticVectorNoise    0.0283
    10           MagnetometerBiasNoise     0.0282

Fuse the sensor data using the tuned filter.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
altEstTuned = zeros(N,1);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(Magnetometer(ii,1)))
        fusemag(filter, Magnetometer(ii,:),tunedNoise.MagnetometerNoise);
    end
    if ~isnan(Altimeter(ii))
        fusealtimeter(filter, Altimeter(ii),tunedNoise.AltimeterNoise);
    end
    [altEstTuned(ii), qEstTuned(ii)] = pose(filter);
end

Compute the RMS errors.

orientationErrorTuned = rad2deg(dist(qEstTuned, Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 
2.2899

positionErrorTuned = altEstTuned - Altitude;
rmsPositionErrorTuned = sqrt(mean( positionErrorTuned.^2))
rmsPositionErrorTuned = 
0.0199

Visualize the results.

figure;
t = (0:N-1)./ filter.IMUSampleRate;
subplot(2,1,1)
plot(t, positionErrorTuned, 'b');
title("Tuned ahrs10filter" + newline + ...
    "Altitude Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationErrorTuned, 'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Figure contains 2 axes objects. Axes object 1 with title Tuned ahrs10filter Altitude Error, xlabel Time (s), ylabel Position Error (meters) contains an object of type line. Axes object 2 with title Orientation Error, xlabel Time (s), ylabel Orientation Error (degrees) contains an object of type line.

Input Arguments

collapse all

Filter object, specified as an ahrs10filter object.

Measurement noise, specified as a structure. The function uses the measurement noise input as the initial guess for tuning the measurement noise. The structure must contain these fields:

Field nameDescription
MagnetometerNoiseVariance of magnetometer noise, specified as a scalar in (μT)2
AltimeterNoiseVariance of altimeter noise, specified as a scalar in m2

Sensor data, specified as a table. In each row, the sensor data is specified as:

  • Accelerometer — Accelerometer data, specified as a 1-by-3 vector of scalars in m2/s.

  • Gyroscope— Gyroscope data, specified as a 1-by-3 vector of scalars in rad/s.

  • Magnetometer — Magnetometer data, specified as a 1-by-3 vector of scalars in μT.

  • Altimeter — Altimeter data, specified as a scalar in meters.

If the magnetometer does not produce measurements, specify the corresponding entry as NaN. If you set the Cost property of the tuner configuration input, config, to Custom, then you can use other data types for the sensorData input based on your choice.

Ground truth data, specified as a table. In each row, the table can optionally contain any of these variables:

  • Orientation — Orientation from the navigation frame to the body frame, specified as a quaternion or a 3-by-3 rotation matrix.

  • Altitude — Altitude, specified as a scalar in meters.

  • VertialVelocity — Velocity in the vertical direction, specified as a scalar in m/s.

  • DeltaAngleBias — Delta angle bias, specified as a 1-by-3 vector of scalars in radians.

  • DeltaVelocityBias — Delta velocity bias, specified as a 1-by-3 vector of scalars in m/s.

  • GeomagneticFieldVector — Geomagnetic field vector in navigation frame, specified as a 1-by-3 vector of scalars.

  • MagnetometerBias — Magnetometer bias in body frame, specified as a 1-by-3 vector of scalars in μT.

The function processes each row of the sensorData and groundTruth tables sequentially to calculate the state estimate and RMS error from the ground truth. State variables not present in groundTruth input are ignored for the comparison. The sensorData and the groundTruth tables must have the same number of rows.

If you set the Cost property of the tuner configuration input, config, to Custom, then you can use other data types for the groundTruth input based on your choice.

Tuner configuration, specified as a tunerconfig object.

Output Arguments

collapse all

Tuned measurement noise, returned as a structure. The structure contains these fields.

Field nameDescription
MagnetometerNoiseVariance of magnetometer noise, specified as a scalar in (μT)2
AltimeterNoiseVariance of altimeter noise, specified as a scalar in m2

References

[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Kalman Filters. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.

Version History

Introduced in R2021a

See Also

|