负载(“loggedDataWithMultipath.mat”，．..“imuFs”，“加速”，“陀螺”，．..% IMU数据“gpsFs”，“lla”，“gpsVel”，．..%的GPS数据“truePosition”，“trueOrientation”，．..地面真位“localOrigin”，“initialState”，“multipathAngles”）%每个GPS样本的IMU样本数量。imuSamplesPerGPS = (imuFs / gpsFs);%第一和最后索引对应多路径错误。multipathIndices = [1850 2020];

%创建过滤器。%使用此过滤器只处理未检测到的传感器读数%离群值。gndFusionWithDetection = insfilterNonholonomic (“ReferenceFrame”，“ENU表示”，．..“IMUSampleRate”imuFs,．..“ReferenceLocation”localOrigin,．..“DecimationFactor”2);%使用此过滤器处理所有传感器读数，无论是否他们不是局外人。gndFusionNoDetection = insfilterNonholonomic (“ReferenceFrame”，“ENU表示”，．..“IMUSampleRate”imuFs,．..“ReferenceLocation”localOrigin,．..“DecimationFactor”2);% GPS测量噪声。Rvel = 0.01;rpo = 1;%这个过滤器的地面车辆的动力学模型假设有运动过程中无侧滑或打滑。这意味着速度是%限定为身体的前向轴。另外两个速度轴%读数用加权的零测量进行校正% | ZeroVelocityConstraintNoise |参数。gndFusionWithDetection。ZeroVelocityConstraintNoise = 1飞行;gndFusionNoDetection。ZeroVelocityConstraintNoise = 1飞行;%过程噪音。gndFusionWithDetection。GyroscopeNoise = 4 e-6;gndFusionWithDetection。GyroscopeBiasNoise = 4 e-14;gndFusionWithDetection。AccelerometerNoise = 4.8依照;gndFusionWithDetection。AccelerometerBiasNoise = 4 e-14;gndFusionNoDetection。GyroscopeNoise = 4 e-6;gndFusionNoDetection。GyroscopeBiasNoise = 4e-14; gndFusionNoDetection.AccelerometerNoise = 4.8e-2; gndFusionNoDetection.AccelerometerBiasNoise = 4e-14;%初始过滤状态。gndFusionWithDetection。状态= initialState;gndFusionNoDetection。状态= initialState;%初始误差协方差。gndFusionWithDetection。StateCovariance = 1 e-9 * 1 (16);gndFusionNoDetection。StateCovariance = 1 e-9 * 1 (16);

usePoseView = true;打开3D姿势查看器如果使用视图[viewerWithDetection, viewerNoDetection, nohandle]．..= helperCreatePoseViewers (initialState multipathAngles);结束

numsamples =元素个数(trueOrientation);numGPSSamples = numsamples / imuSamplesPerGPS;%记录最终度量计算的数据。estPositionNoCheck = 0 (numsamples, 3);estOrientationNoCheck =四元数。0 (numsamples, 1);estPosition = 0 (numsamples, 3);estOrientation =四元数。0 (numsamples, 1);%离群残差阈值。residualThreshold = 6;idx = 0;为sampleIdx = 1: numGPSSamples%在IMU更新频率预测循环。为i = 1:imuSamplesPerGPS idx = idx + 1;%使用预测方法估计基于滤波器的状态%在accelData和gyroData数组上。预测(gndFusionWithDetection accel (idx:),陀螺(idx:));预测(gndFusionNoDetection accel (idx:),陀螺(idx:));%记录估计的方向和位置。: [estPositionNoCheck idx), estOrientationNoCheck (idx:)]．..=姿势(gndFusionWithDetection);: [estPosition idx), estOrientation (idx:)]．..=姿势(gndFusionNoDetection);%更新姿势查看器。如果usePoseView viewerWithDetection (estPositionNoCheck (idx:)．..estOrientationNoCheck (idx:)．..truePosition (idx:), trueOrientation (idx:));viewerNoDetection (estPosition (idx:)．..estOrientation (idx:), truePosition (idx:)．..trueOrientation (idx:));结束结束%下一段代码以GPS采样率运行。%根据GPS数据更新过滤器状态。fusegps (gndFusionWithDetection lla (sampleIdx:), rpo,．..gpsVel (sampleIdx:), Rvel);%检查当前GPS读数的归一化残差。如果%值太大，会被认为是离群值而忽略。[res, resCov] = residualgps(gndFusionNoDetection, lla(sampleIdx，:)，．..rpo, gpsVel (sampleIdx:), Rvel);normalizedRes = res(1:3) ./ sqrt(diag(resCov(1:3,1:3))。”);如果(所有(abs (normalizedRes) < = residualThreshold))%根据GPS数据更新过滤器状态。fusegps (gndFusionNoDetection lla (sampleIdx:), rpo,．..gpsVel (sampleIdx:), Rvel);如果usePoseView集(annoHandle,“字符串”，异常状态:没有的，．..“EdgeColor”，“k”）;结束其他的如果usePoseView集(annoHandle,“字符串”，异常状态:检测到的，．..“EdgeColor”，“r”）;结束结束结束

%计算位置误差。posdNoCheck = estPositionNoCheck - true;posd = estPosition - true;%的阴谋的结果。t =(0:大小(posd, 1) 1)。”。/ imuFs;图(“单位”，“归一化”，“位置”， [0.2615 0.2833 0.4552 0.3700]) subplot(1,2,1) plot(t, posdNoCheck) ax = gca;yLims =得到(ax,“YLim”）;持有在mi = multipathIndices;填充([t (mi(1))、t (mi(1))、t (mi(2))、t (mi (2))], [7 5 5 7),．..(1 0 0),“FaceAlpha”,0.2);集(ax,“YLim”, yLims);标题(“位置误差(没有异常值移除)”)包含(“时间(s)”) ylabel (“错误(m)”)传说(“x”，“y”，“z”sprintf (“离群值\ nregion”)) subplot(1,2,2) plot(t, posd) ax = gca;yLims =得到(ax,“YLim”）;持有在mi = multipathIndices;填充([t (mi(1))、t (mi(1))、t (mi(2))、t (mi (2))], [7 5 5 7),．..(1 0 0),“FaceAlpha”,0.2);集(ax,“YLim”, yLims);标题(“位置误差(离群值移除)”)包含(“时间(s)”) ylabel (“错误(m)”)传说(“x”，“y”，“z”sprintf (“离群值\ nregion”））