IMU和GPS融合惯性导航
此示例显示了如何构建适用于无人机（无人机）或Quadcopters的IMU + GPS融合算法。
本例使用加速度计、陀螺仪、磁力计和GPS来确定无人机的方向和位置。
仿真设置
设置采样率。在典型的系统中，加速度计和陀螺以相对较高的样品速率运行。从融合算法中的那些传感器处理数据的复杂性相对较低。相反，GPS，以及在某些情况下，磁力计以相对较低的采样率运行，以及与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样品以相同的低速率加工在一起，加速度计和陀螺仪样品以相同的高速加工在一起。
为了模拟这种配置，IMU（加速度计，陀螺仪和磁力计）在160Hz上采样，并且GPS在1 Hz时采样。仅给予融合算法中每160个样品中的每个160个样品中的一个，因此在真实系统中，磁力计可以以更低的速率进行采样。
imuFs = 160;gpsFs = 1;％定义地球上的哪个模拟场景使用%纬度，经度和高度。Refloc = [42.2825 -72.3430 53.0352];％验证| GPSFS |分开| imufs |。这允许传感器样本使用嵌套循环模拟的％速率而无需复杂的采样率%匹配。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;
无人机轨迹
此示例使用从UAV记录的保存轨迹作为地面真相。此轨迹被送入多个传感器模拟器以计算模拟加速度计，陀螺仪，磁力计和GPS数据流。
％加载“地面真理”UAV轨迹。负载loggedquadcopter.mat.trajdata.;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 = RESTOC;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.MagneticField = [19.5281-5.0741 48.0067];%加速度计Imu.accelerometerm.measurementRange = 19.6133;Imu.accelerometer。效果= 0.0023928;imu.accelerometer.constantbias = 0.19;IMU.Accelerometer。不生系= 0.0012356;％陀螺仪imu.gyroscope.measurementrange = deg2rad（250）;imu.gyroscope.resolution = deg2rad（0.0625）;imu.gyroscope.constantbias = deg2rad（3.125）;imu.gyroscope.axesmisalignment = 1.5;imu.gyroscope.noisedensity = deg2rad（0.025）;％磁力计imu.magnetometer.measurementRange = 1000;IMU.Magnetometer.resolution = 0.1;imumagnetometer.constantbias = 100;IMU.Magnetometer.NoizeSensy = 0.3 / SQRT（50）;
初始化的状态向量insfiltermarg.
这insfiltermarg.跟踪一个包含22个元素的向量中的姿势状态。美国是:
状态单位状态矢量索引取向为四元数1：4位置（NED）M 5：7速度（NED）M / S 8:10 Delta角度偏置（XYZ）Rad 11:13 Delta Velocity偏置（XYZ）M / S 14：16地形磁场矢量（NED）UT 17:19磁力计偏差（XYZ）UT 20:22
地面真理用于帮助初始化过滤器状态，因此过滤器会很快收敛到良好的答案。
％初始化过滤器的状态initstate = zeros（22,1）;initstate（1：4）= compact（VileRot（Trajorient（1：100）））;初始持久性（5：7）=均值（trajpos（1：100，:)，1）;初学者（8:10）=均值（trajvel（1：100，:)，1）;initstate（11:13）= imu.gyroscope.constantbias./imufs;initstate（14:16）= imu.accelerometer.constantbias./imufs;初学者（17:19）= IMUMagnetfield;initstate（20:22）= imu.magnetometer.constantbias;fusionfilt.state = initstate;
初始化变量的方差insfiltermarg.
这insfiltermarg.测量噪音描述了噪音损坏了传感器读数。这些值基于imusvesor.和GPSSensor.参数。
过程噪声描述了滤波器方程如何很好地描述状态演化。利用参数扫描联合优化滤波器的位置和方向估计，经验地确定过程噪声。
％测量噪声RMAG = 0.09;%磁强计测量噪声rvel = 0.01;％GPS速度测量噪声rpo = 2.56;％GPS位置测量噪声％流程噪音Fusionfilt.accelerometerBiasnoise = 2E-4;fusionfilt.acceleromernoise = 2;Fusionfilt.gyroscopbiasnoise = 1e-16;Fusionfilt.gyroscopenoise = 1E-5;Fusionfilt.MagnetomerBiasnoise = 1E-10;fusionfilt.geomagneticVectornoise = 1e-12;％初始错误协方差fusionfilt.statecovariance = 1e-9 *那些（22）;
初始化范围
这HelperScrollingPlotter范围使得能够随着时间的推移绘制变量。这里用于跟踪姿势的错误。这HelperPoseViewer范围允许过滤器估计和地面真实姿态的三维可视化。示波器会减慢模拟的速度。若要禁用范围，请将相应的逻辑变量设置为假。
umererrscope = true;％打开流媒体错误绘图usePoseView = true;％打开3-D姿势查看器如果useerrscope errscope = evermercrollingplotter（......'numinputs'4......“时间间隔”10，......“SampleRate”imuFs,......'ylabel'，{'度'那......“米”那......“米”那......“米”}，......'标题'，{“四元数距离”那......'位置x错误'那......'位置y错误'那......'位置z错误'}，......“YLimits”那......[-1,1 -2,2 -2 2 -2 2]）;结束如果使用posescope = HelperPoseViewer(......'xpositionlimits'，[-15 15]，......'ypositionLimits'(-15年,15),......'zpositionlimits'，[-10 10]）;结束
模拟循环
主模拟循环是一小时的循环，带有嵌套循环。循环执行虽然循环执行gpsFs，这是GPS采样率。嵌套循环执行IMUFS.，这是IMU采样率。该范围以IMU采样率更新。
％循环设置 - | trajdata |有大约142秒的记录数据。secondsToSimulate = 50;％模拟约50秒numsamples = secondsToSimulate * imuFs;loopBound =地板(numsamples);loopBound =地板(loopBound / imuFs) * imuFs;%确保有足够的IMU样品%记录最终度量计算的数据。pqorient = quaternion.zeros（LoopBound，1）;pqpos = zeros（loopbound，3）;fcnt = 1;尽管(fcnt < = loopBound)％|预测|在IMU更新频率下循环。为ff = 1：imusamplespergps％模拟来自当前姿势的IMU数据。[accel, gyro, mag] = imu(trajAcc(fcnt，:)， trajAngVel(fcnt，:)，)，......trajorient（fcnt））;％使用|预测|估计基于滤波器状态的方法模拟加速度计和陀螺仪信号上的％。预测（Fusionfilt，Accel，Gyro）;%获取过滤器状态的当前估计。[Fusedpos，Fusulorient] =姿势（Fusionfilt）;％保存后处理的位置和方向。pqorient（fcnt）= fusedorient;pqpos（fcnt，:) =融合;％计算错误和绘图。如果umererrscope orienterr = rad2deg（dist（fusedorient，......trajorient（fcnt）））;poserr = fusedpos - trajpos（fcnt，:);errscope（东方，poserr（1），poserr（2），poserr（3））;结束％更新姿势查看器。如果使用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错误。fprintf（'\ n \ n \ n末端模拟位置rms错误\ n'）；
端到端模拟位置RMS错误
MSEP = SQRT（平均值（POSD。^ 2））;fprintf（'\ tx：％.2f，y：％.2f，z：％.2f（米）\ n \ n'msep (1)......msep msep (2), (3));
X：0.57，Y：0.53，Z：0.68（米）
fprintf（'端到端四元数距离RMS误差(度)\n'）；
端到端四元数距离RMS误差(度)
fprintf（' \ t %。2 f(度)\ n \ n '，sqrt（平均值（quatd ^ 2））））;
0.32（度）
您还可以从以下列表中选择一个网站：
