惯性导航中的IMU与GPS融合
这个示例展示了如何构建适用于无人机(uav)或四轴飞行器的IMU + GPS融合算法。
该示例使用加速度计，陀螺仪，磁力计和GPS来确定UAV的方向和位置。
模拟设置
设置采样率。在一个典型的系统中，加速度计和陀螺仪以相对较高的采样率运行。该融合算法处理这些传感器数据的复杂度相对较低。相反，GPS，以及在某些情况下的磁力计，以相对较低的采样率运行，并且与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。
为了模拟这个配置，IMU(加速度计、陀螺仪和磁力计)以160赫兹采样，GPS以1赫兹采样。每160个磁力计样本中，只有1个样本被给出给融合算法，因此在实际系统中，磁力计的采样率要低得多。
imufs = 160;GPSFS = 1;%使用％纬度，经度和高度。RESHOC = [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样本的滤波器状态。此时也会更新错误协方差，此时使用Kalman Gain。
的熔丝标记方法是相似的，但更新状态，卡尔曼增益，误差协方差基于磁强计样本。
尽管insfilterMARG以加速度计和陀螺仪样本为输入，积分计算速度和角度。滤波器跟踪磁强计的偏置和这些综合信号。
Fusionfilt = Insfiltermarg;fusionfilt.imusamplerve = imufs;fusionfilt.referenceLocation = RESTOC;
无人机轨迹
本例使用无人机记录的保存轨迹作为地面真实情况。该轨迹被送入多个传感器模拟器，以计算模拟的加速度计、陀螺仪、磁强计和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传感器
通常，UAV使用集成的MARG传感器（磁，角速率，重力）来进行姿势估计。为了模拟MARG传感器，定义包含加速度计，陀螺仪和磁力计的IMU传感器型号。在真实世界中，三个传感器可以来自单个集成电路或单独的电路。这里设置的属性值是低成本MEMS传感器的典型形式。
imu = imuSensor ('Accel-Gyro-Mag'，'采样率'，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);
初始化范围
的救助人员作用域允许绘制随时间变化的变量。此处使用作用域跟踪姿势中的错误HelperpositeViewer.范围允许3-D滤波器估计和地面真理姿势的可视化。范围可以减慢模拟。要禁用范围，请将相应的逻辑变量设置为错误的．
UseerScope=true；%打开流错误图movingsview = true;%打开三维姿势查看器如果usererrscope errscope = HelperScrollingPlotter(．..“努明普茨”，4，．..'时间跨度', 10,．..'采样率'，imufs，．..“伊拉贝尔”, {“度”，．..'米'，．..'米'，．..'米'}，．..“头衔”, {'四元距离'，．..“位置X错误”，．..“位置Y错误”，．..“Z位置错误”}，．..'ylimits'，．..[- 1,1 - 2,2 -2 2]);结尾如果UpployView 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 =地板（NumSamples）;LoopBound =楼层（LoopBound / IMUFS）* IMUFS;％确保足够的IMU样本最终度量计算的％日志数据。pqorient = quaternion.zeros (loopBound, 1);pqpos = 0 (loopBound, 3);fcnt = 1;虽然（fcnt <= loopbound）%|预测| IMU更新频率下的循环。为了ff = 1: imuSamplesPerGPS%从当前姿势模拟IMU数据。[Accel，Gyro，Mag] = IMU（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））；结尾%更新姿势查看器。如果UpployView 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（度）
您还可以从以下列表中选择网站：