IMU and GPS Fusion for Inertial Navigation Label: Research

轉載自IMU and GPS Fusion for Inertial Navigation

This example shows how you might build an IMU + GPS fusion algorithm suitable for unmanned aerial vehicles (UAVs) or quadcopters.
This example uses accelerometers, gyroscopes, magnetometers, and GPS to determine orientation and position of a UAV.

此示例展示瞭如何構建適用於無人駕駛飛行器 (UAV) 或四軸飛行器的 IMU + GPS 融合算法。
此示例使用加速度計、陀螺儀、磁力計和 GPS 來確定 UAV 的方向和位置。

Simulation Setup

Set the sampling rates. In a typical system, the accelerometer and gyroscope run at relatively high sample rates. The complexity of processing data from those sensors in the fusion algorithm is relatively low. Conversely, the GPS, and in some cases the magnetometer, run at relatively low sample rates, and the complexity associated with processing them is high. In this fusion algorithm, the magnetometer and GPS samples are processed together at the same low rate, and the accelerometer and gyroscope samples are processed together at the same high rate.
To simulate this configuration, the IMU (accelerometer, gyroscope, and magnetometer) are sampled at 160 Hz, and the GPS is sampled at 1 Hz. Only one out of every 160 samples of the magnetometer is given to the fusion algorithm, so in a real system the magnetometer could be sampled at a much lower rate.
設置採樣率。 在典型系統中,加速度計和陀螺儀以相對較高的採樣率運行。 在融合算法中處理來自這些傳感器的數據的複雜性相對較低。 相反,GPS 以及某些情況下的磁力計以相對較低的採樣率運行,並且與處理它們相關的複雜性很高。 在此融合算法中,磁力計和 GPS 樣本以相同的低速率一起處理,加速度計和陀螺儀樣本以相同的高速率一起處理。
爲模擬此配置,IMU(加速度計、陀螺儀和磁力計)以 160 Hz 採樣,GPS 以 1 Hz 採樣。 每 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 Filter

Create the filter to fuse IMU + GPS measurements. The fusion filter uses an extended Kalman filter to track orientation (as a quaternion), velocity, position, sensor biases, and the geomagnetic vector.
This insfilterMARG has a few methods to process sensor data, including predict, fusemag and fusegps. The predict method takes the accelerometer and gyroscope samples from the IMU as inputs. Call the predict method each time the accelerometer and gyroscope are sampled. This method predicts the states one time step ahead based on the accelerometer and gyroscope. The error covariance of the extended Kalman filter is updated here.
The fusegps method takes GPS samples as input. This method updates the filter states based on GPS samples by computing a Kalman gain that weights the various sensor inputs according to their uncertainty. An error covariance is also updated here, this time using the Kalman gain as well.
The fusemag method is similar but updates the states, Kalman gain, and error covariance based on the magnetometer samples.
Though the insfilterMARG takes accelerometer and gyroscope samples as inputs, these are integrated to compute delta velocities and delta angles, respectively. The filter tracks the bias of the magnetometer and these integrated signals.
創建過濾器以融合 IMU + GPS 測量值。 融合濾波器使用擴展的卡爾曼濾波器來跟蹤方向(作爲四元數)、速度、位置、傳感器偏差和地磁矢量。
這個insfilterMARG有幾個處理傳感器數據的方法,包括predict、fusemag和fusegps。 predict 方法將來自 IMU 的加速度計和陀螺儀樣本作爲輸入。 每次對加速度計和陀螺儀進行採樣時調用predict方法。 該方法基於加速度計和陀螺儀提前一步預測狀態。 擴展卡爾曼濾波器的誤差協方差在此處更新。
fusegps 方法將 GPS 樣本作爲輸入。 該方法通過計算根據不確定性對各種傳感器輸入進行加權的卡爾曼增益,根據 GPS 樣本更新濾波器狀態。 誤差協方差也在這裏更新,這次也使用卡爾曼增益。
fusemag 方法類似,但基於磁力計樣本更新狀態、卡爾曼增益和誤差協方差。
雖然 insfilterMARG 將加速度計和陀螺儀樣本作爲輸入,但它們會被集成以分別計算增量速度和增量角度。 濾波器跟蹤磁力計的偏差和這些積分信號。
fusionfilt = insfilterMARG;
fusionfilt.IMUSampleRate = imuFs;
fusionfilt.ReferenceLocation = refloc;

UAV Trajectory

This example uses a saved trajectory recorded from a UAV as the ground truth. This trajectory is fed to several sensor simulators to compute simulated accelerometer, gyroscope, magnetometer, and GPS data streams.
此示例使用從無人機記錄的已保存軌跡作爲地面實況。 該軌跡被饋送到多個傳感器模擬器,以計算模擬的加速度計、陀螺儀、磁力計和 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 Sensor

Set up the GPS at the specified sample rate and reference location. The other parameters control the nature of the noise in the output signal.
以指定的採樣率和參考位置設置 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 Sensors

Typically, a UAV uses an integrated MARG sensor (Magnetic, Angular Rate, Gravity) for pose estimation. To model a MARG sensor, define an IMU sensor model containing an accelerometer, gyroscope, and magnetometer. In a real-world application the three sensors could come from a single integrated circuit or separate ones. The property values set here are typical for low-cost MEMS sensors.
通常,無人機使用集成的 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);

  

Initialize the State Vector of the insfilterMARG

The insfilterMARG tracks the pose states in a 22-element vector. The states are: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
Ground truth is used to help initialize the filter states, so the filter converges to good answers quickly. Ground truth 用於幫助初始化過濾器狀態,因此過濾器可以快速收斂到好的答案。
% 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;

Initialize the Variances of the insfilterMARG

The insfilterMARG measurement noises describe how much noise is corrupting the sensor reading. These values are based on the imuSensor and gpsSensor parameters.
The process noises describe how well the filter equations describe the state evolution. Process noises are determined empirically using parameter sweeping to jointly optimize position and orientation estimates from the filter.

insfilterMARG 測量噪聲描述了有多少噪聲破壞了傳感器讀數。 這些值基於 imuSensor 和 gpsSensor 參數。
過程噪聲描述了濾波器方程描述狀態演化的程度。 過程噪聲是使用參數掃描根據經驗確定的,以聯合優化來自濾波器的位置和方向估計。

% Measurement noises
Rmag = 0.0862; % Magnetometer measurement noise
Rvel = 0.0051; % GPS Velocity measurement noise
Rpos = 5.169; % GPS Position measurement noise
 
% Process noises
fusionfilt.AccelerometerBiasNoise = 0.010716;
fusionfilt.AccelerometerNoise = 9.7785;
fusionfilt.GyroscopeBiasNoise = 1.3436e-14;
fusionfilt.GyroscopeNoise = 0.00016528;
fusionfilt.MagnetometerBiasNoise = 2.189e-11;
fusionfilt.GeomagneticVectorNoise = 7.67e-13;
 
% Initial error covariance
fusionfilt.StateCovariance = 1e-9*eye(22);

Initialize Scopes

The HelperScrollingPlotter scope enables plotting of variables over time. It is used here to track errors in pose. The HelperPoseViewer scope allows 3-D visualization of the filter estimate and ground truth pose. The scopes can slow the simulation. To disable a scope, set the corresponding logical variable to false.
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', ...
[ -3, 3
-2, 2
-2 2
-2 2]);
end
 
if usePoseView
posescope = HelperPoseViewer(...
'XPositionLimits', [-15 15], ...
'YPositionLimits', [-15, 15], ...
'ZPositionLimits', [-10 10]);
end

Simulation Loop

The main simulation loop is a while loop with a nested for loop. The while loop executes at gpsFs, which is the GPS sample rate. The nested for loop executes at imuFs, which is the IMU sample rate. The scopes are updated at the IMU sample rate.
主要的模擬循環是一個帶有嵌套 for 循環的 while 循環。 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

 

  
 
 
 

Error Metric Computation

Position and orientation estimates were logged throughout the simulation. Now compute an end-to-end root mean squared error for both position and orientation.
在整個模擬過程中記錄了位置和方向估計。 現在計算位置和方向的端到端均方根誤差。
對於方向,四元數距離是減去具有不連續性的歐拉角的更好選擇。 四元數距離可以用 |dist| 計算 函數,它給出以弧度爲單位的方向角度差。 轉換爲度數以便在命令窗口中顯示。
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.50 , Y: 0.79, Z: 0.65 (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)));
1.45 (degrees)

Copyright 2017-2022 The MathWorks, Inc.

 
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章