IMU и Fusion GPS для инерционной навигации

Открыть скрипт

В этом примере показано, как вы можете создать IMU + алгоритм сплава GPS, подходящий для беспилотных воздушных транспортных средств (БПЛА) или quadcopters.

Этот пример использует акселерометры, гироскопы, магнитометры и GPS, чтобы определить ориентацию и положение UAV.

Setup симуляции

Установите уровни выборки. В типичной системе, акселерометре и гироскопе, запущенном на уровне относительно высоких частот дискретизации. Сложность обработки данных от тех датчиков в алгоритме сплава является относительно низкой. С другой стороны GPS, и в некоторых случаях магнитометр, запуск на уровне относительно низких частот дискретизации и сложность, сопоставленная с обработкой их, высоки. В этом алгоритме сплава магнитометр и выборки GPS обрабатываются вместе под тот же низкий процент, и акселерометр и выборки гироскопа обрабатываются вместе на том же высоком показателе.

Чтобы симулировать эту настройку, IMU (акселерометр, гироскоп и магнитометр) производятся на уровне 160 Гц, и GPS производится на уровне 1 Гц. Только один из каждых 160 выборок магнитометра дан алгоритму сплава, таким образом, в действительной системе магнитометр мог быть произведен на намного более низком уровне.

imuFs = 160;
gpsFs = 1;

% Define where on the Earth this simulated scenario takes place using the
% latitude, longitude and altitude.
refloc = [42.2825 -72.3430 53.0352];


% Validate that the |gpsFs| divides |imuFs|. This allows the sensor sample
% rates to be simulated using a nested for loop without complex sample rate
% matching.

imuSamplesPerGPS = (imuFs/gpsFs);
assert(imuSamplesPerGPS == fix(imuSamplesPerGPS), ...
    'GPS sampling rate must be an integer factor of IMU sampling rate.');

Фильтр Fusion

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

Этот insfilter имеет несколько методов, чтобы обработать данные о датчике, включая predict, fusemag и fusegps. predict метод берет акселерометр и выборки гироскопа от IMU как входные параметры. Вызовите predict метод каждый раз акселерометр и гироскоп производится. Этот метод предсказывает состояния один временной шаг вперед на основе акселерометра и гироскопа. Ошибочная ковариация расширенного Фильтра Калмана обновляется здесь.

fusegps метод берет выборки GPS в качестве входа. Этот метод обновляется, состояния фильтра на основе выборок GPS путем вычисления Кальмана получают, это взвешивает различные входные параметры датчика согласно их неопределенности. Ошибочная ковариация также обновляется здесь, на этот раз с помощью усиления Кальмана также.

fusemag метод подобен, но обновляет состояния, усиление Кальмана и ошибочную ковариацию на основе выборок магнитометра.

Хотя insfilter берет акселерометр и выборки гироскопа как входные параметры, они интегрированы, чтобы вычислить скорости дельты и углы дельты, соответственно. Фильтр отслеживает смещение магнитометра и этих интегрированных сигналов.

fusionfilt = insfilter;
fusionfilt.IMUSampleRate = imuFs;
fusionfilt.ReferenceLocation = refloc;

Траектория UAV

Этот пример использует сохраненную траекторию, зарегистрированную от UAV как основная истина. Эта траектория питается несколько средств моделирования датчика, чтобы вычислить симулированный акселерометр, гироскоп, магнитометр и потоки данных GPS.

% Load the "ground truth" UAV trajectory.
load LoggedQuadcopter.mat trajData;
trajOrient = trajData.Orientation;
trajVel = trajData.Velocity;
trajPos = trajData.Position;
trajAcc = trajData.Acceleration;
trajAngVel = trajData.AngularVelocity;

% Initialize the random number generator used in the simulation of sensor
% noise.
rng(1)

Датчик GPS

Настройте GPS на уровне заданной частоты дискретизации и ссылочного местоположения. Другие параметры управляют природой шума в выходном сигнале.

gps = gpsSensor('UpdateRate', gpsFs);
gps.ReferenceLocation = refloc;
gps.DecayFactor = 0.5;              % Random walk noise parameter
gps.HorizontalPositionAccuracy = 1.6;
gps.VerticalPositionAccuracy =  1.6;
gps.VelocityAccuracy = 0.1;

Датчики IMU

Как правило, UAV использует интегрированный датчик MARG (Магнитный, Угловой Уровень, Сила тяжести) для оценки положения. Чтобы смоделировать датчик MARG, задайте модель датчика IMU, содержащую акселерометр, гироскоп и магнитометр. В реальном приложении эти три датчика могли прибыть из одной интегральной схемы или разделить единицы. Набор значений свойств здесь типичен для недорогих датчиков MEMS.

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);
imu.MagneticField = [19.5281 -5.0741 48.0067];

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.ConstantBias = deg2rad(3.125);
imu.Gyroscope.AxesMisalignment = 1.5;
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/ sqrt(50);

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

insfilter отслеживает состояния положения в векторе с 22 элементами. Состояния:

     State                           Units        State Vector Index
 Orientation as a quaternion                      1:4
 Velocity (NED)                      m/s          5:7
 Position (NED)                      m            8:10
 Delta Angle Bias (XYZ)              rad          11:13
 Delta Velocity Bias (XYZ)           m/s          14:16
 Geomagnetic Field Vector (NED)      uT           17:19
 Magnetometer Bias (XYZ)             uT           20:22

Основная истина используется, чтобы помочь инициализировать состояния фильтра, таким образом, фильтр сходится к хорошим ответам быстро.

% Initialize the states of the filter

initstate = zeros(22,1);
initstate(1:4) = compact( meanrot(trajOrient(1:100)));
initstate(5:7) = mean( trajPos(1:100,:), 1);
initstate(8:10) = mean( trajVel(1:100,:), 1);
initstate(11:13) =  imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) =  imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) =  imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;

fusionfilt.State = initstate;

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

insfilter шумы измерения описывают, сколько шума повреждает чтение датчика. Эти значения основаны на imuSensor и gpsSensor параметры.

Шумы процесса описывают, как хорошо уравнения фильтра описывают эволюцию состояния. Шумы процесса определяются опытным путем с помощью параметра, развертывающегося, чтобы совместно оптимизировать положение и оценки ориентации от фильтра.

% Measurement noises
Rmag = 0.09; % Magnetometer measurement noise
Rvel = 0.01; % GPS Velocity measurement noise
Rpos = 2.56; % GPS Position measurement noise

% Process noises
fusionfilt.AccelerometerBiasNoise =  2e-4;
fusionfilt.AccelerometerNoise = 2;
fusionfilt.GyroscopeBiasNoise = 1e-16;
fusionfilt.GyroscopeNoise =  1e-5;
fusionfilt.MagnetometerBiasNoise = 1e-10;
fusionfilt.GeomagneticVectorNoise = 1e-12;

% Initial error covariance
fusionfilt.StateCovariance = 1e-9*ones(22);

Инициализируйте осциллографы

HelperScrollingPlotter осциллограф позволяет строить переменных в зависимости от времени. Это используется здесь к дефектам записи в положении. HelperPoseViewer осциллограф позволяет 3-D визуализацию оценки фильтра и положения основной истины. Осциллографы могут замедлить симуляцию. Чтобы отключить осциллограф, установите соответствующую логическую переменную на false.

useErrScope = true;  % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3-D pose viewer

if useErrScope
    errscope = HelperScrollingPlotter(...
        'NumInputs', 4, ...
        'TimeSpan', 10, ...
        'SampleRate', imuFs, ...
        'YLabel', {'degrees', ...
        'meters', ...
        'meters', ...
        'meters'}, ...
        'Title', {'Quaternion Distance', ...
        'Position X Error', ...
        'Position Y Error', ...
        'Position Z Error'}, ...
        'YLimits', ...
        [ -1, 1
        -2, 2
        -2 2
        -2 2]);
end

if usePoseView
    posescope = HelperPoseViewer(...
        'XPositionLimits', [-15 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-10 10]);
end

Цикл симуляции

Основной цикл симуляции является некоторое время циклом с вложенным циклом for. Цикл с условием продолжения выполняется в gpsFs, который является частотой дискретизации GPS. Вложенный цикл for выполняется в imuFs, который является частотой дискретизации IMU. Осциллографы обновляются на уровне частоты дискретизации IMU.

% Loop setup - |trajData| has about 142 seconds of recorded data.
secondsToSimulate = 50; % simulate about 50 seconds
numsamples = secondsToSimulate*imuFs;

loopBound = floor(numsamples);
loopBound = floor(loopBound/imuFs)*imuFs; % ensure enough IMU Samples

% Log data for final metric computation.
pqorient = quaternion.zeros(loopBound,1);
pqpos = zeros(loopBound,3);

fcnt = 1;

while(fcnt <=loopBound)
    % |predict| loop at IMU update frequency.
    for ff=1:imuSamplesPerGPS
        % Simulate the IMU data from the current pose.
        [accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), ...
            trajOrient(fcnt));

        % Use the |predict| method to estimate the filter state based
        % on the simulated accelerometer and gyroscope signals.
        predict(fusionfilt, accel, gyro);

        % Acquire the current estimate of the filter states.
        [fusedPos, fusedOrient] = pose(fusionfilt);

        % Save the position and orientation for post processing.
        pqorient(fcnt) = fusedOrient;
        pqpos(fcnt,:) = fusedPos;

        % Compute the errors and plot.
        if useErrScope
            orientErr = rad2deg(dist(fusedOrient, ...
                trajOrient(fcnt) ));
            posErr = fusedPos - trajPos(fcnt,:);
            errscope(orientErr, posErr(1), posErr(2), posErr(3));
        end

        % Update the pose viewer.
        if usePoseView
            posescope(pqpos(fcnt,:), pqorient(fcnt),  trajPos(fcnt,:), ...
                trajOrient(fcnt,:) );
        end
        fcnt = fcnt + 1;
    end

    % This next step happens at the GPS sample rate.
    % Simulate the GPS output based on the current pose.
    [lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );

    % Correct the filter states based on the GPS data and magnetic
    % field measurements.
    fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);
    fusemag(fusionfilt, mag, Rmag);

end

Ошибочный метрический расчет

Положение и оценки ориентации регистрировались в течение симуляции. Теперь вычислите сквозную среднеквадратическую ошибку и для положения и для ориентации.

posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :);

% For orientation, quaternion distance is a much better alternative to
% subtracting Euler angles, which have discontinuities. The quaternion
% distance can be computed with the |dist| function, which gives the
% angular difference in orientation in radians. Convert to degrees
% for display in the command window.

quatd = rad2deg(dist(pqorient(1:loopBound), trajOrient(1:loopBound)) );

% Display RMS errors in the command window.
fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');
msep = sqrt(mean(posd.^2));
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f   (meters)\n\n',msep(1), ...
    msep(2), msep(3));

fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');
fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));


End-to-End Simulation Position RMS Error
	X: 0.57 , Y: 0.53, Z: 0.68   (meters)

End-to-End Quaternion Distance RMS Error (degrees) 
	0.32 (degrees)
Документация Sensor Fusion and Tracking Toolbox
Поддержка
Для просмотра документации необходимо авторизоваться на сайте