Этот пример показывает, как можно сплавить датчики с различными скоростями для оценки положения. Акселерометр, гироскоп, магнитометр и GPS предназначены для определения ориентации и положения транспортного средства, перемещающегося по круговому пути. Можно использовать элементы управления в окне рисунка, чтобы варьировать скорости датчика и экспериментировать с выпадением датчика, видя эффект на предполагаемое положение.
Загрузите предварительно записанные данные о датчике. Данные о датчике основаны на круговой траектории, созданной с помощью waypointTrajectory класс. Значения датчиков были созданы с помощью gpsSensor и imuSensor классы. The 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 предохранить измерения БИНС + GPS. Этот фильтр слияния использует непрерывно-дискретный расширенный фильтр Калмана (EKF) для отслеживания ориентации (в качестве кватерниона), скорости вращения, положения, скорости, ускорения, смещений датчика и геомагнитного вектора.
Этот insfilterAsync имеет несколько методов обработки данных о датчике: fuseaccel, fusegyro, fusemag и fusegps. Потому что insfilterAsync использует непрерывно-дискретный EKF, predict способ может переместить фильтр вперед на произвольное количество времени.
fusionfilt = insfilterAsync('ReferenceLocation', refloc);
insfilterAsyncThe 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));
The HelperScrollingPlotter возможности включает построение графиков переменных с течением времени. Он используется здесь, чтобы отслеживать ошибки в положении. The 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
The insfilterAsync позволяет для различных и меняющихся частот дискретизации. Качество предполагаемых выходов в большой степени зависит от отдельных скоростей слияния датчиков. Любое выпадение датчика окажет глубокий эффект на выход.