负载(“loggedDataWithMultipath.mat”，．..“imuFs”，'Accel'，'陀螺'，．..% IMU数据'GPSFS'，“lla”，“gpsVel”，．..％GPS读数“truePosition”，“trueOrientation”，．..地面真实姿态'localorigin'，“initialState”，“multipathAngles”）%每个GPS样本的IMU样本数。imusamplespergps =（imufs / gpsfs）;对应于多径错误的名字和最后一个索引。multipathindices = [1850 2020];

%创建过滤器。％使用此过滤器仅处理未检测到的传感器读数％异常值。gndfusionwithdetection = insfilternonholoromic（“ReferenceFrame”，'enu'，．..“IMUSampleRate”，imufs，．..“ReferenceLocation”localOrigin,．..“DecimationFactor”2);%使用此过滤器处理所有传感器读数，无论是否或不是，他们是离群值。GNDFusionNodetection = InsfilternOnHoloric（“ReferenceFrame”，'enu'，．..“IMUSampleRate”，imufs，．..“ReferenceLocation”localOrigin,．..“DecimationFactor”2);% GPS测量噪声。Rvel = 0.01;rpo = 1;这个过滤器的地面车辆的动态模型假设有运动时无侧滑或打滑。这意味着速度是%被限制为仅前身体轴。另外两个速度轴%读数用零测量进行校正，由％| ZerovelocityConstraintNoise |范围。gndFusionWithDetection。ZeroVelocityConstraintNoise = 1飞行;gndFusionNoDetection。ZeroVelocityConstraintNoise = 1飞行;%过程噪音。gndfusionwithdetection.gyroscopenoise = 4e-6;gndfusionwithdetection.gyroscopdbiasnoise = 4e-14;gndfusionwithdetection.acceleromernoise = 4.8e-2;gndfusionwithdetection.accelerometerbiasnoise = 4e-14;gndfusionnodetection.gyroscopenoise = 4e-6;gndfusionnodetection.gyroscopdbiasnoise = 4e-14;gndfusionnodetection.acceleromernoise = 4.8e-2;gndfusionnodetection.accelerometerbiasnoise = 4e-14;％初始滤波器状态。gndfusionwithdetection.state = initialstate;gndfusionnodetection.state = initialstate;％初始错误协方差。gndFusionWithDetection。StateCovariance = 1 e-9 * 1 (16);gndFusionNoDetection。StateCovariance = 1 e-9 * 1 (16);

usePoseView = true;％打开3D姿势查看器如果usePoseView [viewerWithDetection, viewerNoDetection, annoHandle]．..= HelperCreatePoseViewers（InitialState，MultiPathangles）;结束

numsamples = numel（tryerverientation）;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: numGPSSamplesIMU更新频率下％预测循环。为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）;%更新姿态查看器。如果UpployView ViewerWithDetection（estpositionNocheck（IDX，:)，．..estorientationNocheck（IDX，:)，．..trueposition（idx，:)，triace主管（idx，:)）;视网膜级（estposition（idx，:)，．..estOrientation (idx:), truePosition (idx:)．..Triace主顾（IDX，:)）;结束结束％该代码的下一部分以GPS采样率运行。％基于GPS数据更新过滤器状态。fusegps (gndFusionWithDetection lla (sampleIdx:), rpo,．..gpsVel (sampleIdx:), Rvel);％检查当前GPS读数的标准化残余。如果是％值太大，它被认为是一个异常值并被忽视。[res, resCov] = residualgps(gndFusionNoDetection, lla(sampleIdx，:)，．..RPOS，GPSVel（SampleIdx，:)，rvel）;Incormalizedres = 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  -  trueposition;posd = estthosition  -  trueentosition;％绘图结果。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）;标题（'位置错误（删除异常值）'）xlabel（'时间''）ylabel（'错误（m）')传说('X'，'是'，'Z'sprintf ('异常\ nregion'））子图（1,2,2）绘图（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）;标题（位置错误(离群值移除)）xlabel（'时间''）ylabel（'错误（m）')传说('X'，'是'，'Z'sprintf ('异常\ nregion'）））