估计位置和地面车辆的方向
此示例示出了如何通过从惯性测量单元（IMU）和全球定位系统（GPS）接收器融合数据来估计地面车辆的位置和方向。
仿真设置
设置采样率。在一个典型的系统中，IMU加速度计和陀螺仪在相对较高的采样率运行。中相应的融合算法来处理从这些传感器数据的复杂度相对较低。相反，在相对低的采样率的GPS运行和复杂性与处理它是高关联。在这种融合算法的GPS样品以低速率处理，并且加速度计和陀螺仪的样品以相同的高速率一起处理。
为了模拟这种配置，IMU(加速度计和陀螺仪)的采样频率为100hz, GPS的采样频率为10hz。
imuFs = 100;gpsFs = 10;％定义在地球上使用纬度这个模拟发生，经度和高度(LLA)坐标。localOrigin = [42.2825 53.0352 -71.343]。％确认该| 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 ('参考范围'，'ENU'，...“IMUSampleRate”imuFs,...'ReferenceLocation'，localOrigin，...'DecimationFactor'，2）;
创建地面车辆轨迹
该waypointTrajectory对象根据指定的采样率、路径点、到达时间和方向来计算姿态。指定地面车辆的圆形轨迹参数。
％弹道参数R = 8.42;% (m)速度= 2.50;%(米/秒)中心= [0,0]。% (m)initialYaw = 90;％（度）numRevs = 2;定义角度和相应的到达时间t。revTime = 2*pi*r / speed;θ=(0:π/ 2:2 *π* numRevs)。”;t = linspace(0, revTime*numRevs, numel(theta));％定义位置。x = r .* cos() +中心(1);y = r。* sin() + center(2);z = 0(大小(x));位置= [x, y, z];%定义取向。yaw = theta + deg2rad(首字母缩写);yaw = mod(yaw, 2*pi);距= 0(大小(偏航));滚= 0(大小(偏航));方向=四元数([偏航，俯仰，横摇])，“欧拉”，...'ZYX'，“帧”);%生成轨迹。groundTruth = waypointTrajectory ('采样率'imuFs,...“路标”、位置、...'到达时间'，T，...'取向'，取向）;％初始化用于模拟传感器噪声的随机数发生器。RNG（'默认');
GPS接收器
设置在指定的采样率和参考位置的GPS。其它参数控制在输出信号中的噪声的性质。
GPS = gpsSensor（'UpdateRate'，gpsFs，'参考范围'，'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（“加速 - 陀螺仪”，...'参考范围'，'ENU'，'采样率',imuFs);%加速度计imu.Accelerometer.MeasurementRange = 19.6133;imu.Accelerometer.Resolution = 0.0023928;imu.Accelerometer.NoiseDensity = 0.0012356;陀螺％imu.Gyroscope.MeasurementRange = deg2rad（250）;imu.Gyroscope.Resolution = deg2rad（0.0625）;imu.Gyroscope.NoiseDensity = deg2rad（0.025）;
初始化的国家insfilterNonholonomic
的状态是：
国单位指标取向（四元数份）1：4个陀螺仪偏差（XYZ）弧度/秒5：7的位置（NED）米8:10速（NED）米/秒11:13加速度计偏差（XYZ）米/秒^ 214:16
地面真相是用来帮助初始化过滤器的状态，所以过滤器收敛到很好的答案很快。
％来自于轨迹的第一个样品获得初始地面实况姿势％和释放真实地面轨迹，以确保第一样品不模拟过程中跳过％。[initialPos，initialAtt，initialVel] =地面实况（）;复位（地面实况）;％初始化过滤器的状态gndFusion.State（1：4）=紧凑（initialAtt）';。gndFusion.State（5：7）= imu.Gyroscope.ConstantBias;gndFusion.State（8:10）= initialPos'。gndFusion.State（11:13）= initialVel'。gndFusion.State（十四点16）= imu.Accelerometer.ConstantBias;
的方差初始化insfilterNonholonomic
测量噪声描述了噪声对GPS读数的影响程度gpsSensor参数，有多少不确定性是在车辆动态模型。
过程噪声描述了滤波方程对状态演化的描述。利用参数扫频联合优化滤波器的位置估计和方向估计，对过程噪声进行了经验确定。
％测量噪声Rvel = gps.VelocityAccuracy ^ 2。RPOS = gps.HorizontalPositionAccuracy ^ 2。％的地面车辆的该滤波器的动态模型假定有运动时无侧滑或打滑现象。这意味着速度是％约束为仅前向体轴。其它两个轴的速度％读数与由所述加权的零测量校正% | ZeroVelocityConstraintNoise |参数。gndFusion.ZeroVelocityConstraintNoise = 1E-2;％加工噪声gndFusion。GyroscopeNoise = 4 e-6;gndFusion。GyroscopeBiasNoise = 4 e-14;gndFusion。AccelerometerNoise = 4.8依照;gndFusion。AccelerometerBiasNoise = 4 e-14;％初始误差协方差gndFusion.StateCovariance = 1E-9 *酮（16）;
初始化范围
该HelperScrollingPlotter范围能够随着时间的变量绘制。此处用它来跟踪姿势错误。该HelperPoseViewer范围允许三维可视化的过滤器估计和地面真相的姿态。该范围可以减慢模拟。若要禁用范围，请将相应的逻辑变量设置为假。
useErrScope = TRUE;％接通流误差曲线usePoseView = true;打开3D姿态查看器如果useErrScope errscope = HelperScrollingPlotter（...“NumInputs”4...'时间跨度'10...'采样率'imuFs,...'YLabel',{“度”，...“米”，...“米”，...“米”},...'标题',{“四元数距离”，...“位置X错误”，...“Y位置错误”，...“位置Z错误”},...“YLimits”，...[-1, 1 -1, 1 -1, 1 -1, 1 -1]);结束如果usePoseView观察者= HelperPoseViewer（...'XPositionLimits'(-15年,15),...'YPositionLimits'(-15年,15),...'ZPositionLimits',5,5],...'参考范围'，'ENU');结束
模拟循环
主要的仿真循环是一个嵌套的for循环while循环。在while循环执行gpsFs，即GPS测量速率。嵌套的for循环在imuFs，这是IMU采样率。该作用域在IMU采样率进行更新。
totalSimTime = 30;%秒用于最终度量计算的日志数据。NUMSAMPLES =地板（分钟（吨（结束），totalSimTime）* gpsFs）;TruePosition公司=零（NUMSAMPLES，3）;trueOrientation = quaternion.zeros（NUMSAMPLES，1）;estPosition =零（NUMSAMPLES，3）;estOrientation = quaternion.zeros（NUMSAMPLES，1）;IDX = 0;对于sampleIdx = 1: numsamples％在IMU更新频率预测环路。对于I = 1：imuSamplesPerGPS如果~isDone(groundTruth) idx = idx + 1;％模拟从当前姿势的IMU数据。[TruePosition公司（IDX，:)，trueOrientation（IDX，:)，...trueVel，trueAcc，trueAngVel] =地面实况（）;[accelData，gyroData] = IMU（trueAcc，trueAngVel，...trueOrientation (idx:));使用基于预测的方法估计滤波状态％的accelData和gyroData阵列。预测(gndFusion accelData gyroData);记录估计的方向和位置。[estPosition(idx，:)， estOrientation(idx，:)] = pose(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，:)）;结束结束结束如果〜isDone（地面实况）％该下一步骤发生在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错误。fprintf中（“\ n \ NEND至端模拟位置RMS错误。\ n”);msep =√意味着(posd ^ 2));fprintf中（'\ TX：％.2f，Y：％.2f，Z：％.2f（米）\ n \ n'，MSEP（1），...MSEP（2），MSEP（3））;fprintf中（“端到端四元数距离RMS误差(度)”);fprintf中（' \ t %。2 f(度)\ n \ n '，SQRT（均值（quatd ^ 2）。））;
终端到终端的仿真位置RMS错误X：1.16，Y：0.98，Z：0.03（米）的端至端四元数距离RMS误差（度）0.51（度）
你也可以从以下列表中选择一个网站:
