Этот пример показывает, как оценить положение и ориентацию наземных транспортных средств путем плавления данных из инерционного модуля измерения (IMU) и получателя системы глобального позиционирования (GPS).
Установите уровни выборки. В типичной системе, акселерометре и гироскопе в 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.');
Создайте фильтр, чтобы плавить 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 = 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 с 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)