Автоматическая настройка фильтра insfilterAsync
The insfilterAsync (Navigation Toolbox) является комплексным расширенным фильтром Калмана, который оценивает положение устройства. Однако ручная настройка фильтра или нахождение оптимальных значений для параметров шума может оказаться сложной задачей. Этот пример иллюстрирует, как использовать tune (Navigation Toolbox) функция для оптимизации параметров шума фильтра.
Траектория и Setup датчика
Чтобы проиллюстрировать процесс настройки insfilterAsync filter, используйте простую случайную траекторию точки пути. The 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);
Создайте фильтр
The 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');
