Область Setup

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

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

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