Exponenta Banner

Автоматическая Настройка Фильтра insfilterAsync

Скрипт Open Live Script

insfilterAsync объектом является расширенный Фильтр Калмана комплекса, который оценивает положение устройства. Однако вручную настройка фильтра или нахождение оптимальных значений для шумовых параметров могут быть сложной задачей. Этот пример иллюстрирует, как использовать tune функция, чтобы оптимизировать параметры шума фильтра.

Траектория и Setup датчика

Проиллюстрировать настраивающий процесс insfilterAsync отфильтруйте, используйте простую случайную waypoint траекторию. imuSensor и gpsSensor объекты создают входные параметры для фильтра.

% The IMU runs at 100 Hz and the GPS runs at 1 Hz.
imurate = 100;
gpsrate = 1;
decim = imurate/gpsrate;

% Create a random waypoint trajectory.
rng(1)
Npts = 4; % number of waypoints
wpPer = 5; % time between waypoints
tstart = 0;
tend = wpPer*(Npts -1);
wp = waypointTrajectory('Waypoints',5*rand(Npts,3), ...
        'TimeOfArrival',tstart:wpPer:tend, ...
        'Orientation',[quaternion.ones; randrot(Npts-1,1)], ...
        'SampleRate', imurate);

[Position,Orientation,Velocity,Acceleration,AngularVelocity] = lookupPose(...
    wp, tstart:(1/imurate):tend);

% Set up an IMU and process the trajectory.
imu = imuSensor('SampleRate',imurate);
loadparams(imu,fullfile(matlabroot, ...
    "toolbox","shared","positioning","positioningdata","generic.json"), ...
    "GenericLowCost9Axis");
[Accelerometer, Gyroscope, Magnetometer] = imu(Acceleration, ...
    AngularVelocity, Orientation);
imuData = timetable(Accelerometer,Gyroscope,Magnetometer,'SampleRate',imurate);

% Set up a GPS sensor and process the trajectory.
gps = gpsSensor('SampleRate', gpsrate,'DecayFactor',0.5, ...
    'HorizontalPositionAccuracy',1.6,'VerticalPositionAccuracy',1.6, ...
    'VelocityAccuracy',0.1);
[GPSPosition,GPSVelocity] = gps(Position(1:decim:end,:), Velocity(1:decim:end,:));
gpsData = timetable(GPSPosition,GPSVelocity,'SampleRate',gpsrate);

% Create a timetable for the tune function.
sensorData = synchronize(imuData,gpsData);

% Create a timetable capturing the ground truth pose.
groundTruth = timetable(Position,Orientation,'SampleRate',imurate);

Создайте фильтр

insfilterAsync фильтр объединяет данные от нескольких датчиков, действующих асинхронно

filtUntuned = insfilterAsync;

Определите начальные условия фильтра

Установите начальные значения для State и StateCovariance свойства на основе основной истины. Обычно, чтобы получить начальные значения, вы использовали бы первые несколько выборок sensorData наряду с калибровочными стандартными программами. Однако в этом примере groundTruth используется, чтобы установить начальное состояние для быстрой сходимости фильтра.

idx = stateinfo(filtUntuned);
filtUntuned.State(idx.Orientation) = compact(Orientation(1));
filtUntuned.State(idx.AngularVelocity) = AngularVelocity(1,:);
filtUntuned.State(idx.Position) = Position(1,:);
filtUntuned.State(idx.Velocity) = Velocity(1,:);
filtUntuned.State(idx.Acceleration) = Acceleration(1,:);
filtUntuned.State(idx.AccelerometerBias) = imu.Accelerometer.ConstantBias;
filtUntuned.State(idx.GyroscopeBias) = imu.Gyroscope.ConstantBias;
filtUntuned.State(idx.GeomagneticFieldVector) = imu.MagneticField;
filtUntuned.State(idx.MagnetometerBias) = imu.Magnetometer.ConstantBias;
filtUntuned.StateCovariance = 1e-5*eye(numel(filtUntuned.State));

% Create a copy of the filtUntuned object for tuning later.
filtTuned = copy(filtUntuned);

Процесс sensorData с ненастроенным фильтром

Используйте tunernoise функция, чтобы создать шумы измерения, которые также должны быть настроены. Чтобы проиллюстрировать необходимость настройки, сначала используйте фильтр его параметрами по умолчанию. 

mn = tunernoise('insfilterAsync');
[posUntunedEst, orientUntunedEst] = fuse(filtUntuned, sensorData, mn);

Настройте фильтр и процесс sensorData

Используйте tune функция, чтобы минимизировать ошибку среднеквадратического корня (RMS) между groundTruth и оценки состояния.

cfg = tunerconfig(class(filtTuned),'MaxIterations',15,'StepForward',1.1);
tunedmn = tune(filtTuned,mn,sensorData,groundTruth,cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.5898
    1            GyroscopeNoise            4.5694
    1            MagnetometerNoise         4.5481
    1            GPSPositionNoise          4.4737
    1            GPSVelocityNoise          4.2984
    1            QuaternionNoise           4.2984
    1            AngularVelocityNoise      3.9668
    1            PositionNoise             3.9668
    1            VelocityNoise             3.9668
    1            AccelerationNoise         3.9556
    1            GyroscopeBiasNoise        3.9556
    1            AccelerometerBiasNoise    3.9511
    1            GeomagneticVectorNoise    3.9511
    1            MagnetometerBiasNoise     3.9360
    2            AccelerometerNoise        3.9360
    2            GyroscopeNoise            3.9360
    2            MagnetometerNoise         3.9064
    2            GPSPositionNoise          3.9064
    2            GPSVelocityNoise          3.7129
    2            QuaternionNoise           3.7122
    2            AngularVelocityNoise      3.0116
    2            PositionNoise             3.0116
    2            VelocityNoise             3.0116
    2            AccelerationNoise         2.9850
    2            GyroscopeBiasNoise        2.9850
    2            AccelerometerBiasNoise    2.9824
    2            GeomagneticVectorNoise    2.9824
    2            MagnetometerBiasNoise     2.9821
    3            AccelerometerNoise        2.9821
    3            GyroscopeNoise            2.9613
    3            MagnetometerNoise         2.9432
    3            GPSPositionNoise          2.9432
    3            GPSVelocityNoise          2.8373
    3            QuaternionNoise           2.8369
    3            AngularVelocityNoise      2.6993
    3            PositionNoise             2.6993
    3            VelocityNoise             2.6993
    3            AccelerationNoise         2.6993
    3            GyroscopeBiasNoise        2.6993
    3            AccelerometerBiasNoise    2.6971
    3            GeomagneticVectorNoise    2.6971
    3            MagnetometerBiasNoise     2.6955
    4            AccelerometerNoise        2.6941
    4            GyroscopeNoise            2.6797
    4            MagnetometerNoise         2.6676
    4            GPSPositionNoise          2.6626
    4            GPSVelocityNoise          2.5530
    4            QuaternionNoise           2.5530
    4            AngularVelocityNoise      2.4285
    4            PositionNoise             2.4285
    4            VelocityNoise             2.4285
    4            AccelerationNoise         2.4232
    4            GyroscopeBiasNoise        2.4232
    4            AccelerometerBiasNoise    2.4217
    4            GeomagneticVectorNoise    2.4217
    4            MagnetometerBiasNoise     2.4023
    5            AccelerometerNoise        2.4019
    5            GyroscopeNoise            2.3900
    5            MagnetometerNoise         2.3900
    5            GPSPositionNoise          2.3887
    5            GPSVelocityNoise          2.2986
    5            QuaternionNoise           2.2986
    5            AngularVelocityNoise      2.1945
    5            PositionNoise             2.1945
    5            VelocityNoise             2.1945
    5            AccelerationNoise         2.1863
    5            GyroscopeBiasNoise        2.1863
    5            AccelerometerBiasNoise    2.1856
    5            GeomagneticVectorNoise    2.1856
    5            MagnetometerBiasNoise     2.1615
    6            AccelerometerNoise        2.1613
    6            GyroscopeNoise            2.1393
    6            MagnetometerNoise         2.1199
    6            GPSPositionNoise          2.1112
    6            GPSVelocityNoise          2.0140
    6            QuaternionNoise           2.0140
    6            AngularVelocityNoise      1.9288
    6            PositionNoise             1.9288
    6            VelocityNoise             1.9288
    6            AccelerationNoise         1.9242
    6            GyroscopeBiasNoise        1.9242
    6            AccelerometerBiasNoise    1.9216
    6            GeomagneticVectorNoise    1.9216
    6            MagnetometerBiasNoise     1.9054
    7            AccelerometerNoise        1.9047
    7            GyroscopeNoise            1.8887
    7            MagnetometerNoise         1.8820
    7            GPSPositionNoise          1.8803
    7            GPSVelocityNoise          1.7713
    7            QuaternionNoise           1.7712
    7            AngularVelocityNoise      1.7145
    7            PositionNoise             1.7145
    7            VelocityNoise             1.7145
    7            AccelerationNoise         1.7122
    7            GyroscopeBiasNoise        1.7122
    7            AccelerometerBiasNoise    1.7112
    7            GeomagneticVectorNoise    1.7112
    7            MagnetometerBiasNoise     1.6873
    8            AccelerometerNoise        1.6853
    8            GyroscopeNoise            1.6790
    8            MagnetometerNoise         1.6694
    8            GPSPositionNoise          1.6565
    8            GPSVelocityNoise          1.5700
    8            QuaternionNoise           1.5676
    8            AngularVelocityNoise      1.5366
    8            PositionNoise             1.5366
    8            VelocityNoise             1.5366
    8            AccelerationNoise         1.5366
    8            GyroscopeBiasNoise        1.5366
    8            AccelerometerBiasNoise    1.5353
    8            GeomagneticVectorNoise    1.5337
    8            MagnetometerBiasNoise     1.5166
    9            AccelerometerNoise        1.5129
    9            GyroscopeNoise            1.5117
    9            MagnetometerNoise         1.5114
    9            GPSPositionNoise          1.4953
    9            GPSVelocityNoise          1.4106
    9            QuaternionNoise           1.4106
    9            AngularVelocityNoise      1.3742
    9            PositionNoise             1.3742
    9            VelocityNoise             1.3742
    9            AccelerationNoise         1.3731
    9            GyroscopeBiasNoise        1.3731
    9            AccelerometerBiasNoise    1.3715
    9            GeomagneticVectorNoise    1.3715
    9            MagnetometerBiasNoise     1.3576
    10           AccelerometerNoise        1.3528
    10           GyroscopeNoise            1.3528
    10           MagnetometerNoise         1.3510
    10           GPSPositionNoise          1.3322
    10           GPSVelocityNoise          1.2512
    10           QuaternionNoise           1.2512
    10           AngularVelocityNoise      1.2507
    10           PositionNoise             1.2507
    10           VelocityNoise             1.2507
    10           AccelerationNoise         1.2497
    10           GyroscopeBiasNoise        1.2497
    10           AccelerometerBiasNoise    1.2480
    10           GeomagneticVectorNoise    1.2480
    10           MagnetometerBiasNoise     1.2301
    11           AccelerometerNoise        1.2233
    11           GyroscopeNoise            1.2222
    11           MagnetometerNoise         1.2220
    11           GPSPositionNoise          1.2008
    11           GPSVelocityNoise          1.1043
    11           QuaternionNoise           1.1043
    11           AngularVelocityNoise      1.1038
    11           PositionNoise             1.1038
    11           VelocityNoise             1.1038
    11           AccelerationNoise         1.1028
    11           GyroscopeBiasNoise        1.1028
    11           AccelerometerBiasNoise    1.1013
    11           GeomagneticVectorNoise    1.1013
    11           MagnetometerBiasNoise     1.0867
    12           AccelerometerNoise        1.0782
    12           GyroscopeNoise            1.0767
    12           MagnetometerNoise         1.0733
    12           GPSPositionNoise          1.0505
    12           GPSVelocityNoise          0.9564
    12           QuaternionNoise           0.9563
    12           AngularVelocityNoise      0.9563
    12           PositionNoise             0.9563
    12           VelocityNoise             0.9563
    12           AccelerationNoise         0.9550
    12           GyroscopeBiasNoise        0.9550
    12           AccelerometerBiasNoise    0.9534
    12           GeomagneticVectorNoise    0.9534
    12           MagnetometerBiasNoise     0.9402
    13           AccelerometerNoise        0.9303
    13           GyroscopeNoise            0.9291
    13           MagnetometerNoise         0.9269
    13           GPSPositionNoise          0.9036
    13           GPSVelocityNoise          0.8072
    13           QuaternionNoise           0.8071
    13           AngularVelocityNoise      0.8071
    13           PositionNoise             0.8071
    13           VelocityNoise             0.8071
    13           AccelerationNoise         0.8065
    13           GyroscopeBiasNoise        0.8065
    13           AccelerometerBiasNoise    0.8052
    13           GeomagneticVectorNoise    0.8052
    13           MagnetometerBiasNoise     0.7901
    14           AccelerometerNoise        0.7806
    14           GyroscopeNoise            0.7806
    14           MagnetometerNoise         0.7768
    14           GPSPositionNoise          0.7547
    14           GPSVelocityNoise          0.6932
    14           QuaternionNoise           0.6930
    14           AngularVelocityNoise      0.6923
    14           PositionNoise             0.6923
    14           VelocityNoise             0.6923
    14           AccelerationNoise         0.6906
    14           GyroscopeBiasNoise        0.6906
    14           AccelerometerBiasNoise    0.6896
    14           GeomagneticVectorNoise    0.6896
    14           MagnetometerBiasNoise     0.6801
    15           AccelerometerNoise        0.6704
    15           GyroscopeNoise            0.6676
    15           MagnetometerNoise         0.6653
    15           GPSPositionNoise          0.6469
    15           GPSVelocityNoise          0.6047
    15           QuaternionNoise           0.6047
    15           AngularVelocityNoise      0.6037
    15           PositionNoise             0.6037
    15           VelocityNoise             0.6037
    15           AccelerationNoise         0.6019
    15           GyroscopeBiasNoise        0.6019
    15           AccelerometerBiasNoise    0.6015
    15           GeomagneticVectorNoise    0.6015
    15           MagnetometerBiasNoise     0.5913

[posTunedEst, orientTunedEst] = fuse(filtTuned,sensorData,tunedmn);

Сравните настроенный по сравнению с ненастроенным фильтром

Постройте оценки положения от настроенных и ненастроенных фильтров наряду с положениями основной истины. Затем постройте ошибку ориентации (расстояние кватерниона) в градусах и для настроенных и для ненастроенных фильтров. Настроенный фильтр оценивает положение и ориентацию лучше, чем ненастроенный фильтр.

% Position error
figure;
t = sensorData.Time;
subplot(3,1,1);
plot(t, [posTunedEst(:,1) posUntunedEst(:,1) Position(:,1)]);
title('Position');
ylabel('X-axis');
legend('Tuned', 'Untuned', 'Truth');
subplot(3,1,2);
plot(t, [posTunedEst(:,2) posUntunedEst(:,2) Position(:,2)]);
ylabel('Y-axis');
subplot(3,1,3);
plot(t, [posTunedEst(:,3) posUntunedEst(:,3) Position(:,3)]);
ylabel('Z-axis');

% Orientation Error
figure;
plot(t, rad2deg(dist(Orientation, orientTunedEst)), ...
    t, rad2deg(dist(Orientation, orientUntunedEst)));
title('Orientation Error');
ylabel('Degrees');
legend('Tuned', 'Untuned');

Документация Navigation Toolbox

Поддержка