Оцените положение и ориентацию наземного транспортного средства

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

В этом примере показано, как оценить положение и ориентацию наземных транспортных средств путем объединения данных от инерционного модуля измерения (IMU) и получателя системы глобального позиционирования (GPS).

Setup симуляции

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

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

imuFs = 100;
gpsFs = 10;

% Define where on the Earth this simulation takes place using latitude,
% longitude, and altitude (LLA) coordinates.
localOrigin = [42.2825 -71.343 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. Фильтр сплава использует расширенный Фильтр Калмана, чтобы отследить ориентацию (как кватернион), положение, скорость и смещения датчика.

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

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

insfilterNonholonomic объект имеет два основных свойства: IMUSampleRate и DecimationFactor. Наземное транспортное средство имеет два скоростных ограничения, которые принимают, что оно не возвращается от земли или понижения на земле. Эти ограничения применяются с помощью расширенных уравнений обновления Фильтра Калмана. Эти обновления применяются к состояниям фильтра на уровне IMUSampleRate/DecimationFactor Гц.

gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ...
    'IMUSampleRate', imuFs, ...
    'ReferenceLocation', localOrigin, ...
    'DecimationFactor', 2);

Создайте траекторию наземного транспортного средства

waypointTrajectory объект вычисляет положение на основе заданного уровня выборки, waypoints, времена прибытия и ориентации. Задайте параметры круговой траектории для наземного транспортного средства.

% Trajectory parameters
r = 8.42; % (m)
speed = 2.50; % (m/s)
center = [0, 0]; % (m)
initialYaw = 90; % (degrees)
numRevs = 2;

% Define angles theta and corresponding times of arrival t.
revTime = 2*pi*r / speed;
theta = (0:pi/2:2*pi*numRevs).';
t = linspace(0, revTime*numRevs, numel(theta)).';

% Define position.
x = r .* cos(theta) + center(1);
y = r .* sin(theta) + center(2);
z = zeros(size(x));
position = [x, y, z];

% Define orientation.
yaw = theta + deg2rad(initialYaw);
yaw = mod(yaw, 2*pi);
pitch = zeros(size(yaw));
roll = zeros(size(yaw));
orientation = quaternion([yaw, pitch, roll], 'euler', ...
    'ZYX', 'frame');

% Generate trajectory.
groundTruth = waypointTrajectory('SampleRate', imuFs, ...
    'Waypoints', position, ...
    'TimeOfArrival', t, ...
    'Orientation', orientation);

% Initialize the random number generator used to simulate sensor noise.
rng('default');

GPS-приемник

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

gps = gpsSensor('UpdateRate', gpsFs, 'ReferenceFrame', 'ENU');
gps.ReferenceLocation = localOrigin;
gps.DecayFactor = 0.5;                % Random walk noise parameter
gps.HorizontalPositionAccuracy = 1.0;
gps.VerticalPositionAccuracy =  1.0;
gps.VelocityAccuracy = 0.1;

Датчики IMU

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

imu = imuSensor('accel-gyro', ...
    'ReferenceFrame', 'ENU', 'SampleRate', imuFs);

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

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

Инициализируйте Штаты insfilterNonholonomic

Состояния:

States                            Units    Index
Orientation (quaternion parts)             1:4
Gyroscope Bias (XYZ)              rad/s    5:7
Position (NED)                    m        8:10
Velocity (NED)                    m/s      11:13
Accelerometer Bias (XYZ)          m/s^2    14:16

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

% Get the initial ground truth pose from the first sample of the trajectory
% and release the ground truth trajectory to ensure the first sample is not
% skipped during simulation.
[initialPos, initialAtt, initialVel] = groundTruth();
reset(groundTruth);

% Initialize the states of the filter
gndFusion.State(1:4) = compact(initialAtt).';
gndFusion.State(5:7) = imu.Gyroscope.ConstantBias;
gndFusion.State(8:10) = initialPos.';
gndFusion.State(11:13) = initialVel.';
gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;

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

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

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

% Measurement noises
Rvel = gps.VelocityAccuracy.^2;
Rpos = gps.HorizontalPositionAccuracy.^2;

% The dynamic model of the ground vehicle for this filter assumes there is
% no side slip or skid during movement. This means that the velocity is
% constrained to only the forward body axis. The other two velocity axis
% readings are corrected with a zero measurement weighted by the
% |ZeroVelocityConstraintNoise| parameter.
gndFusion.ZeroVelocityConstraintNoise = 1e-2;

% Process noises
gndFusion.GyroscopeNoise = 4e-6;
gndFusion.GyroscopeBiasNoise = 4e-14;
gndFusion.AccelerometerNoise = 4.8e-2;
gndFusion.AccelerometerBiasNoise = 4e-14;

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

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

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

useErrScope = true; % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3D 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
             -1, 1
             -1, 1
             -1, 1]);
end

if usePoseView
    viewer = HelperPoseViewer( ...
        'XPositionLimits', [-15, 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-5, 5], ...
        'ReferenceFrame', 'ENU');
end

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

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

totalSimTime = 30; % seconds

% Log data for final metric computation.
numsamples = floor(min(t(end), totalSimTime) * gpsFs);
truePosition = zeros(numsamples,3);
trueOrientation = quaternion.zeros(numsamples,1);
estPosition = zeros(numsamples,3);
estOrientation = quaternion.zeros(numsamples,1);

idx = 0;

for sampleIdx = 1:numsamples
    % Predict loop at IMU update frequency.
    for i = 1:imuSamplesPerGPS
        if ~isDone(groundTruth)
            idx = idx + 1;

            % Simulate the IMU data from the current pose.
            [truePosition(idx,:), trueOrientation(idx,:), ...
                trueVel, trueAcc, trueAngVel] = groundTruth();
            [accelData, gyroData] = imu(trueAcc, trueAngVel, ...
                trueOrientation(idx,:));

            % Use the predict method to estimate the filter state based
            % on the accelData and gyroData arrays.
            predict(gndFusion, accelData, gyroData);

            % Log the estimated orientation and position.
            [estPosition(idx,:), estOrientation(idx,:)] = pose(gndFusion);

            % Compute the errors and plot.
            if useErrScope
                orientErr = rad2deg( ...
                    dist(estOrientation(idx,:), trueOrientation(idx,:)));
                posErr = estPosition(idx,:) - truePosition(idx,:);
                errscope(orientErr, posErr(1), posErr(2), posErr(3));
            end

            % Update the pose viewer.
            if usePoseView
                viewer(estPosition(idx,:), estOrientation(idx,:), ...
                    truePosition(idx,:), estOrientation(idx,:));
            end
        end
    end

    if ~isDone(groundTruth)
        % This next step happens at the GPS sample rate.
        % Simulate the GPS output based on the current pose.
        [lla, gpsVel] = gps(truePosition(idx,:), trueVel);

        % Update the filter states based on the GPS data.
        fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);
    end
end

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

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

posd = estPosition - truePosition;

% 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(estOrientation, trueOrientation));

% 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: 1.16 , Y: 0.99, Z: 0.03   (meters)

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