Setup симуляции

Загрузите записанные заранее данные о датчике. Данные о датчике основаны на созданном использовании круговой траектории 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;

Фильтр Fusion

Создайте 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 визуализацию оценки фильтра и положения основной истины. Осциллографы могут замедлить симуляцию. Чтобы отключить осциллограф, установите соответствующую логическую переменную на ложь.

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 допускает различные и различные частоты дискретизации. Качество предполагаемых выходных параметров зависит в большой степени от отдельных уровней сплава датчика. Любой уволенный датчика будет иметь сильное воздействие на выход.