Этот пример показывает, как можно создать алгоритм слияния IMU + GPS, подходящий для беспилотных летательных транспортных средств (БПЛА) или квадрокоптеров.
Этот пример использует акселерометры, гироскопы, магнитометры и GPS, чтобы определить ориентацию и положение БПЛА.
Установите частоты дискретизации. В типичной системе акселерометр и гироскоп работают с относительно высокими скоростями дискретизации. Сложность обработки данных от этих датчиков в алгоритме слияния относительно низкая. И наоборот, GPS, а в некоторых случаях и магнитометр, запускается с относительно низкими скоростями дискретизации, и сложность, связанная с их обработкой, высока. В этом алгоритме слияния выборки магнитометра и GPS обрабатываются вместе с одной и той же низкой скоростью, и выборки акселерометра и гироскопа обрабатываются вместе с одной и той же высокой скоростью.
Для моделирования этого строения БИНС (акселерометр, гироскоп и магнитометр) отбирают при 160 Гц, а GPS - при 1 Гц. Алгоритму слияния дается только одна из каждых 160 выборок магнитометра, поэтому в реальной системе магнитометр может быть взят с гораздо меньшей скоростью.
imuFs = 160; gpsFs = 1; % Define where on the Earth this simulated scenario takes place using the % latitude, longitude and altitude. refloc = [42.2825 -72.3430 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. Фильтр слияния использует расширенный фильтр Калмана для отслеживания ориентации (в качестве кватерниона), скорости, положения, смещений датчика и геомагнитного вектора.
Этот insfilterMARG имеет несколько методов обработки данных о датчике, включая predict, fusemag и fusegps. The predict метод принимает в качестве входов выборки акселерометра и гироскопа из БИНС. Вызовите predict способ каждый раз отбирают акселерометр и гироскоп. Этот метод предсказывает состояния на один временной шаг вперед на основе акселерометра и гироскопа. Здесь обновляется ковариация ошибок расширенного фильтра Калмана.
The fusegps метод принимает за вход выборок. Этот метод обновляет состояния фильтра на основе выборок GPS путем вычисления усиления Калмана, которое взвешивает различные входы датчика в соответствии с их неопределенностью. Здесь также обновляется ковариация ошибок, на этот раз используя коэффициент усиления Калмана.
The fusemag метод аналогичен, но обновляет состояния, коэффициент усиления Калмана и ковариацию ошибок на основе выборок магнитометра.
Хотя и insfilterMARG принимает в качестве входов выборки акселерометра и гироскопа, они интегрированы для вычисления скоростей дельты и углов дельты, соответственно. Фильтр отслеживает смещение магнитометра и эти интегрированные сигналы.
fusionfilt = insfilterMARG; fusionfilt.IMUSampleRate = imuFs; fusionfilt.ReferenceLocation = refloc;
Этот пример использует сохраненную траекторию, записанную с БПЛА, в качестве основной истины. Эта траектория подается на несколько симуляторов датчиков для вычисления симулированных потоков данных акселерометра, гироскопа, магнитометра и GPS.
% Load the "ground truth" UAV trajectory. load LoggedQuadcopter.mat trajData; trajOrient = trajData.Orientation; trajVel = trajData.Velocity; trajPos = trajData.Position; trajAcc = trajData.Acceleration; trajAngVel = trajData.AngularVelocity; % Initialize the random number generator used in the simulation of sensor % noise. rng(1)
Настройте GPS с заданной частотой дискретизации и опорным местом. Другие параметры контролируют характер шума в выход сигнале.
gps = gpsSensor('UpdateRate', gpsFs); gps.ReferenceLocation = refloc; gps.DecayFactor = 0.5; % Random walk noise parameter gps.HorizontalPositionAccuracy = 1.6; gps.VerticalPositionAccuracy = 1.6; gps.VelocityAccuracy = 0.1;
Обычно для оценки положения БПЛА использует интегрированный датчик MARG (Magnetic, Angular Rate, Gravity). Чтобы смоделировать датчик MARG, задайте модель датчика IMU, содержащую акселерометр, гироскоп и магнитометр. В реальное приложение три датчика могут быть получены из одной интегральной схемы или отдельных таковых. Набор значений свойств здесь типичен для недорогих MEMS-датчиков.
imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs); imu.MagneticField = [19.5281 -5.0741 48.0067]; % Accelerometer imu.Accelerometer.MeasurementRange = 19.6133; imu.Accelerometer.Resolution = 0.0023928; imu.Accelerometer.ConstantBias = 0.19; imu.Accelerometer.NoiseDensity = 0.0012356; % Gyroscope imu.Gyroscope.MeasurementRange = deg2rad(250); imu.Gyroscope.Resolution = deg2rad(0.0625); imu.Gyroscope.ConstantBias = deg2rad(3.125); imu.Gyroscope.AxesMisalignment = 1.5; imu.Gyroscope.NoiseDensity = deg2rad(0.025); % Magnetometer imu.Magnetometer.MeasurementRange = 1000; imu.Magnetometer.Resolution = 0.1; imu.Magnetometer.ConstantBias = 100; imu.Magnetometer.NoiseDensity = 0.3/ sqrt(50);
insfilterMARGThe insfilterMARG отслеживает состояния положения в векторе с 22 элементами. Состояниями являются:
State Units State Vector Index Orientation as a quaternion 1:4 Position (NED) m 5:7 Velocity (NED) m/s 8:10 Delta Angle Bias (XYZ) rad 11:13 Delta Velocity Bias (XYZ) m/s 14:16 Geomagnetic Field Vector (NED) uT 17:19 Magnetometer Bias (XYZ) uT 20:22
Основная истина используется, чтобы помочь инициализировать состояния фильтра, поэтому фильтр быстро сходится к хорошим ответам.
% Initialize the states of the filter
initstate = zeros(22,1);
initstate(1:4) = compact( meanrot(trajOrient(1:100)));
initstate(5:7) = mean( trajPos(1:100,:), 1);
initstate(8:10) = mean( trajVel(1:100,:), 1);
initstate(11:13) = imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) = imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) = imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;insfilterMARGThe insfilterMARG измерительные шумы описывают, насколько шум повреждает показания датчика. Эти значения основаны на imuSensor и gpsSensor параметры.
Шумы процесса описывают, насколько хорошо уравнения фильтра описывают эволюцию состояния. Технологические шумы определяются эмпирически с помощью протягивания параметра для совместной оптимизации оценок положения и ориентации от фильтра.
% Measurement noises Rmag = 0.09; % Magnetometer measurement noise Rvel = 0.01; % GPS Velocity measurement noise Rpos = 2.56; % GPS Position measurement noise % Process noises fusionfilt.AccelerometerBiasNoise = 2e-4; fusionfilt.AccelerometerNoise = 2; fusionfilt.GyroscopeBiasNoise = 1e-16; fusionfilt.GyroscopeNoise = 1e-5; fusionfilt.MagnetometerBiasNoise = 1e-10; fusionfilt.GeomagneticVectorNoise = 1e-12; % Initial error covariance fusionfilt.StateCovariance = 1e-9*ones(22);
The HelperScrollingPlotter возможности включает построение графиков переменных с течением времени. Он используется здесь, чтобы отслеживать ошибки в положении. The HelperPoseViewer Возможности позволяют 3-D визуализацию оценки фильтра и основной истины положения. Возможности могут замедлить симуляцию. Чтобы отключить возможности, установите соответствующую логическую переменную равной false.
useErrScope = true; % Turn on the streaming error plot usePoseView = true; % Turn on the 3-D 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 -2, 2 -2 2 -2 2]); end if usePoseView posescope = HelperPoseViewer(... 'XPositionLimits', [-15 15], ... 'YPositionLimits', [-15, 15], ... 'ZPositionLimits', [-10 10]); end
Основной цикл симуляции является циклом while с вложенным циклом for. Цикл while выполняется в gpsFs, который является частотой выборки GPS. Вложенный цикл for выполняется в imuFs, который является частотой дискретизации БИНС. Возможности обновляются с частотой дискретизации БИНС.
% Loop setup - |trajData| has about 142 seconds of recorded data. secondsToSimulate = 50; % simulate about 50 seconds numsamples = secondsToSimulate*imuFs; loopBound = floor(numsamples); loopBound = floor(loopBound/imuFs)*imuFs; % ensure enough IMU Samples % Log data for final metric computation. pqorient = quaternion.zeros(loopBound,1); pqpos = zeros(loopBound,3); fcnt = 1; while(fcnt <=loopBound) % |predict| loop at IMU update frequency. for ff=1:imuSamplesPerGPS % Simulate the IMU data from the current pose. [accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), ... trajOrient(fcnt)); % Use the |predict| method to estimate the filter state based % on the simulated accelerometer and gyroscope signals. predict(fusionfilt, accel, gyro); % Acquire the current estimate of the filter states. [fusedPos, fusedOrient] = pose(fusionfilt); % Save the position and orientation for post processing. pqorient(fcnt) = fusedOrient; pqpos(fcnt,:) = fusedPos; % Compute the errors and plot. if useErrScope orientErr = rad2deg(dist(fusedOrient, ... trajOrient(fcnt) )); posErr = fusedPos - trajPos(fcnt,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end % Update the pose viewer. if usePoseView posescope(pqpos(fcnt,:), pqorient(fcnt), trajPos(fcnt,:), ... trajOrient(fcnt,:) ); end fcnt = fcnt + 1; end % This next step happens at the GPS sample rate. % Simulate the GPS output based on the current pose. [lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) ); % Correct the filter states based on the GPS data and magnetic % field measurements. fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel); fusemag(fusionfilt, mag, Rmag); end
Оценки положения и ориентации регистрировались в течение всей симуляции. Теперь вычислите сквозную корневую среднюю квадратичную невязку для положения и ориентации.
posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :); % 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(pqorient(1:loopBound), trajOrient(1:loopBound)) ); % Display RMS errors in the command window. fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');
End-to-End Simulation Position RMS Error
msep = sqrt(mean(posd.^2)); fprintf('\tX: %.2f , Y: %.2f, Z: %.2f (meters)\n\n',msep(1), ... msep(2), msep(3));
X: 0.57 , Y: 0.53, Z: 0.68 (meters)
fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');End-to-End Quaternion Distance RMS Error (degrees)
fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));0.32 (degrees)