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