IMU и Fusion GPS для инерционной навигации
В этом примере показано, как вы можете создать IMU + алгоритм сплава GPS, подходящий для беспилотных воздушных транспортных средств (БПЛА) или quadcopters.
Этот пример использует акселерометры, гироскопы, магнитометры и GPS, чтобы определить ориентацию и положение UAV.
Setup симуляции
Установите частоты дискретизации. В типичной системе, акселерометре и гироскопе, запущенном на уровне относительно высоких частот дискретизации. Сложность обработки данных от тех датчиков в алгоритме сплава является относительно низкой. С другой стороны 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.');
Фильтр Fusion
Создайте фильтр, чтобы плавить IMU + измерения GPS. Фильтр сплава использует расширенный Фильтр Калмана, чтобы отследить ориентацию (как кватернион), скорость, положение, смещения датчика и геомагнитный вектор.
Этот insfilterMARG имеет несколько методов, чтобы обработать данные о датчике, включая predict, fusemag и fusegps. predict метод берет акселерометр и выборки гироскопа от IMU как входные параметры. Вызовите predict метод каждый раз акселерометр и гироскоп производится. Этот метод предсказывает состояния один временной шаг вперед на основе акселерометра и гироскопа. Ошибочная ковариация расширенного Фильтра Калмана обновляется здесь.
fusegps метод берет выборки GPS в качестве входа. Этот метод обновляется, состояния фильтра на основе выборок GPS путем вычисления Кальмана получают, это взвешивает различные входные параметры датчика согласно их неопределенности. Ошибочная ковариация также обновляется здесь, на этот раз с помощью усиления Кальмана также.
fusemag метод подобен, но обновляет состояния, усиление Кальмана и ошибочную ковариацию на основе выборок магнитометра.
Хотя insfilterMARG берет акселерометр и выборки гироскопа как входные параметры, они интегрированы, чтобы вычислить скорости дельты и углы дельты, соответственно. Фильтр отслеживает смещение магнитометра и этих интегрированных сигналов.
fusionfilt = insfilterMARG; fusionfilt.IMUSampleRate = imuFs; fusionfilt.ReferenceLocation = refloc;
Траектория UAV
Этот пример использует сохраненную траекторию, зарегистрированную от UAV как основная истина. Эта траектория питается несколько средств моделирования датчика, чтобы вычислить симулированный акселерометр, гироскоп, магнитометр и потоки данных 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 на уровне заданной частоты дискретизации и ссылочного местоположения. Другие параметры управляют природой шума в выходном сигнале.
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;
Датчики IMU
Как правило, UAV использует интегрированный датчик MARG (Магнитный, Угловой Уровень, Сила тяжести) для оценки положения. Чтобы смоделировать датчик 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);
Инициализируйте вектор состояния 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
Цикл симуляции
Основной цикл симуляции является некоторое время циклом с вложенным циклом for. Цикл с условием продолжения выполняется в 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)