加载（'loggeddatawithmulipath.mat'那......'imufs'那'Accel'那'陀螺'那......％imu读数'GPSFS'那'lla'那'gpsvel'那......％GPS读数'trueposition'那'triace主管'那......％地面真理姿势'localorigin'那'InitialState'那'multipathangles'）每个GPS样本的IMU样本数量。imusamplespergps =（imufs / gpsfs）;对应于多径错误的名字和最后一个索引。multipathindices = [1850 2020];

％创建过滤器。％使用此过滤器仅处理未检测到的传感器读数％异常值。gndfusionwithdetection = insfilternonholoromic（'参考范围'那'enu'那......'imusamplerate'，imufs，......'参考'，局部素，......'decimationfactor'，2）;％使用此过滤器来处理所有传感器读数，无论是或％不是他们是异常值。GNDFusionNodetection = InsfilternOnHoloric（'参考范围'那'enu'那......'imusamplerate'，imufs，......'参考'，局部素，......'decimationfactor'，2）;％GPS测量噪声。rvel = 0.01;RPOS = 1;该滤波器的地面车辆的动态模型假定存在百分比在运动期间没有侧滑或滑动。这意味着速度是％约束仅对前体轴。另外两个速度轴用零测量校正％读数被加权％| ZerovelocityConstraintNoise |范围。gndfusionwithdetection.zerovelocityConstraintNoise = 1E-2;gndfusionnodetection.zerovelocityConstraintNoise = 1E-2;％流程噪音。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 = 1e-9 *那些（16）;gndfusionnodetection.statecovariance = 1e-9 *那些（16）;

movingsview = true;％打开3D姿势查看器如果MOVERMOSEVIEW [查看器WithDetection，Viewernodetection，AnnoHandle]......= HelperCreatePoseViewers（InitialState，MultiPathangles）;结尾

numsamples = numel（tryerverientation）;numgpssamples = numsamples / imusamplespergps;最终度量计算的％日志数据。estpositionNocheck =零（Numsamples，3）;estorientationNocheck =四星。会（Numsamples，1）;estposition = zeros（numsamples，3）;estorientation = Quaternion.zeros（Numsamples，1）;异常值残差的％阈值。ResealThreshold = 6;Idx = 0;为了SampleIdx = 1：NumGPSSamplesIMU更新频率下％预测循环。为了i = 1：imusamplespergps idx = idx + 1;％使用预测方法来估计基于滤波器状态的方法AcceldAta和Gyrodata阵列的百分比。预测（GNDFusionWithDetection，Accel（IDX，:)，Gyro（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（INSORMIZEDRES）<= REDILUNTHESHOLD））％基于GPS数据更新过滤器状态。Fusegps（GNDFusionNodetection，LLA（SampleIdX，:)，RPO，......gpsvel（sampleidx，:)，rvel）;如果UppoimopeView集（AnnoHandle，'细绳'那'异常值状态：无'那......'Edgecolor'那'K'）;结尾别的如果UppoimopeView集（AnnoHandle，'细绳'那'异常值状态：检测到'那......'Edgecolor'那'r'）;结尾结尾结尾

％计算位置错误。posdnocheck = estpositionnocheck  -  trueposition;posd = estthosition  -  trueentosition;％绘图结果。t =（0：大小（POSD，1）-1）。'./ imufs;数字（'单位'那'标准化'那'位置'，[0.2615 0.2833 0.4552 0.3700]）子图（1,2,1）图（t，posdnocheck）ax = gca;ylims = get（ax，'ylim'）;抓住上mi =多田径;填充（[t（mi（1）），t（mi（1）），t（mi（2）），t（mi（2））]，[7 -5 -5 7]，......[1 0 0]，'Facealpha'，0.2）;套（斧头，'ylim'，ylims）;标题（'位置错误（删除异常值）'）xlabel（'时间''）ylabel（'错误（m）'） 传奇（'X'那'是'那'z'，Sprintf（'异常\ nregion'））子图（1,2,2）绘图（t，posd）ax = gca;ylims = get（ax，'ylim'）;抓住上mi =多田径;填充（[t（mi（1）），t（mi（1）），t（mi（2）），t（mi（2））]，[7 -5 -5 7]，......[1 0 0]，'Facealpha'，0.2）;套（斧头，'ylim'，ylims）;标题（'位置错误（异常删除）'）xlabel（'时间''）ylabel（'错误（m）'） 传奇（'X'那'是'那'z'，Sprintf（'异常\ nregion'）））