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.
该示例使用加速度计，陀螺仪，磁力计和GPS来确定无人机的方向和位置。
仿真设置
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.
imufs = 160;gpsfs = 1;% Define where on the Earth this simulated scenario takes place using the％纬度，经度和高度。反射= [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％匹配。imusamplespergps =（imufs/gpsfs）;断言（imusamplespergps == fix（imusamplespergps），...'GPS sampling rate must be an integer factor of IMU sampling rate.'）；
Fusion Filter
创建过滤器以融合IMU + GPS测量。融合过滤器使用扩展的卡尔曼滤波器来跟踪取向（作为四元组），速度，位置，传感器偏见和地磁矢量。
ThisinsfilterMARG有几种处理传感器数据的方法，包括predict,fusemagandfusegps. Thepredictmethod takes the accelerometer and gyroscope samples from the IMU as inputs. Call thepredictmethod 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.
Thefusegps方法将GPS样本作为输入。该方法通过计算KALMAN根据其不确定性加权各种传感器输入来更新过滤器状态。此时还使用Kalman增益来更新错误协方差。
Thefusemagmethod is similar but updates the states, Kalman gain, and error covariance based on the magnetometer samples.
Though theinsfilterMARGtakes 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.
fusionfilt = insfiltermarg;fusionfilt.imusamplerate = imufs;fusionFilt.ReferenceLocation = REPLOC;
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.
% Load the "ground truth" UAV trajectory.加载LoggedQuadcopter.mattrajData; 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％ 噪音。rng(1)
GPS Sensor
建立了GPS在指定采样率和再保险ference location. The other parameters control the nature of the noise in the output signal.
gps = gpsSensor('UpdateRate', gpsFs); gps.ReferenceLocation = refloc; gps.DecayFactor = 0.5;% Random walk noise parametergps.HorizontalPositionAccuracy = 1.6; gps.VerticalPositionAccuracy = 1.6; gps.VelocityAccuracy = 0.1;
IMU传感器
通常，无人机使用集成的MARG传感器（磁性，角度，重力）进行姿势估计。为了建模MARG传感器，定义包含加速度计，陀螺仪和磁力计的IMU传感器模型。在现实世界中，这三个传感器可能来自单个集成电路或单独的电路。此处设置的属性值是低成本MEMS传感器的典型特征。
imu = imuSensor(“ Accel-Gyro-mag”,'采样率', imuFs); imu.MagneticField = [19.5281 -5.0741 48.0067];％加速度计imu.Accelerometer.MeasurementRange = 19.6133; imu.Accelerometer.Resolution = 0.0023928; imu.Accelerometer.ConstantBias = 0.19; imu.Accelerometer.NoiseDensity = 0.0012356;% Gyroscopeimu.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);% Magnetometerimu.Magnetometer.MeasurementRange = 1000; imu.Magnetometer.Resolution = 0.1; imu.Magnetometer.ConstantBias = 100; imu.Magnetometer.NoiseDensity = 0.3/ sqrt(50);
Initialize the State Vector of theinsfilterMARG
TheinsfilterMARG跟踪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.
% Initialize the states of the filterinitstate =零（22,1）;initstate（1：4）= compact（seanrot（trajorient（1：100）））;initstate（5：7）=平均值（trajpos（1：100，:)，1）;initstate（8:10）=平均值（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
TheinsfilterMARGmeasurement noises describe how much noise is corrupting the sensor reading. These values are based on theimusensorandgpsSensorparameters.
过程噪声描述了滤波器方程式的描述状态演变的效果。使用参数扫描以共同优化滤波器的位置和方向估计值，从经验上确定过程噪声。
% Measurement noisesRmag = 0.09;％磁力计测量噪声Rvel = 0.01;% GPS Velocity measurement noiseRPO = 2.56;% GPS Position measurement noise% Process noisesfusionfilt.AccelerometerBiasNoise = 2e-4; fusionfilt.AccelerometerNoise = 2; fusionfilt.GyroscopeBiasNoise = 1e-16; fusionfilt.GyroscopeNoise = 1e-5; fusionfilt.MagnetometerBiasNoise = 1e-10; fusionfilt.GeomagneticVectorNoise = 1e-12;% Initial error covariancefusionfilt.StateCovariance = 1e-9*ones(22);
初始化范围
ThehelpersCrollingPlotterscope enables plotting of variables over time. It is used here to track errors in pose. Thehelpposeviewer范围允许3D可视化滤波器估计和地面真相姿势。范围可以减慢模拟。要禁用示波器，请将相应的逻辑变量设置为错误的.
useErrScope = true;% Turn on the streaming error plotuseposeview = true;% Turn on the 3-D pose viewer如果useErrScope errscope = HelperScrollingPlotter(...“NumInputs”，4，...'时间跨度', 10,...'采样率'，imufs，...'YLabel', {'degrees',...“米”,...“米”,...“米”}，，...'Title', {“四个距离”,...'Position X Error',...'Position Y Error',...'Position Z Error'}，，...'ylimits',...[ -1, 1 -2, 2 -2 2 -2 2]);结尾如果useposeview posescope = helperposeviewer（...'XPositionLimits'15 [-15],...'YPositionLimits'，[-15，15]，...'ZPositionLimits', [-10 10]);结尾
仿真循环
The main simulation loop is a while loop with a nested for loop. The while loop executes atGPSFS, which is the GPS sample rate. The nested for loop executes atimuFs, which is the IMU sample rate. The scopes are updated at the IMU sample rate.
% Loop setup - |trajData| has about 142 seconds of recorded data.第二稳定= 50;% simulate about 50 secondsnumsamples = secondStossimulate*imufs;loopbound = floor（numsamples）;loopbound = floor（loopbound/imufs）*imufs;％确保足够的IMU样品最终度量计算的日志数据％。pqorient = quaternion.zeros(loopBound,1); pqpos = zeros(loopBound,3); fcnt = 1;while（fcnt <=循环）% |predict| loop at IMU update frequency.为了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);％获取过滤状态的当前估计值。[fusedPos, fusedOrient] = pose(fusionfilt);% Save the position and orientation for post processing.pqorient(fcnt) = fusedOrient; pqpos(fcnt,:) = fusedPos;% Compute the errors and plot.如果useErrScope orientErr = rad2deg(dist(fusedOrient,...trajOrient(fcnt) )); posErr = fusedPos - trajPos(fcnt,:); errscope(orientErr, posErr(1), posErr(2), posErr(3));结尾% Update the pose viewer.如果useposeview posescope（pqpos（fcnt，:)，pqorient（fcnt），trajpos（fcnt，:)，:)，...trajOrient(fcnt,:) );结尾fcnt = fcnt + 1;结尾% This next step happens at the GPS sample rate.％基于当前姿势模拟GPS输出。[lla，gpsvel] = gps（trajpos（fcnt，:)，trajvel（fcnt，:)）;％根据GPS数据和磁性更正过滤器状态％现场测量。fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel); fusemag(fusionfilt, mag, Rmag);结尾
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.
posd = pqpos（1：loopbound，:) -trajpos（1：loopbound，:);% For orientation, quaternion distance is a much better alternative to％减去欧拉角，具有不连续性。四元组％距离可以使用| DIST |计算。功能，给出弧度方向的角度差异％。转换为学位%为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('端到端季节距离rms误差（度）\ n'）；
端到端的四个距离RMS误差（度）
fprintf('\ t％.2f（degrees）\ n \ n', sqrt(mean(quatd.^2)));
0.32 (degrees)
You can also select a web site from the following list:
美洲
- América Latina(Español)
- 加拿大（英语）
- 美国（英语）