tune
Syntax
Description
adjusts the properties of the tunedMeasureNoise
= tune(filter
,measureNoise
,sensorData
,groundTruth
)insfilterMARG
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.
specifies the tuning configuration based on a tunedMeasureNoise
= tune(___,config
)tunerconfig
object,
config
.
Examples
Tune insfilterMARG
to Optimize Pose Estimate
Load the recorded sensor data and ground truth data.
load('insfilterMARGTuneData.mat');
Create tables for the sensor data and the truth data.
sensorData = table(Accelerometer, Gyroscope, ...
Magnetometer, GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);
Create an insfilterMARG
filter object that has a few noise properties.
filter = insfilterMARG('State',initialState,... 'StateCovariance',initialStateCovariance,... 'AccelerometerBiasNoise',1e-7,... 'GyroscopeBiasNoise',1e-7,... 'MagnetometerBiasNoise',1e-7,... 'GeomagneticVectorNoise',1e-7);
Create a tuner configuration object for the filter. Set the maximum iterations to eight. Also, set the tunable parameters.
cfg = tunerconfig('insfilterMARG', 'MaxIterations', 8); cfg.TunableParameters = setdiff(cfg.TunableParameters, ... {'GeomagneticFieldVector', 'AccelerometerBiasNoise', ... 'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});
Use the tuner noise function to obtain a set of initial sensor noises used in the filter.
measNoise = tunernoise('insfilterMARG')
measNoise = struct with fields:
MagnetometerNoise: 1
GPSPositionNoise: 1
GPSVelocityNoise: 1
Tune the filter and obtain the tuned parameters.
tunedParams = tune(filter, measNoise, sensorData, ...
groundTruth, cfg);
Iteration Parameter Metric _________ _________ ______ 1 AccelerometerNoise 2.5701 1 GPSPositionNoise 2.5446 1 GPSVelocityNoise 2.5279 1 GeomagneticVectorNoise 2.5268 1 GyroscopeNoise 2.5268 1 MagnetometerNoise 2.5204 2 AccelerometerNoise 2.5203 2 GPSPositionNoise 2.4908 2 GPSVelocityNoise 2.4695 2 GeomagneticVectorNoise 2.4684 2 GyroscopeNoise 2.4684 2 MagnetometerNoise 2.4615 3 AccelerometerNoise 2.4615 3 GPSPositionNoise 2.4265 3 GPSVelocityNoise 2.4000 3 GeomagneticVectorNoise 2.3988 3 GyroscopeNoise 2.3988 3 MagnetometerNoise 2.3911 4 AccelerometerNoise 2.3911 4 GPSPositionNoise 2.3500 4 GPSVelocityNoise 2.3164 4 GeomagneticVectorNoise 2.3153 4 GyroscopeNoise 2.3153 4 MagnetometerNoise 2.3068 5 AccelerometerNoise 2.3068 5 GPSPositionNoise 2.2587 5 GPSVelocityNoise 2.2166 5 GeomagneticVectorNoise 2.2154 5 GyroscopeNoise 2.2154 5 MagnetometerNoise 2.2063 6 AccelerometerNoise 2.2063 6 GPSPositionNoise 2.1505 6 GPSVelocityNoise 2.0981 6 GeomagneticVectorNoise 2.0971 6 GyroscopeNoise 2.0971 6 MagnetometerNoise 2.0875 7 AccelerometerNoise 2.0874 7 GPSPositionNoise 2.0240 7 GPSVelocityNoise 1.9601 7 GeomagneticVectorNoise 1.9594 7 GyroscopeNoise 1.9594 7 MagnetometerNoise 1.9499 8 AccelerometerNoise 1.9499 8 GPSPositionNoise 1.8802 8 GPSVelocityNoise 1.8035 8 GeomagneticVectorNoise 1.8032 8 GyroscopeNoise 1.8032 8 MagnetometerNoise 1.7959
Fuse the sensor data using the tuned filter.
N = size(sensorData,1); qEstTuned = quaternion.zeros(N,1); posEstTuned = zeros(N,3); for ii=1:N predict(filter,Accelerometer(ii,:),Gyroscope(ii,:)); if all(~isnan(Magnetometer(ii,1))) fusemag(filter,Magnetometer(ii,:),... tunedParams.MagnetometerNoise); end if all(~isnan(GPSPosition(ii,1))) fusegps(filter,GPSPosition(ii,:),... tunedParams.GPSPositionNoise,GPSVelocity(ii,:),... tunedParams.GPSVelocityNoise); end [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter); end
Compute the RMS errors.
orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation)); rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 0.8580
positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2)); rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))
rmsPositionErrorTuned = 1.7946
Visualize the results.
figure(); t = (0:N-1)./filter.IMUSampleRate; subplot(2,1,1) plot(t,positionErrorTuned,'b'); title("Tuned insfilterMARG" + newline + ... "Euclidean Distance Position 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)');
Input Arguments
filter
— Filter object
infilterMARG
object
Filter object, specified as an insfilterMARG
object.
measureNoise
— Measurement noise
structure
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 name | Description |
---|---|
MagnetometerNoise | Variance of magnetometer noise, specified as a scalar in (μT)2 |
GPSPositionNoise | Variance of GPS position noise, specified as a scalar in m2 |
GPSVelocityNoise | Variance of GPS velocity noise, specified as a scalar in (m/s)2 |
sensorData
— Sensor data
table
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.GPSPosition
— GPS position data, specified as a 1-by-3 vector of scalars in [degrees, degrees, meters].GPSVelocity
— GPS velocity data, specified as a 1-by-3 vector of scalars in m/s.
If the GPS sensor does not produce complete measurements, specify the
corresponding entry for GPSPosition
and/or
GPSVelocity
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.
groundTruth
— Ground truth data
table
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 aquaternion
or a 3-by-3 rotation matrix.Position
— Position in navigation frame, specified as a 1-by-3 vector of scalars in meters.Velocity
— Velocity in navigation frame, specified as a 1-by-3 vector of scalars 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.
config
— Tuner configuration
tunerconfig
object
Tuner configuration, specified as a tunerconfig
object.
Output Arguments
tunedMeasureNoise
— Tuned measurement noise
structure
Tuned measurement noise, returned as a structure. The structure contains these fields.
Field name | Description |
---|---|
MagnetometerNoise | Variance of magnetometer noise, specified as a scalar in (μT)2 |
GPSPositionNoise | Variance of GPS position noise, specified as a scalar in m2 |
GPSVelocityNoise | Variance of GPS velocity noise, specified as a scalar in (m/s)2 |
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
MATLAB 命令
您点击的链接对应于以下 MATLAB 命令:
请在 MATLAB 命令行窗口中直接输入以执行命令。Web 浏览器不支持 MATLAB 命令。
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)