惯性导航的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有几种处理传感器数据的方法，包括预测，fusemag和fusegps．的预测方法以来自IMU的加速度计和陀螺仪样本作为输入。调用预测方法对加速度计和陀螺仪进行采样。该方法利用加速度计和陀螺仪提前一个时间步预测状态。本文对扩展卡尔曼滤波器的误差协方差进行了更新。
的fusegps方法将GPS样本作为输入。该方法通过计算根据它们的不确定性来计算加权各种传感器输入的卡尔曼增益来更新基于GPS样本的滤波器状态。此时也会更新错误协方差，此时使用Kalman Gain。
的fusemag方法是相似的，但更新状态，卡尔曼增益，误差协方差基于磁强计样本。
虽然insfilterMARG以加速度计和陀螺仪样本为输入，积分计算速度和角度。滤波器跟踪磁强计的偏置和这些综合信号。
Fusionfilt = Insfiltermarg;fusionfilt.imusamplerve = imufs;fusionfilt.referenceLocation = RESTOC;
无人机轨迹
本例使用从无人机记录的保存轨迹作为地面真实。该轨迹被馈送到几个传感器模拟器，以计算模拟加速度计、陀螺仪、磁力计和GPS数据流。
加载“地面真实”无人机轨迹。加载LoggedQuadcopter.mattrajData；trajOrient = trajData.Orientation;trajVel = trajData.Velocity;trajPos = trajData.Position;trajAcc = trajData.Acceleration;trajAngVel = trajData.AngularVelocity;%初始化传感器仿真中使用的随机数生成器％ 噪音。rng (1)
GPS传感器
在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
全球定位系统(gps) = gpsSensor (“UpdateRate”, gpsFs);gps。ReferenceLocation = refloc;gps。DecayFactor = 0.5;%随机漫步噪声参数gps。HorizontalPositionAccuracy = 1.6;gps。VerticalPositionAccuracy = 1.6;gps。VelocityAccuracy = 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.Gyroscope.MeasurementRange =函数(250);imu.Gyroscope.Resolution函数= (0.0625);imu.Gyroscope.ConstantBias函数= (3.125);imu.Gyroscope.AxesMisalignment = 1.5;imu.Gyroscope.NoiseDensity函数= (0.025);%磁强计imu.Magnetometer.MeasurementRange = 1000;imu.Magnetometer.Resolution = 0.1;imu.Magnetometer.ConstantBias = 100;imu.Magnetometer.NoiseDensity = 0.3/√(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被用来帮助初始化过滤器的状态，所以过滤器会很快收敛到好的答案。
%初始化过滤器的状态initstate = 0(22日1);initstate(1:4) = compact(meanrot(trajOrient(1:100)));initstate(5:7) = mean(trajPos(1:100，:)， 1); / /初始化initstate(8:10) = mean(trajVel(1:10，:)， 1); / /初始化initstate (11:13) = imu.Gyroscope.ConstantBias. / imuFs;initstate(十四16)= imu.Accelerometer.ConstantBias. / imuFs;initstate(十七19)= imu.MagneticField;initstate (20:22) = imu.Magnetometer.ConstantBias;fusionfilt。状态= initstate;
初始化的差异insfilterMARG
的insfilterMARG测量噪声描述了有多少噪声破坏了传感器的读数。这些值是基于imuSensor和gpsSensor参数。
过程噪音描述了过滤器方程描述了状态演进的程度。流程噪声使用扫描以共同优化过滤器的位置和方向估计来确定流程噪声。
%测量噪声Rmag = 0.09;％磁力计测量噪声Rvel = 0.01;速度测量噪声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滤波器估计和地面真理姿势的可视化。范围可以减慢模拟。要禁用范围，请将相应的逻辑变量设置为错误的．
useErrScope = true;%打开流错误图movingsview = true;打开3d姿势查看器如果usererrscope errscope = HelperScrollingPlotter(.．.“NumInputs”，4，.．.'时间跨度'10.．.'采样率'，imufs，.．.“YLabel”,{“度”，.．.'米'，.．.'米'，.．.'米'}，.．.“标题”,{'四元距离'，.．.“X位置错误”，.．.“Y位置错误”，.．.“Z位置错误”}，.．.'ylimits'，.．.[- 1,1 - 2,2 -2 2]);结尾如果UpployView PoseScope = HelperposeViewer（.．.“XPositionLimits”15 [-15],.．.“YPositionLimits”，[-15,15]，.．.“ZPositionLimits”, -10年[10]);结尾
仿真环路
主模拟循环是一个带有嵌套的for循环的while循环。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));%使用|预测|方法估计基于滤波器的状态%的模拟加速度计和陀螺仪信号。预测(accel fusionfilt,陀螺);％获取当前对滤波器状态的估计。[fusedPos, fusedOrient] = pose(融合);%保存位置和方向，以便后期处理。pqorient (fcnt) = fusedOrient;pqpos (fcnt:) = fusedPos;%计算误差并绘图。如果userscope orientErr = rad2deg(dist(fusedOrient，.．.trajOrient (fcnt)));posErr = fusedPos - trajPos(fcnt，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结尾%更新姿势查看器。如果UpployView Posescope（PQPOS（FCNT，:)，PQORIENT（FCNT），Trajpos（FCNT，:)，.．.trajOrient (fcnt:));结尾= FCNT + 1;结尾%接下来的步骤发生在GPS采样率。％基于当前姿势模拟GPS输出。[lla，gpsvel] = gps（trajpos（fcnt，:)，trajvel（fcnt，:)）;％基于GPS数据和磁性校正过滤器状态％场测量。fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);fusemag (fusionfilt杂志,Rmag);结尾
误差指标计算
在整个仿真过程中记录了位置和方向估计。现在计算端到端均方根误差的位置和方向。
posd = pqpos（1：loopbound，:) - trajpos（1：loopbound，:);对于方向，四元数距离是一个更好的选择减去具有不连续性的欧拉角度。四元数可以使用| dist |计算％距离|功能，它给了弧度方向的百分比差异。转换为程度%显示在命令窗口中。quatd = rad2deg(dist(pqorient(1:loopBound)， trajOrient(1:loopBound))); / / / / / / / / / /%在命令窗口中显示RMS错误。流('\n\ n端到端仿真位置均方根误差\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(平均(quatd ^ 2)));
0.32(度)
你也可以从以下列表中选择一个网站:
