Этот пример показывает, как оценить положение и ориентацию наземных транспортных средств путем сплавления данных от инерциального измерительного блока (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. Фильтр слияния использует расширенный фильтр Калмана для отслеживания ориентации (в качестве кватерниона), положения, скорости и смещений датчика.
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 объект вычисляет позу на основе заданной частоты дискретизации, ППМ, времени прибытия и ориентации. Задайте параметры круговой траектории для наземного транспортного средства.
% 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, '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;
Как правило, наземные транспортные средства используют 6-осевой IMU-датчик для оценки позы. Для моделирования датчика IMU определите модель датчика IMU, содержащую акселерометр и гироскоп. В реальном применении два датчика могут быть получены из одной интегральной схемы или отдельных схем. Установленные здесь значения свойств типичны для недорогих МЭМС-датчиков.
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
Основным контуром моделирования является цикл while с вложенным циклом for. Цикл while выполняется в 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.09 (degrees)