惯性导航的IMU和GPS融合
本例展示了如何构建适用于无人机（UAV）或四架直升机的IMU+GPS融合算法。
本例使用加速度计、陀螺仪、磁强计和GPS来确定无人机的方向和位置。
模拟装置
设置采样率。在典型系统中，加速计和陀螺仪以相对较高的采样率运行。在融合算法中处理来自这些传感器的数据的复杂度相对较低。相反，GPS，在某些情况下还有磁强计，以相对较低的采样率运行，以及与处理相关的复杂度在该融合算法中，磁强计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。
为了模拟这种配置，IMU（加速计、陀螺仪和磁强计）的采样频率为160 Hz，GPS的采样频率为1 Hz。磁强计的每160个样本中只有一个样本用于融合算法，因此在实际系统中，磁强计的采样率可以低得多。
imuFs=160；gpsFs=1；定义这个模拟场景在地球上发生的位置%纬度、经度和高度。refloc=[42.2825-72.3430 53.0352]；%验证|gpsFs|是否除|imuFs|。这允许传感器取样%的比率将使用嵌套的for循环而不使用复杂的抽样比率来模拟%匹配。imuSamplesPerGPS=（imuFs/gpsFs）；断言（imuSamplesPerGPS==fix（imuSamplesPerGPS），...GPS采样率必须是IMU采样率的整数因子。);
融合滤波器
创建融合IMU+GPS测量的滤波器。融合滤波器使用扩展卡尔曼滤波器跟踪方向（作为四元数）、速度、位置、传感器偏差和地磁矢量。
这因斯菲尔特马尔格有几种处理传感器数据的方法，包括预测,fusemag和fusegps．这个预测方法将来自IMU的加速度计和陀螺仪样本作为输入预测方法每次对加速度计和陀螺仪进行采样，该方法基于加速度计和陀螺仪提前一个时间步预测状态，并更新扩展卡尔曼滤波器的误差协方差。
这个fusegps该方法将GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波器状态，该卡尔曼增益根据各种传感器输入的不确定性对其进行加权。这里还更新了误差协方差，这次也使用了卡尔曼增益。
这个fusemag该方法类似，但根据磁强计样本更新状态、卡尔曼增益和误差协方差。
虽然因斯菲尔特马尔格将加速度计和陀螺仪样本作为输入，将它们集成，分别计算速度增量和角度增量。滤波器跟踪磁强计的偏置和这些集成信号。
fusinfilt=insfilterMARG；fusinfilt.IMUSampleRate=imuFs；fusinfilt.ReferenceLocation=refloc；
无人机弹道
本例使用从无人机记录的保存轨迹作为地面真实。该轨迹被馈送到几个传感器模拟器，以计算模拟加速度计、陀螺仪、磁力计和GPS数据流。
加载“地面真实”无人机轨迹。负载LoggedQuadcopter.mattrajData；trajOrient = trajData.Orientation;trajVel = trajData.Velocity;trajPos = trajData.Position;trajAcc = trajData.Acceleration;trajAngVel = trajData.AngularVelocity;%初始化用于传感器仿真的随机数发生器%噪音。rng（1）
GPS传感器
在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
gps=gps传感器(“UpdateRate”，gpsFs）；gps.ReferenceLocation=refloc；gps.DecayFactor=0.5；%随机游动噪声参数gps。HorizontalPositionAccuracy = 1.6;gps。VerticalPositionAccuracy = 1.6;gps。VelocityAccuracy = 0.1;
IMU传感器
通常，无人机使用集成MARG传感器（磁性、角速度、重力）用于姿态估计。要对MARG传感器建模，请定义包含加速计、陀螺仪和磁强计的IMU传感器模型。在实际应用中，三个传感器可能来自单个集成电路或单独的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。
imu=imu传感器(“加速陀螺仪磁头”,“采样器”, imuFs);imu。磁场= [19.5281 -5.0741 48.0067];%加速计imu.Accelerator.MeasurementTrange=19.6133；imu.Accelerator.Resolution=0.0023928；imu.Accelerator.ConstantBias=0.19；imu.Accelerator.Noisedenity=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);
初始化对象的状态向量因斯菲尔特马尔格
这个因斯菲尔特马尔格跟踪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
地面真实值用于帮助初始化过滤器状态，因此过滤器可以快速收敛到正确答案。
%初始化过滤器的状态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;
初始化变量的方差因斯菲尔特马尔格
这个因斯菲尔特马尔格测量噪声描述有多少噪声会损坏传感器读数。这些值基于imuSensor和gpsSensor参数。
过程噪声描述了滤波器方程对状态演化的描述程度。过程噪声通过参数扫描以经验方式确定，从而联合优化滤波器的位置和方向估计。
%测量噪声Rmag=0.09；%磁强计测量噪声Rvel=0.01；速度测量噪声Rpos=2.56；%GPS位置测量噪声%过程噪声fusionfilt.AccelerometerBiasNoise=2e-4；fusionfilt.AccelerometerNoise=2；fusionfilt.陀螺仪biasNoise=1e-16；fusionfilt.陀螺仪Noise=1e-5；fusionfilt.MagnetometerBiasNoise=1e-10；fusionfilt.GeomagneticVectorNoise=1e-12；%初始误差协方差fusionfilt.StateConvariance=1e-9*个（22）；
初始化作用域
这个助手CrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势错误。这个HelperPoseViewer作用域允许对滤波器估计和地面真实姿态进行三维可视化。作用域会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为错误的．
useErrScope = true;%打开流错误绘图usePoseView=true；打开3d姿势查看器如果UseerScope errscope=HelperScrollingPlotter(...“NumInputs”4....“时间跨度”10...“采样器”，imuFs，...“YLabel”, {“学位”,...“米”,...“米”,...“米”},...“标题”, {“四元数距离”,...“X位置错误”,...“Y位置错误”,...“位置Z错误”},...“YLimits”,...[ -1, 1 -2, 2 -2 2 -2 2]);终止如果usePoseView posescope=HelperPoseViewer(...“XPositionLimits”15 [-15],...“YPositionLimits”, [-15, 15],...“ZPositionLimits”, -10年[10]);终止
模拟回路
主模拟循环是一个带有嵌套的for循环的while循环。while循环在gpsFs，即GPS采样率。嵌套的for循环在imuFs，即IMU采样率。范围是按照IMU采样率更新的。
%循环设置-| trajData |有大约142秒的记录数据。二次模拟=50；%模拟大约50秒numsamples=secondsToSimulate*imuFs；loopBound=floor（numsamples）；loopBound=floor（loopBound/imuFs）*imuFs；%确保有足够的IMU样本%记录用于最终度量计算的数据。pqorient=四元数。零（环限，1）；pqpos=零（环限，3）；fcnt=1；而（fcnt<=循环绑定）% |预测IMU更新频率下|环路。对于ff=1:imuSamplesPerGPS%模拟来自当前姿势的IMU数据。[accel，gyro，mag]=惯性测量单元（trajAcc（fcnt，：），trajAngVel（fcnt，：），...trajOrient (fcnt));%使用|预测|方法估计基于滤波器的状态%的模拟加速度计和陀螺仪信号。预测(accel fusionfilt,陀螺);%获取过滤器状态的当前估计值。[fusedPos，fusedOrient]=姿势（fusionfilt）；%保存位置和方向以进行后期处理。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));终止%更新姿势查看器。如果使用PoseView 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错误。fprintf('\n\ n端到端仿真位置均方根误差\n');
端到端模拟位置均方根误差
msep=sqrt（平均值（位置^2））；fprintf('\tX:%.2f，Y:%.2f，Z:%.2f（米）\n\n'，msep（1），...msep（2），msep（3））；
X: 0.57 Y: 0.53 Z: 0.68(米)
fprintf('端到端四元数距离RMS错误（度）\n');
端到端四元数距离RMS误差（度）
fprintf('\t%.2f（度）\n\n', sqrt(平均(quatd ^ 2)));
0.32(度)
你也可以从以下列表中选择一个网站: