IMU和GPS融合惯性导航
这个示例展示了如何构建适用于无人机(uav)或四轴飞行器的IMU + GPS融合算法。
本例使用加速度计、陀螺仪、磁力计和GPS来确定无人机的方向和位置。
模拟设置
设置采样率。在一个典型的系统中，加速度计和陀螺仪以相对较高的采样率运行。该融合算法处理这些传感器数据的复杂度相对较低。相反，GPS，以及在某些情况下的磁力计，以相对较低的采样率运行，并且与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。
为了模拟这个配置，IMU(加速度计、陀螺仪和磁力计)以160赫兹采样，GPS以1赫兹采样。每160个磁力计样本中，只有1个样本被给出给融合算法，因此在实际系统中，磁力计的采样率要低得多。
imufs = 160;GPSFS = 1;％定义地球上的哪个模拟场景使用％纬度，经度和高度。RESHOC = [42.2825 -72.3430 53.0352];％验证| GPSFS |分开| imufs |。这允许传感器样本使用嵌套循环模拟的％速率而无需复杂的采样率％匹配。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;
无人机轨迹
此示例使用从UAV记录的保存轨迹作为地面真相。此轨迹被送入多个传感器模拟器以计算模拟加速度计，陀螺仪，磁力计和GPS数据流。
％加载“地面真理”UAV轨迹。加载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.MagneticField = [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 = 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
Ground truth被用来帮助初始化过滤器的状态，所以过滤器会很快收敛到好的答案。
％初始化过滤器的状态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速度测量噪声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滤波器估计和地面真理姿势的可视化。范围可以减慢模拟。要禁用范围，请将相应的逻辑变量设置为错误的．
umererrscope = true;%打开流错误图movingsview = true;％打开3-D姿势查看器如果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]）;结尾
仿真环路
主模拟循环是一小时的循环，带有嵌套循环。循环执行虽然循环执行GPSFS.，这是GPS采样率。嵌套循环执行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））;％使用|预测|估计基于滤波器状态的方法模拟加速度计和陀螺仪信号上的％。预测（Fusionfilt，Accel，Gyro）;％获取当前对滤波器状态的估计。[fusedPos, fusedOrient] = pose(融合);%保存位置和方向，以便后期处理。pqorient (fcnt) = fusedOrient;pqpos (fcnt:) = fusedPos;%计算误差并绘图。如果umererrscope orienterr = rad2deg（dist（fusedorient，．..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 \ 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（平均值（quatd ^ 2））））;
0.32（度）
您还可以从以下列表中选择一个网站：
