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

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.7634
    1            GyroscopeNoise            4.7439
    1            MagnetometerNoise         4.7260
    1            GPSPositionNoise          4.6562
    1            GPSVelocityNoise          4.4895
    1            QuaternionNoise           4.4895
    1            AngularVelocityNoise      4.1764
    1            PositionNoise             4.1764
    1            VelocityNoise             4.1764
    1            AccelerationNoise         4.1657
    1            GyroscopeBiasNoise        4.1657
    1            AccelerometerBiasNoise    4.1615
    1            GeomagneticVectorNoise    4.1615
    1            MagnetometerBiasNoise     4.1466
    2            AccelerometerNoise        4.1466
    2            GyroscopeNoise            4.1466
    2            MagnetometerNoise         4.1218
    2            GPSPositionNoise          4.1218
    2            GPSVelocityNoise          3.9411
    2            QuaternionNoise           3.9404
    2            AngularVelocityNoise      3.2970
    2            PositionNoise             3.2970
    2            VelocityNoise             3.2970
    2            AccelerationNoise         3.2735
    2            GyroscopeBiasNoise        3.2735
    2            AccelerometerBiasNoise    3.2711
    2            GeomagneticVectorNoise    3.2711
    2            MagnetometerBiasNoise     3.2709
    3            AccelerometerNoise        3.2704
    3            GyroscopeNoise            3.2475
    3            MagnetometerNoise         3.2338
    3            GPSPositionNoise          3.2338
    3            GPSVelocityNoise          3.1333
    3            QuaternionNoise           3.1333
    3            AngularVelocityNoise      3.0267
    3            PositionNoise             3.0267
    3            VelocityNoise             3.0267
    3            AccelerationNoise         3.0267
    3            GyroscopeBiasNoise        3.0267
    3            AccelerometerBiasNoise    3.0248
    3            GeomagneticVectorNoise    3.0248
    3            MagnetometerBiasNoise     3.0184
    4            AccelerometerNoise        3.0156
    4            GyroscopeNoise            3.0028
    4            MagnetometerNoise         2.9903
    4            GPSPositionNoise          2.9872
    4            GPSVelocityNoise          2.9043
    4            QuaternionNoise           2.9043
    4            AngularVelocityNoise      2.7873
    4            PositionNoise             2.7873
    4            VelocityNoise             2.7873
    4            AccelerationNoise         2.7833
    4            GyroscopeBiasNoise        2.7833
    4            AccelerometerBiasNoise    2.7821
    4            GeomagneticVectorNoise    2.7821
    4            MagnetometerBiasNoise     2.7727
    5            AccelerometerNoise        2.7693
    5            GyroscopeNoise            2.7591
    5            MagnetometerNoise         2.7591
    5            GPSPositionNoise          2.7580
    5            GPSVelocityNoise          2.6806
    5            QuaternionNoise           2.6804
    5            AngularVelocityNoise      2.5956
    5            PositionNoise             2.5956
    5            VelocityNoise             2.5956
    5            AccelerationNoise         2.5880
    5            GyroscopeBiasNoise        2.5880
    5            AccelerometerBiasNoise    2.5874
    5            GeomagneticVectorNoise    2.5874
    5            MagnetometerBiasNoise     2.5693
    6            AccelerometerNoise        2.5651
    6            GyroscopeNoise            2.5529
    6            MagnetometerNoise         2.5472
    6            GPSPositionNoise          2.5344
    6            GPSVelocityNoise          2.4514
    6            QuaternionNoise           2.4514
    6            AngularVelocityNoise      2.3829
    6            PositionNoise             2.3829
    6            VelocityNoise             2.3829
    6            AccelerationNoise         2.3826
    6            GyroscopeBiasNoise        2.3826
    6            AccelerometerBiasNoise    2.3822
    6            GeomagneticVectorNoise    2.3822
    6            MagnetometerBiasNoise     2.3675
    7            AccelerometerNoise        2.3640
    7            GyroscopeNoise            2.3547
    7            MagnetometerNoise         2.3540
    7            GPSPositionNoise          2.3428
    7            GPSVelocityNoise          2.2634
    7            QuaternionNoise           2.2633
    7            AngularVelocityNoise      2.2179
    7            PositionNoise             2.2179
    7            VelocityNoise             2.2179
    7            AccelerationNoise         2.2159
    7            GyroscopeBiasNoise        2.2159
    7            AccelerometerBiasNoise    2.2150
    7            GeomagneticVectorNoise    2.2150
    7            MagnetometerBiasNoise     2.2020
    8            AccelerometerNoise        2.1967
    8            GyroscopeNoise            2.1951
    8            MagnetometerNoise         2.1948
    8            GPSPositionNoise          2.1804
    8            GPSVelocityNoise          2.1153
    8            QuaternionNoise           2.1153
    8            AngularVelocityNoise      2.0978
    8            PositionNoise             2.0978
    8            VelocityNoise             2.0978
    8            AccelerationNoise         2.0944
    8            GyroscopeBiasNoise        2.0944
    8            AccelerometerBiasNoise    2.0944
    8            GeomagneticVectorNoise    2.0944
    8            MagnetometerBiasNoise     2.0822
    9            AccelerometerNoise        2.0771
    9            GyroscopeNoise            2.0758
    9            MagnetometerNoise         2.0754
    9            GPSPositionNoise          2.0645
    9            GPSVelocityNoise          2.0028
    9            QuaternionNoise           2.0027
    9            AngularVelocityNoise      1.9943
    9            PositionNoise             1.9943
    9            VelocityNoise             1.9943
    9            AccelerationNoise         1.9883
    9            GyroscopeBiasNoise        1.9883
    9            AccelerometerBiasNoise    1.9876
    9            GeomagneticVectorNoise    1.9876
    9            MagnetometerBiasNoise     1.9770
    10           AccelerometerNoise        1.9707
    10           GyroscopeNoise            1.9707
    10           MagnetometerNoise         1.9706
    10           GPSPositionNoise          1.9564
    10           GPSVelocityNoise          1.9005
    10           QuaternionNoise           1.9003
    10           AngularVelocityNoise      1.9003
    10           PositionNoise             1.9003
    10           VelocityNoise             1.9003
    10           AccelerationNoise         1.8999
    10           GyroscopeBiasNoise        1.8999
    10           AccelerometerBiasNoise    1.8992
    10           GeomagneticVectorNoise    1.8992
    10           MagnetometerBiasNoise     1.8885
    11           AccelerometerNoise        1.8815
    11           GyroscopeNoise            1.8807
    11           MagnetometerNoise         1.8806
    11           GPSPositionNoise          1.8674
    11           GPSVelocityNoise          1.8111
    11           QuaternionNoise           1.8110
    11           AngularVelocityNoise      1.8110
    11           PositionNoise             1.8110
    11           VelocityNoise             1.8110
    11           AccelerationNoise         1.8094
    11           GyroscopeBiasNoise        1.8094
    11           AccelerometerBiasNoise    1.8085
    11           GeomagneticVectorNoise    1.8085
    11           MagnetometerBiasNoise     1.7984
    12           AccelerometerNoise        1.7921
    12           GyroscopeNoise            1.7919
    12           MagnetometerNoise         1.7902
    12           GPSPositionNoise          1.7774
    12           GPSVelocityNoise          1.7315
    12           QuaternionNoise           1.7313
    12           AngularVelocityNoise      1.7282
    12           PositionNoise             1.7282
    12           VelocityNoise             1.7282
    12           AccelerationNoise         1.7278
    12           GyroscopeBiasNoise        1.7278
    12           AccelerometerBiasNoise    1.7268
    12           GeomagneticVectorNoise    1.7268
    12           MagnetometerBiasNoise     1.7160
    13           AccelerometerNoise        1.7103
    13           GyroscopeNoise            1.7094
    13           MagnetometerNoise         1.7094
    13           GPSPositionNoise          1.6980
    13           GPSVelocityNoise          1.6589
    13           QuaternionNoise           1.6587
    13           AngularVelocityNoise      1.6587
    13           PositionNoise             1.6587
    13           VelocityNoise             1.6587
    13           AccelerationNoise         1.6575
    13           GyroscopeBiasNoise        1.6575
    13           AccelerometerBiasNoise    1.6566
    13           GeomagneticVectorNoise    1.6566
    13           MagnetometerBiasNoise     1.6480
    14           AccelerometerNoise        1.6432
    14           GyroscopeNoise            1.6422
    14           MagnetometerNoise         1.6415
    14           GPSPositionNoise          1.6330
    14           GPSVelocityNoise          1.6097
    14           QuaternionNoise           1.6095
    14           AngularVelocityNoise      1.6093
    14           PositionNoise             1.6093
    14           VelocityNoise             1.6093
    14           AccelerationNoise         1.6083
    14           GyroscopeBiasNoise        1.6083
    14           AccelerometerBiasNoise    1.6077
    14           GeomagneticVectorNoise    1.6077
    14           MagnetometerBiasNoise     1.6012
    15           AccelerometerNoise        1.5972
    15           GyroscopeNoise            1.5949
    15           MagnetometerNoise         1.5942
    15           GPSPositionNoise          1.5886
    15           GPSVelocityNoise          1.5802
    15           QuaternionNoise           1.5801
    15           AngularVelocityNoise      1.5786
    15           PositionNoise             1.5786
    15           VelocityNoise             1.5786
    15           AccelerationNoise         1.5786
    15           GyroscopeBiasNoise        1.5786
    15           AccelerometerBiasNoise    1.5783
    15           GeomagneticVectorNoise    1.5783
    15           MagnetometerBiasNoise     1.5737
[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');

Для просмотра документации необходимо авторизоваться на сайте