В этом примере показано, как можно сплавить датчики с различной скоростью для оценки позы. По акселерометру, гироскопу, магнитометру и GPS определяют ориентацию и положение транспортного средства, движущегося по круговой траектории. Вы можете использовать элементы управления в окне рисунка для изменения скорости сенсора и экспериментировать с выпадением сенсора, одновременно наблюдая влияние на предполагаемую позу.
Загрузить предварительно записанные данные датчика. Данные датчика основаны на круговой траектории, созданной с помощью waypointTrajectory класс. Значения датчиков были созданы с помощью gpsSensor и imuSensor классы. CircularTrajectorySensorData.mat файл, используемый здесь, может быть создан с помощью generateCircularTrajSensorData функция.
ld = load('CircularTrajectorySensorData.mat'); Fs = ld.Fs; % maximum MARG rate gpsFs = ld.gpsFs; % maximum GPS rate ratio = Fs./gpsFs; refloc = ld.refloc; trajOrient = ld.trajData.Orientation; trajVel = ld.trajData.Velocity; trajPos = ld.trajData.Position; trajAcc = ld.trajData.Acceleration; trajAngVel = ld.trajData.AngularVelocity; accel = ld.accel; gyro = ld.gyro; mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;
Создание insfilterAsync для предохранителя измерений IMU + GPS. Этот термоядерный фильтр использует непрерывный дискретный расширенный фильтр Калмана (EKF) для отслеживания ориентации (в качестве кватерниона), угловой скорости, положения, скорости, ускорения, смещений датчика и геомагнитного вектора.
Это insfilterAsync имеет несколько методов обработки данных датчиков: fuseaccel, fusegyro, fusemag и fusegps. Поскольку insfilterAsync использует непрерывную дискретную EKF, predict способ может выполнять пошаговый переход фильтра вперед на произвольное время.
fusionfilt = insfilterAsync('ReferenceLocation', refloc);
insfilterAsync insfilterAsync отслеживает состояния позы в 28-элементном векторе. Штаты:
States Units Index Orientation (quaternion parts) 1:4 Angular Velocity (XYZ) rad/s 5:7 Position (NED) m 8:10 Velocity (NED) m/s 11:13 Acceleration (NED) m/s^2 14:16 Accelerometer Bias (XYZ) m/s^2 17:19 Gyroscope Bias (XYZ) rad/s 20:22 Geomagnetic Field Vector (NED) uT 23:25 Magnetometer Bias (XYZ) uT 26:28
Для инициализации состояний фильтра используется истина основания, поэтому фильтр быстро сходится к хорошим ответам.
Nav = 100; initstate = zeros(28,1); initstate(1:4) = compact( meanrot(trajOrient(1:Nav))); initstate(5:7) = mean( trajAngVel(10:Nav,:), 1); initstate(8:10) = mean( trajPos(1:Nav,:), 1); initstate(11:13) = mean( trajVel(1:Nav,:), 1); initstate(14:16) = mean( trajAcc(1:Nav,:), 1); initstate(23:25) = ld.magField; % The gyroscope bias initial value estimate is low for the Z-axis. This is % done to illustrate the effects of fusing the magnetometer in the % simulation. initstate(20:22) = deg2rad([3.125 3.125 3.125]); fusionfilt.State = initstate;
insfilterAsyncДисперсия шума процесса описывает неопределенность модели движения, используемой фильтром.
fusionfilt.QuaternionNoise = 1e-2; fusionfilt.AngularVelocityNoise = 100; fusionfilt.AccelerationNoise = 100; fusionfilt.MagnetometerBiasNoise = 1e-7; fusionfilt.AccelerometerBiasNoise = 1e-7; fusionfilt.GyroscopeBiasNoise = 1e-7;
Каждый датчик имеет некоторый шум в измерениях. Эти значения обычно можно найти в спецификации датчика.
Rmag = 0.4; Rvel = 0.01; Racc = 610; Rgyro = 0.76e-5; Rpos = 3.4; fusionfilt.StateCovariance = diag(1e-3*ones(28,1));
HelperScrollingPlotter область позволяет выполнять печать переменных с течением времени. Он используется здесь для отслеживания ошибок в позе. PoseViewerWithSwitches объем позволяет 3D визуализировать оценку фильтра и позу истинности основания. Области могут замедлить моделирование. Чтобы отключить область, установите для соответствующей логической переменной значение false.
useErrScope = true; % Turn on the streaming error plot. usePoseView = true; % Turn on the 3D pose viewer. if usePoseView posescope = PoseViewerWithSwitches(... 'XPositionLimits', [-30 30], ... 'YPositionLimits', [-30, 30], ... 'ZPositionLimits', [-10 10]); end f = gcf; if useErrScope errscope = HelperScrollingPlotter(... 'NumInputs', 4, ... 'TimeSpan', 10, ... 'SampleRate', Fs, ... 'YLabel', {'degrees', ... 'meters', ... 'meters', ... 'meters'}, ... 'Title', {'Quaternion Distance', ... 'Position X Error', ... 'Position Y Error', ... 'Position Z Error'}, ... 'YLimits', ... [ -1, 30 -2, 2 -2 2 -2 2]); end
Моделирование алгоритма слияния позволяет проверить эффекты изменения частоты дискретизации датчика. Кроме того, слияние отдельных датчиков может быть предотвращено снятием соответствующего флажка. Это можно использовать для моделирования отсева сенсора.
Некоторые конфигурации дают впечатляющие результаты. Например, выключение GPS-датчика приводит к быстрому дрейфу оценки положения. Выключение датчика магнитометра приведет к медленному отклонению оценки ориентации от истинного положения земли, так как оценка вращается слишком быстро. И наоборот, если гироскоп выключен и магнитометр включен, расчетная ориентация показывает колебание и не имеет гладкости, если используются оба датчика.
Включение всех датчиков, но установка их для работы с наименьшей скоростью дает оценку, которая заметно отклоняется от истинного положения земли, а затем защелкивается к более правильному результату, когда датчики сплавляются. Это легче всего увидеть в HelperScrollingPlotter ошибок текущей оценки.
Основное моделирование выполняется на частоте 100 Гц. Каждая итерация проверяет флажки в окне рисунка и, если датчик включен, предохраняет данные для этого датчика с соответствующей скоростью.
for ii=1:size(accel,1) fusionfilt.predict(1./Fs); % Fuse Accelerometer if (f.UserData.Accelerometer) && ... mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0 fusionfilt.fuseaccel(accel(ii,:), Racc); end % Fuse Gyroscope if (f.UserData.Gyroscope) && ... mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0 fusionfilt.fusegyro(gyro(ii,:), Rgyro); end % Fuse Magnetometer if (f.UserData.Magnetometer) && ... mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0 fusionfilt.fusemag(mag(ii,:), Rmag); end % Fuse GPS if (f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0 fusionfilt.fusegps(lla(ii,:), Rpos, gpsvel(ii,:), Rvel); end % Plot the pose error [p,q] = pose(fusionfilt); posescope(p, q, trajPos(ii,:), trajOrient(ii)); orientErr = rad2deg(dist(q, trajOrient(ii) )); posErr = p - trajPos(ii,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end
insfilterAsync позволяет использовать различные и изменяющиеся частоты выборки. Качество расчетных выходных данных в значительной степени зависит от скорости слияния отдельных датчиков. Любое выпадение сенсора окажет глубокое влияние на выход.