В этом примере показано, как вы можете плавить датчики на различных уровнях, чтобы оценить положение. Акселерометр, гироскоп, магнитометр и 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);
insfilterAsyncinsfilterAsync отслеживает состояния положения в векторе с 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 визуализацию оценки фильтра и положения основной истины. Осциллографы могут замедлить симуляцию. Чтобы отключить осциллограф, установите соответствующую логическую переменную на ложь.
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 допускает различные и различные частоты дискретизации. Качество предполагаемых выходных параметров зависит в большой степени от отдельных уровней сплава датчика. Любой уволенный датчика будет иметь сильное воздействие на выход.