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

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

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

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

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

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

gndFusion = insfilter('Magnetometer', false, 'NonholonomicHeading', true);
gndFusion.IMUSampleRate = imuFs;
gndFusion.ReferenceLocation = localOrigin;
gndFusion.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);
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', '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);

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

Состояния:

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;

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

Шумы измерения описывают, сколько шума повреждает 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]);
end

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

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

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

idx = 0;

while ~isDone(groundTruth)
    % 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.15 , Y: 0.98, Z: 0.03   (meters)

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