Этот пример показов, как оценить положение и ориентацию наземных транспортных средств путем сплавления данных от приемника инерциальных измерительных блоков (IMU) и глобальной системы позиционирования (GPS).
Установите частоты дискретизации. В типичной системе акселерометр и гироскоп в БИНС выполняются с относительно высокими скоростями дискретизации. Сложность обработки данных от этих датчиков в алгоритме слияния относительно низкая. И наоборот, GPS работает с относительно низкой частотой дискретизации, и сложность, связанная с его обработкой, высока. В этом алгоритме слияния выборки GPS обрабатываются с низкой скоростью, и выборки акселерометра и гироскопа обрабатываются вместе с одной и той же высокой скоростью.
Для моделирования этого строения БИНС (акселерометр и гироскоп) отбирается с частотой 100 Гц, а БИНС - с частотой дискретизации 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. Фильтр слияния использует расширенный фильтр Калмана для отслеживания ориентации (в качестве кватерниона), положения, скорости и смещений датчика.
The insfilterNonholonomic объект, который имеет два основных метода: predict и fusegps. The predict метод принимает за вход выборки акселерометра и гироскопа из БИНС. Вызовите predict способ каждый раз отбирают акселерометр и гироскоп. Этот метод предсказывает состояния вперед на один временной шаг на основе акселерометра и гироскопа. Ковариация ошибок расширенного фильтра Калмана обновляется на этом шаге.
The fusegps метод принимает выборки за вход. Этот метод обновляет состояния фильтра на основе GPS выборки путем вычисления усиления Калмана, которое взвешивает различные входы датчика в соответствии с их неопределенностью. Ковариация ошибки также обновляется на этом шаге, на этот раз также используя коэффициент усиления Калмана.
The insfilterNonholonomic объект имеет два основных свойства: IMUSampleRate и DecimationFactor. Наземное транспортное средство имеет два ограничения скорости, которые предполагают, что он не отскакивает от земли или скользит по земле. Эти ограничения применяются с помощью расширенных уравнений обновления фильтра Калмана. Эти обновления применяются к состояниям фильтра со скоростью IMUSampleRate/DecimationFactor Гц.
gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ... 'IMUSampleRate', imuFs, ... 'ReferenceLocation', localOrigin, ... 'DecimationFactor', 2);
The 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 для оценки положения. Чтобы смоделировать датчик INU, задайте модель датчика INU, содержащую акселерометр и гироскоп. В реальном случае эти два датчика могут быть получены из одной интегральной схемы или отдельных таковых. Набор значений свойств здесь типичен для недорогих 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);
The HelperScrollingPlotter возможности включает построение графиков переменных с течением времени. Он используется здесь, чтобы отслеживать ошибки в положении. The 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, который является частотой дискретизации БИНС. Возможности обновляются с частотой дискретизации БИНС.
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)