Документация

Setup симуляции

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

Фильтр Fusion

Создайте insfilter, чтобы плавить IMU + измерения GPS. Этот фильтр сплава использует непрерывно-дискретный расширенный фильтр Калмана (EKF), чтобы отследить ориентацию (как кватернион), угловая скорость, положение, скорость, ускорение, смещения датчика и геомагнитный вектор.

Этот insfilter имеет несколько методов, чтобы обработать данные о датчике: fuseaccel, fusegyro, fusemag и fusegps. Поскольку insfilter использует contiuous-дискретный EKF, метод predict может продвинуться, фильтр передает произвольное количество времени.

fusionfilt = insfilter('AsyncIMU', true);
fusionfilt.ReferenceLocation = refloc;

Инициализируйте вектор состояния insfilter

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