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
Поддержка
Для просмотра документации необходимо авторизоваться на сайте