tune
Syntax
Description
tunes the tunedMeasureNoise
= tune(filter
,measureNoise
,sensorData
,groundTruth
)AdditiveProcessNoise
property of the
insEKF
filter object filter
, and the measurement
noise, 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 using a tunedMeasureNoise
= tune(___,config
)tunerconfig
object
config
, in addition to all input arguments from the previous syntax.
Examples
Tune insEKF
to Optimize Orientation Estimation
Load the recorded sensor data and ground truth data.
load("accelGyroINSEKFData.mat");
Create an insEKF
filter object. Specify the orientation part of the state in the filter using the initial orientation from the measurement data. Specify the diagonal elements of the state estimate error covariance matrix corresponding to the orientation state as 0.01
.
filt = insEKF; stateparts(filt,"Orientation",compact(initOrient)); statecovparts(filt,"Orientation",1e-2);
Obtain a representative measurement noise structure and use it to estimate states before tuning.
mnoise = tunernoise(filt); untunedEst = estimateStates(filt,sensorData,mnoise);
Reinitialize the filter, set up a tunerconfig
object, and tune the filter.
stateparts(filt,"Orientation",compact(initOrient)); statecovparts(filt,"Orientation",1e-2); cfg = tunerconfig(filt,MaxIterations=10,ObjectiveLimit=1e-4); tunedmn = tune(filt,mnoise,sensorData,groundTruth,cfg);
Iteration Parameter Metric _________ _________ ______ 1 AdditiveProcessNoise(1) 0.3787 1 AdditiveProcessNoise(15) 0.3761 1 AdditiveProcessNoise(29) 0.3695 1 AdditiveProcessNoise(43) 0.3655 1 AdditiveProcessNoise(57) 0.3533 1 AdditiveProcessNoise(71) 0.3446 1 AdditiveProcessNoise(85) 0.3431 1 AdditiveProcessNoise(99) 0.3428 1 AdditiveProcessNoise(113) 0.3427 1 AdditiveProcessNoise(127) 0.3426 1 AdditiveProcessNoise(141) 0.3298 1 AdditiveProcessNoise(155) 0.3206 1 AdditiveProcessNoise(169) 0.3200 1 AccelerometerNoise 0.3199 1 GyroscopeNoise 0.3198 2 AdditiveProcessNoise(1) 0.3126 2 AdditiveProcessNoise(15) 0.3098 2 AdditiveProcessNoise(29) 0.3018 2 AdditiveProcessNoise(43) 0.2988 2 AdditiveProcessNoise(57) 0.2851 2 AdditiveProcessNoise(71) 0.2784 2 AdditiveProcessNoise(85) 0.2760 2 AdditiveProcessNoise(99) 0.2744 2 AdditiveProcessNoise(113) 0.2744 2 AdditiveProcessNoise(127) 0.2743 2 AdditiveProcessNoise(141) 0.2602 2 AdditiveProcessNoise(155) 0.2537 2 AdditiveProcessNoise(169) 0.2527 2 AccelerometerNoise 0.2524 2 GyroscopeNoise 0.2524 3 AdditiveProcessNoise(1) 0.2476 3 AdditiveProcessNoise(15) 0.2432 3 AdditiveProcessNoise(29) 0.2397 3 AdditiveProcessNoise(43) 0.2381 3 AdditiveProcessNoise(57) 0.2255 3 AdditiveProcessNoise(71) 0.2226 3 AdditiveProcessNoise(85) 0.2221 3 AdditiveProcessNoise(99) 0.2202 3 AdditiveProcessNoise(113) 0.2201 3 AdditiveProcessNoise(127) 0.2201 3 AdditiveProcessNoise(141) 0.2090 3 AdditiveProcessNoise(155) 0.2070 3 AdditiveProcessNoise(169) 0.2058 3 AccelerometerNoise 0.2052 3 GyroscopeNoise 0.2052 4 AdditiveProcessNoise(1) 0.2051 4 AdditiveProcessNoise(15) 0.2027 4 AdditiveProcessNoise(29) 0.2019 4 AdditiveProcessNoise(43) 0.2000 4 AdditiveProcessNoise(57) 0.1909 4 AdditiveProcessNoise(71) 0.1897 4 AdditiveProcessNoise(85) 0.1882 4 AdditiveProcessNoise(99) 0.1871 4 AdditiveProcessNoise(113) 0.1870 4 AdditiveProcessNoise(127) 0.1870 4 AdditiveProcessNoise(141) 0.1791 4 AdditiveProcessNoise(155) 0.1783 4 AdditiveProcessNoise(169) 0.1751 4 AccelerometerNoise 0.1748 4 GyroscopeNoise 0.1747 5 AdditiveProcessNoise(1) 0.1742 5 AdditiveProcessNoise(15) 0.1732 5 AdditiveProcessNoise(29) 0.1712 5 AdditiveProcessNoise(43) 0.1712 5 AdditiveProcessNoise(57) 0.1626 5 AdditiveProcessNoise(71) 0.1615 5 AdditiveProcessNoise(85) 0.1598 5 AdditiveProcessNoise(99) 0.1590 5 AdditiveProcessNoise(113) 0.1589 5 AdditiveProcessNoise(127) 0.1589 5 AdditiveProcessNoise(141) 0.1517 5 AdditiveProcessNoise(155) 0.1508 5 AdditiveProcessNoise(169) 0.1476 5 AccelerometerNoise 0.1473 5 GyroscopeNoise 0.1470 6 AdditiveProcessNoise(1) 0.1470 6 AdditiveProcessNoise(15) 0.1470 6 AdditiveProcessNoise(29) 0.1463 6 AdditiveProcessNoise(43) 0.1462 6 AdditiveProcessNoise(57) 0.1367 6 AdditiveProcessNoise(71) 0.1360 6 AdditiveProcessNoise(85) 0.1360 6 AdditiveProcessNoise(99) 0.1350 6 AdditiveProcessNoise(113) 0.1350 6 AdditiveProcessNoise(127) 0.1350 6 AdditiveProcessNoise(141) 0.1289 6 AdditiveProcessNoise(155) 0.1288 6 AdditiveProcessNoise(169) 0.1262 6 AccelerometerNoise 0.1253 6 GyroscopeNoise 0.1246 7 AdditiveProcessNoise(1) 0.1246 7 AdditiveProcessNoise(15) 0.1244 7 AdditiveProcessNoise(29) 0.1205 7 AdditiveProcessNoise(43) 0.1203 7 AdditiveProcessNoise(57) 0.1125 7 AdditiveProcessNoise(71) 0.1122 7 AdditiveProcessNoise(85) 0.1117 7 AdditiveProcessNoise(99) 0.1106 7 AdditiveProcessNoise(113) 0.1104 7 AdditiveProcessNoise(127) 0.1104 7 AdditiveProcessNoise(141) 0.1058 7 AdditiveProcessNoise(155) 0.1052 7 AdditiveProcessNoise(169) 0.1035 7 AccelerometerNoise 0.1024 7 GyroscopeNoise 0.1014 8 AdditiveProcessNoise(1) 0.1014 8 AdditiveProcessNoise(15) 0.1012 8 AdditiveProcessNoise(29) 0.1012 8 AdditiveProcessNoise(43) 0.1005 8 AdditiveProcessNoise(57) 0.0948 8 AdditiveProcessNoise(71) 0.0948 8 AdditiveProcessNoise(85) 0.0938 8 AdditiveProcessNoise(99) 0.0934 8 AdditiveProcessNoise(113) 0.0931 8 AdditiveProcessNoise(127) 0.0931 8 AdditiveProcessNoise(141) 0.0896 8 AdditiveProcessNoise(155) 0.0889 8 AdditiveProcessNoise(169) 0.0867 8 AccelerometerNoise 0.0859 8 GyroscopeNoise 0.0851 9 AdditiveProcessNoise(1) 0.0851 9 AdditiveProcessNoise(15) 0.0850 9 AdditiveProcessNoise(29) 0.0824 9 AdditiveProcessNoise(43) 0.0819 9 AdditiveProcessNoise(57) 0.0771 9 AdditiveProcessNoise(71) 0.0771 9 AdditiveProcessNoise(85) 0.0762 9 AdditiveProcessNoise(99) 0.0759 9 AdditiveProcessNoise(113) 0.0754 9 AdditiveProcessNoise(127) 0.0754 9 AdditiveProcessNoise(141) 0.0734 9 AdditiveProcessNoise(155) 0.0724 9 AdditiveProcessNoise(169) 0.0702 9 AccelerometerNoise 0.0697 9 GyroscopeNoise 0.0689 10 AdditiveProcessNoise(1) 0.0689 10 AdditiveProcessNoise(15) 0.0686 10 AdditiveProcessNoise(29) 0.0658 10 AdditiveProcessNoise(43) 0.0655 10 AdditiveProcessNoise(57) 0.0622 10 AdditiveProcessNoise(71) 0.0620 10 AdditiveProcessNoise(85) 0.0616 10 AdditiveProcessNoise(99) 0.0615 10 AdditiveProcessNoise(113) 0.0607 10 AdditiveProcessNoise(127) 0.0606 10 AdditiveProcessNoise(141) 0.0590 10 AdditiveProcessNoise(155) 0.0578 10 AdditiveProcessNoise(169) 0.0565 10 AccelerometerNoise 0.0562 10 GyroscopeNoise 0.0557
Estimate states again, this time using the tuned filter.
tunedEst = estimateStates(filt,sensorData,tunedmn);
Compare the tuned and untuned estimates against the ground truth data.
times = groundTruth.Properties.RowTimes; duntuned = rad2deg(dist(untunedEst.Orientation,groundTruth.Orientation)); dtuned = rad2deg(dist(tunedEst.Orientation,groundTruth.Orientation)); plot(times,duntuned,times,dtuned); xlabel("Time (sec)") ylabel("Error (deg)") legend("Untuned","Tuned") title("Filter Orientation Error")
Print the root-mean-squared (RMS) error of both the untuned and the tuned filters.
untunedRMSError = sqrt(mean(duntuned.^2)); tunedRMSError = sqrt(mean(dtuned.^2)); fprintf("Untuned RMS error: %.2f degrees\n", ... untunedRMSError);
Untuned RMS error: 39.47 degrees
fprintf("Tuned RMS error: %.2f degrees\n", ... tunedRMSError);
Tuned RMS error: 6.39 degrees
Input Arguments
filter
— INS filter
insEKF
object
INS filter, specified as an insEKF
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 should
contain the measurement noise for sensor models specified in the
Sensors
property of the INS filter. For example, if the
insEKF
filter object only contains an insAccelerometer
object and an insGyroscope
object, you should specify the structure like this:
Field name | Description |
---|---|
AccelerometerNoise | Variance of accelerometer noise, specified as a scalar in (m/s2)2. |
GyroscopeNoise | Variance of gyroscope noise, specified as a scalar in (rad/s)2. |
Tip
Use the tunernoise
function to obtain a
representative structure for the measureNoise
structure. For
example:
filter = insEKF; mNoise = tunerNoise(filter)
sensorData
— Sensor data
timetable
Sensor data, specified as a timetable
. Each variable name (as a column)
in the time table must match one of the sensor names specified in the
SensorNames
property of the filter
. Each
entry in the table is the measurement from the sensor at the corresponding row time.
If a sensor does not produce measurements at the row time, 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.
groundTruth
— Ground truth data
timetable
Ground truth data, specified as a timetable
. In each row, the table contains the truth data for the row time.
Each variable name (as a column) in the table must be one of the filter state names that
you can obtain using the stateinfo
object
function.
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 row times.
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 the same
fields as the structure specified in the measureNoise
input.
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 R2022a
See Also
tunerconfig
| tunernoise
| predict
| fuse
| residual
| correct
| stateparts
| statecovparts
| stateinfo
| estimateStates
| createTunerCostTemplate
| tunerCostFcnParam
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)