估计地面车辆的位置和方向
这个例子展示了如何通过融合来自惯性测量单元(IMU)和全球定位系统(GPS)接收器的数据来估计地面车辆的位置和方向。
仿真设置
设定采样率。在一个典型的系统中，IMU中的加速度计和陀螺仪以较高的采样率运行。融合算法处理这些传感器数据的复杂度较低。相反，GPS运行在一个相对较低的采样率和与处理相关的复杂性是高的。在该融合算法中，GPS采样以较低的速率进行处理，加速度计和陀螺仪采样以相同的速率进行处理。
为了模拟这种结构中，IMU（加速度计和陀螺仪）在100Hz进行采样，并以10Hz的GPS进行采样。
imuFs = 100;gpsFs = 10;%定义在地球上的什么地方使用纬度进行模拟，％经度和海拔高度（LLA）坐标。localOrigin = [42.2825 -71.343 53.0352];%验证|gpsFs|可以分割|imuFs|。这允许传感器采样使用无复杂采样率的嵌套for循环模拟%率％匹配。imuSamplesPerGPS = (imuFs / gpsFs);断言(imuSamplesPerGPS = =修复(imuSamplesPerGPS),…GPS采样率必须是IMU采样率的整数因子。);
融合滤波器
创建过滤器，以融合IMU + GPS测量。融合滤波器使用扩展的卡尔曼滤波器来跟踪方向(作为四元数)、位置、速度和传感器偏差。
的insfilterNonholonomic对象主要有两个方法：预测和fusegps。的预测方法以IMU中的加速度计和陀螺仪采样为输入。调用预测方法每次采样加速度计和陀螺仪。该方法基于加速度计和陀螺仪对状态进行单时步预测。这一步更新了扩展卡尔曼滤波器的误差协方差。
的fusegps方法采用GPS采样作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波状态，并根据各传感器输入的不确定性对其进行加权。一个误差协方差也更新在这一步，这一次使用卡尔曼增益以及。
的insfilterNonholonomic对象有两个主要性能：IMUSampleRate和DecimationFactor。地面车辆有两个速度限制，假设它不会从地面反弹或在地面上滑动。利用扩展的卡尔曼滤波更新方程来应用这些约束条件。这些更新应用于过滤器状态的速率为IMUSampleRate / DecimationFactor赫兹。
gndFusion = insfilterNonholonomic（“ReferenceFrame”,“ENU表示”,…'IMUSampleRate'，imuFs，…“ReferenceLocation”localOrigin,…“DecimationFactor”2);
创建地面车辆轨迹
的waypointTrajectory对象姿态计算基于规定的采样率，航点，到达时间，和取向。指定地面车辆的圆形轨迹的参数。
%轨迹参数r = 8.42;％（M）速度= 2.50;％ （多发性硬化症）center = [0,0];％（M）initialYaw = 90;%(度)numRevs = 2;％定义角度theta和到达吨对应倍。revTime = 2 * PI * R /速度;THETA =（0：PI / 2：2 * PI * numRevs）';。T = linspace（0，revTime * numRevs，numel（THETA））'。%定义的位置。X = R * cos（THETA）+中心（1）;Y = R * SIN（THETA）+中心（2）。Z =零（大小（X））;位置= [X，Y，Z];％定义方向。偏航= THETA + deg2rad（initialYaw）;偏航= MOD（偏航，2 * PI）;节距=零（大小（偏航））;辊=零（大小（偏航））;取向=四元数（[偏航，俯仰，滚动]“欧拉”,…“ZYX股票”,'帧');％生成轨迹。地面实况= waypointTrajectory（“SampleRate”，imuFs，…“锚点”，位置，…“TimeOfArrival”t…“定位”、方向);初始化用于模拟传感器噪声的随机数生成器。rng (“默认”);
GPS接收器
按指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
全球定位系统(gps) = gpsSensor (“UpdateRate”gpsFs,“ReferenceFrame”,“ENU表示”);gps。ReferenceLocation = localOrigin;gps。DecayFactor = 0.5;%随机行走噪声参数gps。HorizontalPositionAccuracy = 1.0;gps。VerticalPositionAccuracy = 1.0;gps。VelocityAccuracy = 0.1;
IMU传感器
通常，地面车辆使用姿态估计6轴IMU传感器。为了模拟的IMU传感器，定义包含加速度计和陀螺仪的IMU传感器模型。在实际的应用中，两个传感器可能来自单一的集成电路或单独的。这里设置的属性值是典型的低成本的MEMS传感器。
imu = imuSensor (“accel-gyro”,…“ReferenceFrame”,“ENU表示”,“SampleRate”，imuFs）;加速度计％imu.Accelerometer。MeasurementRange = 19.6133;imu.Accelerometer。分辨率= 0.0023928;imu.Accelerometer。NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope。MeasurementRange =函数(250);imu.Gyroscope。解析函数= (0.0625);imu.Gyroscope。NoiseDensity函数= (0.025);
类的状态初始化insfilterNonholonomic
美国是:
状态单位指数方向(四元数部分)1:4陀螺仪偏差(XYZ) rad/s 5:7位置(NED) m 8:10速度(NED) m/s 11:13加速度计偏差(XYZ) m/s^2 14:16
Ground truth用于帮助初始化筛选器状态，因此筛选器可以快速收敛到良好的结果。
%从弹道的第一个样本得到初始的地面真实姿态%，并释放地面真实轨迹，以确保第一个样本不是%在模拟期间跳过。[initialPos, initialAtt, initialVel] = groundTruth();重置(groundTruth);初始化筛选器的状态gndFusion.State(1:4) =紧凑(initialAtt)。”;gndFusion.State (7) = imu.Gyroscope.ConstantBias;gndFusion.State (8) = initialPos。”;gndFusion.State (11:13) = initialVel。”;gndFusion.State(十四16)= imu.Accelerometer.ConstantBias;
初始化的方差的insfilterNonholonomic
测量噪声描述多少噪音被破坏GPS读数基础上，gpsSensor参数和车辆动态模型中的不确定性。
该过程描述噪声滤波器方程式如何描述状态演化。过程噪声经验确定使用参数扫以从过滤器共同优化位置和定向估计。
%测量噪声Rvel = gps.VelocityAccuracy。^ 2;rpo = gps.HorizontalPositionAccuracy。^ 2;该滤波器的地面车辆动态模型假设存在％无侧滑或运动过程中打滑。这意味着速度是%仅限于身体前轴。另外两个速度轴%的读数用一个0的测量值进行校正，该值的权重为％| ZeroVelocityConstraintNoise |参数。gndFusion。ZeroVelocityConstraintNoise = 1飞行;%过程噪音gndFusion.GyroscopeNoise = 4E-6;gndFusion.GyroscopeBiasNoise = 4E-14;gndFusion.AccelerometerNoise = 4.8E-2;gndFusion.AccelerometerBiasNoise = 4E-14;%初始误差协方差gndFusion。StateCovariance = 1 e-9 * 1 (16);
初始化作用域
的HelperScrollingPlotter范围允许绘制随时间变化的变量。它用于跟踪姿态中的错误。的HelperPoseViewer范围允许滤波器估计和地面实况姿态的3-d可视化。该范围可以减缓模拟。要禁用范围，相应的逻辑变量设置为假。
useErrScope = TRUE;打开流错误图usePoseView = TRUE;％开启3D姿态观众如果errscope = HelperScrollingPlotter(…'NumInputs'4，…“时间间隔”10，…“SampleRate”，imuFs，…“YLabel”{“度”,…“米”,…“米”,…“米”}，…“标题”{“四元数距离”,…“X位置错误”,…“位置Y错误”,…“Z位置错误”}，…'YLimits',…[-1，1 - 1，1 - 1，1 1，1]）;结束如果usePoseView查看器= HelperPoseViewer(…“XPositionLimits”[-15，15]，…“YPositionLimits”[-15，15]，…'ZPositionLimits'[-5,5]，…“ReferenceFrame”,“ENU表示”);结束
仿真循环
主仿真循环是一个while循环，其中嵌套了一个for循环。执行while循环gpsFs，这是GPS测量速率。嵌套在循环执行imuFs，这是IMU采样率。该作用域在IMU采样率进行更新。
totalSimTime = 30;％秒％对于最终的度量计算日志数据。numsamples = floor(min(t(end)， totalSimTime) * gpsFs);truePosition = 0 (numsamples, 3);trueOrientation = quaternion.zeros (numsamples, 1);estPosition = 0 (numsamples, 3);estOrientation = quaternion.zeros (numsamples, 1);idx = 0;为sampleIdx = 1：NUMSAMPLES%预测在IMU更新频率的循环。为我= 1:imuSamplesPerGPS如果〜isDone（地面实况）IDX = IDX + 1;%从当前位姿模拟IMU数据。:[truePosition idx), trueOrientation (idx:)…trueVel, trueAcc, trueAngVel] = groundTruth();[accelData, gyroData] = imu(trueAcc, trueAngVel，…trueOrientation（IDX，:)）;％使用预测方法来估计基于过滤器状态%的accelData和陀螺仪数据阵列。预测（gndFusion，accelData，gyroData）;％登录所估计的取向和位置。[estPosition（IDX，:)，estOrientation（IDX，:)] =姿态（gndFusion）;计算误差并绘图。如果useErrScope orientErr = rad2deg（…dist (estOrientation (idx:), trueOrientation (idx,:)));posErr = estPosition(idx，:) - truePosition(idx，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束%更新姿势查看器。如果usePoseView观众（estPosition（IDX，:)，estOrientation（IDX，:)，…truePosition (idx:), estOrientation (idx:));结束结束结束如果~结束(groundTruth)%下一步发生在GPS采样率。基于当前的姿势％模拟输出的GPS。[LLA，gpsVel] = GPS（TruePosition公司（IDX，:)，trueVel）;％更新基于GPS数据的过滤器状态。fusegps（gndFusion，LLA，RPOS，gpsVel，Rvel）;结束结束
误差指标计算
整个模拟过程中的位置和方向进行了记录。现在，计算两者的位置和方向的端至端均方根误差。
POSD = estPosition - TruePosition公司;对于方向，四元数距离是一个更好的选择％中减去欧拉角，其具有不连续性。四元%距离可用|dist|函数计算，该函数给出％以弧度为单位在取向角度差。转换为度%显示在命令窗口中。quatd = rad2deg(dist(estOrientation, trueOrientation));％在命令窗口显示RMS误差。流(“\ n \ NEND至端模拟位置RMS错误。\ n”);MSEP = SQRT（均值（POSD ^ 2））;流(“\ tX: %。2f, Y: %。Z: % 2 f。2 f(米)\ n \ n”，MSEP（1），…msep msep (2), (3));流(“端至尾四元数距离RMS误差（度）\ N”);流('\吨％.2f（度）\ n \ n',sqrt(平均(quatd ^ 2)));
端到端仿真位置RMS误差X: 1.16, Y: 0.98, Z: 0.03(米)端到端四元数距离RMS误差(度)0.51(度)
