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.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');
