估计地面车辆的位置和方向
这个例子展示了如何通过融合来自惯性测量单元(IMU)和全球定位系统(GPS)接收器的数据来估计地面车辆的位置和方向。
模拟装置
设置采样率。在一个典型的系统中，IMU中的加速度计和陀螺仪以相对较高的采样率运行。该融合算法处理这些传感器数据的复杂度相对较低。相反，GPS运行在一个相对较低的采样率和与处理它相关的复杂性是高的。该融合算法对GPS样本进行低速率处理，同时对加速度计和陀螺仪样本进行相同的高速率处理。
为了模拟这种配置，IMU（加速计和陀螺仪）以100 Hz的频率采样，GPS以10 Hz的频率采样。
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对象有两个主要属性：不采样和DecimationFactor．地面车辆有两个速度限制，假设它不反弹地面或在地面上滑动。利用扩展卡尔曼滤波更新方程应用这些约束。这些更新以以下速率应用于过滤器状态IMUSampleRate / DecimationFactor赫兹。
gndFusion=insfilternonomonic(“ReferenceFrame”，“ENU表示”，.．.“IMUSampleRate”，imuFs，.．.“ReferenceLocation”localOrigin,.．.“DecimationFactor”2);
创建地面车辆轨迹
的航路点轨迹对象根据指定的采样率、航路点、到达时间和方向计算姿势。指定地面车辆的圆形轨迹参数。
%轨迹参数r = 8.42;%（m）速度= 2.50;%（米/秒）中心= [0,0];%（m）初始偏航=90；%(度)numRevs = 2;%定义角度θ和相应的到达时间t。转速=2*pi*r/速度；θ=（0:pi/2:2*pi*numRevs）。；t=linspace（0，转速*numrevevs，nummel（θ））；%定义的位置。x=r.*cos（θ）+中心（1）；y=r.*sin（θ）+中心（2）；z=0（大小（x））；位置=[x，y，z]；%定义方向。偏航=θ+deg2rad（初始偏航）；偏航=mod（偏航，2*pi）；俯仰=零（尺寸（偏航））；滚动=零（尺寸（偏航））；方向=四元数（[偏航、俯仰、横摇]，“欧拉”，.．.“ZYX股票”，“框架”）;%生成轨迹。地面真相=航路点轨迹(“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.Resolution = 0.0023928;imu.Accelerometer.NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope.MeasurementRange =函数(250);imu.Gyroscope.Resolution函数= (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的GPS读数gpsSensor车辆动力学模型的参数和不确定性。
过程噪声描述了滤波器方程对状态演化的描述程度。过程噪声通过参数扫描以经验方式确定，从而联合优化滤波器的位置和方向估计。
%测量噪声Rvel = gps.VelocityAccuracy。^ 2;rpo = gps.HorizontalPositionAccuracy。^ 2;%这个过滤器的地面车辆的动力学模型假设有%移动过程中无侧滑或打滑。这意味着速度%限定为身体的前向轴。另外两个速度轴%读数用加权的零测量进行校正%| ZeroVelocityConstraintNoise |参数。gndFusion。ZeroVelocityConstraintNoise = 1飞行;%过程噪音gndFusion.陀螺仪噪声=4e-6；gndFusion.陀螺仪偏差=4e-14；gndFusion.AccelerometerNoise=4.8e-2；gndFusion.AccelerometerBiasNoise=4e-14；%初始误差协方差gndFusion。StateCovariance = 1 e-9 * 1 (16);
初始化作用域
的助手CrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势错误。的HelperPoseViewer作用域允许对滤波器估计和地面真实姿态进行三维可视化。作用域会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为错误的．
useErrScope = true;%打开流错误图usePoseView=true；%启用三维姿势查看器如果usererrscope errscope = HelperScrollingPlotter(.．.“努明普茨”4..．.“时间间隔”, 10,.．.“SampleRate”，imuFs，.．.“YLabel”, {“度”，.．.“米”，.．.“米”，.．.“米”},.．.“标题”, {“四元数距离”，.．.“X位置错误”，.．.“位置Y错误”，.．.“Z位置错误”},.．.“YLimits”，.．.[-1, 1 -1, 1 -1, 1 -1, 1]);终止如果usePoseView viewer = HelperPoseViewer(.．.“XPositionLimits”, [-15, 15],.．.“YPositionLimits”, [-15, 15],.．.“ZPositionLimits”, [-5, 5],.．.“ReferenceFrame”，“ENU表示”）;终止
模拟回路
主模拟循环是一个带有嵌套的for循环的while循环。的处执行while循环gpsFs，这是GPS测量速率。嵌套for循环在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:).．.true, true, true] = groundTruth();[accelData, gyroData] = imu(trueAcc, trueAngVel, true).．.真方位（idx，：）；%使用预测方法来估计基于%在accelData和gyroData数组上。预测（gndFusion、accelData、gyroData）；%记录估计的方向和位置。[estPosition（idx，：），estOrientation（idx，：）]=姿势（gndFusion）；%计算误差并绘图。如果UseerScope定向器=rad2deg(.．.dist (estOrientation (idx:), trueOrientation (idx,:)));posErr = estPosition(idx，:) - true (idx，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));终止%更新姿势查看器。如果usePoseView viewer (estPosition (idx:), estOrientation (idx:).．.truePosition (idx:), estOrientation (idx:));终止终止终止如果~结束(groundTruth)%接下来的步骤发生在GPS采样率。%基于当前姿势模拟GPS输出。[lla，gpsVel]=gps（真位置（idx，：），真水平）；%根据GPS数据更新过滤器状态。fusegps（gndFusion、lla、RPO、gpsVel、Rvel）；终止终止
误差指标计算
在整个模拟过程中记录位置和方向。现在计算位置和方向的端到端均方根误差。
posd=estPosition-truePosition；对于方向，四元数距离是一个更好的选择%减去不连续的欧拉角。四元数%距离可以用|dist|函数计算，该函数给出%以弧度为单位的方向角差。转换为度表示%显示在命令窗口中。quatd = rad2deg(dist(estOrientation, trueOrientation));%在命令窗口中显示RMS错误。流('\n\ n端到端仿真位置均方根误差\n')；msep=sqrt（平均值（位置^2））；fprintf(“\ tX: %。2f, Y: %。Z: % 2 f。2 f(米)\ n \ n”msep (1).．.msep msep (2), (3));流('端到端四元数距离RMS错误（度）\n'); fprintf('\t%.2f（度）\n\n', sqrt(平均(quatd ^ 2)));
端到端仿真位置RMS误差X: 1.16, Y: 0.99, Z: 0.03(米)端到端四元数距离RMS误差(度)0.09(度)
您还可以从以下列表中选择网站：
