В этом примере показано, как можно построить алгоритм слияния IMU + GPS, подходящий для беспилотных летательных аппаратов (БПЛА) или квадрокоптеров.
В этом примере для определения ориентации и положения БПЛА используются акселерометры, гироскопы, магнитометры и GPS.
Установите частоту дискретизации. В типичной системе акселерометр и гироскоп работают с относительно высокими скоростями выборки. Сложность обработки данных от этих датчиков в алгоритме слияния относительно низка. И наоборот, GPS и в некоторых случаях магнитометр работают с относительно низкой частотой дискретизации, и сложность, связанная с их обработкой, высока. В этом алгоритме слияния магнитометр и образцы GPS обрабатываются вместе с одной и той же низкой скоростью, а образцы акселерометра и гироскопа обрабатываются вместе с одной и той же высокой скоростью.
Для моделирования этой конфигурации IMU (акселерометр, гироскоп и магнитометр) отбираются на частоте 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. predict в качестве входных данных берут образцы акселерометра и гироскопа из IMU. Позвоните в predict каждый раз при выборке акселерометра и гироскопа. Этот метод предсказывает состояния на один шаг вперед на основе акселерометра и гироскопа. Здесь обновляется ковариация ошибок расширенного фильтра Калмана.
fusegps способ принимает выборки GPS в качестве входных данных. Этот способ обновляет состояния фильтра на основе выборок GPS путем вычисления коэффициента усиления Калмана, который взвешивает различные входы датчиков в соответствии с их неопределенностью. Здесь также обновляется ковариация ошибок, на этот раз с использованием коэффициента усиления Калмана.
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 (магнитный, угловой, гравитационный). Для моделирования датчика MARG определите модель датчика IMU, содержащую акселерометр, гироскоп и магнитометр. В реальном применении три датчика могут быть получены из одной интегральной схемы или отдельных схем. Установленные здесь значения свойств типичны для недорогих МЭМС-датчиков.
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);
insfilterMARG 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;insfilterMARG 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);
HelperScrollingPlotter область позволяет выполнять печать переменных с течением времени. Он используется здесь для отслеживания ошибок в позе. 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, которая является частотой выборки IMU. Области обновляются с частотой выборки IMU.
% 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)