惯性导航的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 = =修复(imuSamplesPerGPS),．．.GPS采样率必须是IMU采样率的整数因子。）；
融合滤波器
创建滤波器融合IMU + GPS测量值。融合滤波器使用扩展卡尔曼滤波器来跟踪定向(四元数)、速度、位置、传感器偏差和地磁矢量。
这insfilterMARG有几种处理传感器数据的方法，包括预测，fusemag和fusegps．的预测方法以来自IMU的加速度计和陀螺仪样本作为输入。调用预测方法对加速度计和陀螺仪进行采样。该方法利用加速度计和陀螺仪提前一个时间步预测状态。本文对扩展卡尔曼滤波器的误差协方差进行了更新。
的fusegps方法以GPS样本为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波状态，卡尔曼增益根据传感器输入的不确定性对其进行加权。这里也更新了误差协方差，这次也使用了卡尔曼增益。
的fusemag方法是相似的，但更新状态，卡尔曼增益，误差协方差基于磁强计样本。
虽然insfilterMARG以加速度计和陀螺仪样本为输入，积分计算速度和角度。滤波器跟踪磁强计的偏置和这些综合信号。
fusionfilt = insfilterMARG;fusionfilt。IMUSampleRate = imuFs;fusionfilt。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) = gpsSensor (“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 = imuSensor (“accel-gyro-mag”，“SampleRate”, 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;速度测量噪声rpo = 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);
初始化范围
的HelperScrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势错误。的HelperPoseViewer范围允许过滤器估计和地面真实姿态的三维可视化。示波器会减慢模拟的速度。若要禁用范围，请将相应的逻辑变量设置为假．
useErrScope = true;%打开流错误图usePoseView = true;打开3d姿势查看器如果usererrscope errscope = HelperScrollingPlotter(．．.“NumInputs”4．．.“时间间隔”10．．.“SampleRate”imuFs,．．.“YLabel”, {“度”，．．.“米”，．．.“米”，．．.“米”}，．．.“标题”, {“四元数距离”，．．.“X位置错误”，．．.“Y位置错误”，．．.“Z位置错误”}，．．.“YLimits”，．．.[- 1,1 - 2,2 -2 2]);结束如果使用posescope = HelperPoseViewer(．．.“XPositionLimits”15 [-15],．．.“YPositionLimits”(-15年,15),．．.“ZPositionLimits”, -10年[10]);结束
模拟循环
主模拟循环是一个带有嵌套的for循环的while循环。while循环在gpsFs，即GPS采样率。嵌套的for循环在imuFs，即IMU采样率。范围是按照IMU采样率更新的。
|有大约142秒的记录数据。secondsToSimulate = 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));结束%更新姿势查看器。如果使用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 msep (2), (3));
X: 0.57 Y: 0.53 Z: 0.68(米)
流('端到端四元数距离RMS误差(度)\n'）；
端到端四元数距离RMS误差(度)
流(' \ t %。2 f(度)\ n \ n ', sqrt(平均(quatd ^ 2)));
0.32(度)
你也可以从以下列表中选择一个网站:
