惯性导航中的IMU与GPS融合
这个示例展示了如何构建适用于无人机(uav)或四轴飞行器的IMU + GPS融合算法。
本例使用加速度计、陀螺仪、磁强计和GPS来确定无人机的方向和位置。
模拟装置
设置采样率。在一个典型的系统中，加速度计和陀螺仪以相对较高的采样率运行。该融合算法处理这些传感器数据的复杂度相对较低。相反，GPS，以及在某些情况下的磁力计，以相对较低的采样率运行，并且与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。
为了模拟这个配置，IMU(加速度计、陀螺仪和磁力计)以160赫兹采样，GPS以1赫兹采样。每160个磁力计样本中，只有1个样本被给出给融合算法，因此在实际系统中，磁力计的采样率要低得多。
imuFs=160；gpsFs=1；%使用%纬度、经度和高度。refloc=[42.2825-72.3430 53.0352]；%验证| gpsFs |分割| imuFs |。这允许传感器采样%使用无复杂采样率的嵌套for循环模拟的速率%匹配。imuSamplesPerGPS=（imuFs/gpsFs）；断言（imuSamplesPerGPS==fix（imuSamplesPerGPS），．..“GPS采样率必须是IMU采样率的整数因子。”);
融合滤波器
创建融合IMU+GPS测量的滤波器。融合滤波器使用扩展卡尔曼滤波器跟踪方向（作为四元数）、速度、位置、传感器偏差和地磁矢量。
这insfilterMARG有几种处理传感器数据的方法，包括预测，熔丝标记和fusegps这个预测方法以来自IMU的加速度计和陀螺仪样本作为输入。调用预测方法对加速度计和陀螺仪进行采样。该方法利用加速度计和陀螺仪提前一个时间步预测状态。本文对扩展卡尔曼滤波器的误差协方差进行了更新。
的fusegps该方法将GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波器状态，该卡尔曼增益根据各种传感器输入的不确定性对其进行加权。这里还更新了误差协方差，这次也使用了卡尔曼增益。
的熔丝标记方法是相似的，但更新状态，卡尔曼增益，误差协方差基于磁强计样本。
尽管insfilterMARG以加速度计和陀螺仪样本为输入，积分计算速度和角度。滤波器跟踪磁强计的偏置和这些综合信号。
fusinfilt=insfilterMARG；fusinfilt.IMUSampleRate=imuFs；fusinfilt.ReferenceLocation=refloc；
无人机轨迹
本例使用无人机记录的保存轨迹作为地面真实情况。该轨迹被送入多个传感器模拟器，以计算模拟的加速度计、陀螺仪、磁强计和GPS数据流。
%加载“地面真相”无人机弹道。负载LoggedQuadcopter.mat特拉伊达; trajOrient=trajData.Orientation；trajVel=trajData.Velocity；trajPos=trajData.Position；trajAcc=trajData.加速度；trajAngVel=trajData.AngularVelocity；%初始化传感器仿真中使用的随机数生成器%噪音。rng (1)
GPS传感器
在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
全球定位系统(gps) = gpsSensor (“更新”, gpsFs);gps。ReferenceLocation = refloc;gps。DecayFactor = 0.5;%随机漫步噪声参数gps.水平位置精度=1.6；gps.垂直位置精度=1.6；gps.速度精度=0.1；
IMU传感器
通常，无人机使用集成MARG传感器（磁性、角速度、重力）用于姿态估计。要对MARG传感器建模，请定义包含加速计、陀螺仪和磁强计的IMU传感器模型。在实际应用中，三个传感器可能来自单个集成电路或单独的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。
imu = imuSensor (“加速陀螺仪磁头”，“采样器”，imuFs）；imu.磁场=[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;%陀螺仪imu.Gyrool.MeasurementRange=deg2rad（250）；imu.Gyrool.Resolution=deg2rad（0.0625）；imu.Gyrool.ConstantBias=deg2rad（3.125）；imu.Gyrool.AxesMisalignment=1.5；imu.Gyrool.NoiseDensity=deg2rad（0.025）；%磁强计imu.Magnetometer.MeasurementTrange=1000；imu.Magnetometer.Resolution=0.1；imu.Magnetometer.ConstantBias=100；imu.Magnetometer.NoiseDensity=0.3/sqrt（50）；
对象的状态向量初始化insfilterMARG
的insfilterMARG跟踪22个元素向量中的姿势状态。状态为：
状态单位状态向量索引方向为四元数1:4位置（NED）m 5:7速度（NED）m/s 8:10δ角偏差（XYZ）rad 11:13δ角偏差（XYZ）m/s 14:16地磁场向量（NED）uT 17:19磁强计偏差（XYZ）uT 20:22
Ground truth被用来帮助初始化过滤器的状态，所以过滤器会很快收敛到好的答案。
%初始化过滤器的状态初始状态=零（22,1）；初始状态（1:4）=紧致（平均值（1:100））；初始状态（5:7）=平均值（trajPos（1:100，：），1）；初始状态（8:10）=平均值（trajVel（1:100，：），1）；初始状态（11:13）=惯性陀螺仪康斯坦比亚斯/imuFs；初始状态（14:16）=惯性加速度计康斯坦比亚斯/imuFs；初始状态（17:19）=惯性磁场；初始状态（20:22）=imu.磁强计.康斯坦比亚斯；fusinfilt.State=initstate；
初始化变量的方差insfilterMARG
的insfilterMARG测量噪声描述了有多少噪声破坏了传感器的读数。这些值是基于免疫传感器和GPS传感器参数。
过程噪声描述了滤波器方程对状态演化的描述程度。过程噪声通过参数扫描以经验方式确定，从而联合优化滤波器的位置和方向估计。
%测量噪声Rmag = 0.09;%磁强计测量噪声Rvel = 0.01;%GPS测速噪声Rpos=2.56；% GPS位置测量噪声%过程噪音fusionfilt。AccelerometerBiasNoise = 2的军医;fusionfilt。AccelerometerNoise = 2;fusionfilt。GyroscopeBiasNoise = 1 e-16;fusionfilt。GyroscopeNoise = 1 e-5;fusionfilt。MagnetometerBiasNoise = 1平台以及; fusionfilt.GeomagneticVectorNoise = 1e-12;%初始误差协方差fusionfilt。StateCovariance = 1 e-9 *的(22);
初始化作用域
的助手CrollingPlotter作用域允许绘制随时间变化的变量。此处使用作用域跟踪姿势中的错误HelperPoseViewer作用域允许对滤波器估计和地面真实姿态进行三维可视化。作用域会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为错误的．
UseerScope=true；%打开流错误图usePoseView=true；%打开三维姿势查看器如果usererrscope errscope = HelperScrollingPlotter(．..“努明普茨”4.．..“时间跨度”, 10,．..“采样器”，imuFs，．..“伊拉贝尔”, {“度”，．..“米”，．..“米”，．..“米”},．..“头衔”, {“四元数距离”，．..“位置X错误”，．..“位置Y错误”，．..“Z位置错误”},．..“YLimits”，．..[- 1,1 - 2,2 -2 2]);终止如果usePoseView posescope=HelperPoseViewer(．..“XPositionLimits”, [-15 15],．..“YPositionLimits”, [-15, 15],．.."限制", [-10 10]);终止
模拟回路
主模拟循环是一个带有嵌套for循环的while循环gpsFs，这是GPS采样率。嵌套for循环在imuFs，这是IMU采样率。范围以IMU采样率更新。
|有大约142秒的记录数据。二次模拟=50；%模拟大约50秒numsamples=secondsToSimulate*imuFs；loopBound=floor（numsamples）；loopBound=floor（loopBound/imuFs）*imuFs；%确保有足够的IMU样本%记录用于最终度量计算的数据。pqorient = quaternion.zeros (loopBound, 1);pqpos = 0 (loopBound, 3);fcnt = 1;虽然（fcnt<=循环绑定）%|预测| IMU更新频率下的循环。对于ff = 1: imuSamplesPerGPS%从当前姿势模拟IMU数据。[accel，gyro，mag]=惯性测量单元（trajAcc（fcnt，：），trajAngVel（fcnt，：），．..trajOrient（fcnt））；%使用| predict |方法估计基于%模拟加速度计和陀螺仪信号。预测（引信、加速度、陀螺仪）；%获取过滤器状态的当前估计值。[fusedPos, fusedOrient] = pose(融合);%保存位置和方向，以便后期处理。pqorient (fcnt) = fusedOrient;pqpos (fcnt:) = fusedPos;%计算误差并绘图。如果UseerScope定向器=rad2deg（距离（保险丝定向），．..trajOrient（fcnt））；posErr=fusedPos-trajPos（fcnt，：）；errscope（定向器、posErr（1）、posErr（2）、posErr（3））；终止%更新姿势查看器。如果使用PoseView posescope（pqpos（fcnt，：）、pqorient（fcnt）、trajPos（fcnt，：），．..trajOrient（fcnt，：）；终止fcnt=fcnt+1；终止%下一步将以GPS采样率进行。%基于当前姿势模拟GPS输出。[lla，gpsvel]=gps（trajPos（fcnt，：），trajVel（fcnt，：）；%根据GPS数据和磁场校正滤波器状态%现场测量。fusegps（fusionfilt、lla、RPO、gpsvel、Rvel）；fusemag（fusionfilt、mag、Rmag）；终止
误差度量计算
在整个仿真过程中记录了位置和方向估计。现在计算端到端均方根误差的位置和方向。
posd=pqpos（1:loopBound，：）-trajPos（1:loopBound，：）；对于方向，四元数距离是一个更好的选择%减去不连续的欧拉角。四元数%可以使用| dist |函数计算距离，该函数给出%以弧度表示的方向角差。转换为度%用于在命令窗口中显示。quatd = rad2deg(dist(pqorient(1:loopBound)， trajOrient(1:loopBound))); / / / / / / / / / /%在命令窗口中显示RMS错误。流(“\n\n终止模拟位置RMS错误\n”);
端到端仿真位置均方根误差
msep =√意味着(posd ^ 2));流(“\ tX: %。2f, Y: %。Z: % 2 f。2 f(米)\ n \ n”，msep（1），．..msep（2），msep（3））；
X:0.57，Y:0.53，Z:0.68（米）
流('端到端四元数距离RMS错误（度）\n');
端到端四元数距离RMS误差（度）
流('\t%.2f（度）\n\n'，sqrt（平均数（四分之^2））；
0.32（度）
您还可以从以下列表中选择网站：
