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