加载（'loggeddatawithivath.mat'那......'imufs'那“加速”那“陀螺”那......％IMU读数“gpsFs”那'lla'那'gpsvel'那......%的GPS数据'trueposition'那'triace主管'那......％地面真理姿势“localOrigin”那'InitialState'那'multipathangles'的）每GPS样品的IMU样本的数量。imuSamplesPerGPS = (imuFs / gpsFs);%第一和最后索引对应多路径错误。multipathIndices = [1850 2020];

％创建过滤器。%使用此过滤器只处理未检测到的传感器读数%离群值。gndFusionWithDetection = insfilterNonholonomic ('参考范围'那“ENU表示”那......'imusamplever'imuFs,......'referencelocation'，局部素，......'decimationfactor'，2）;％使用此过滤器来处理所有传感器读数，无论是或％不是他们是异常值。gndFusionNoDetection = insfilterNonholonomic ('参考范围'那“ENU表示”那......'imusamplever'imuFs,......'referencelocation'，局部素，......'decimationfactor'，2）;％GPS测量噪声。rvel = 0.01;RPOS = 1;该过滤器的地面车辆的动态模型假定存在运动期间百分比侧滑或滑动。这意味着速度是％约束到正向体轴。另外两个速度轴用零测量校正％读数被校正% | ZeroVelocityConstraintNoise |参数。gndfusionwithdetection.zerovelocityConstraintNoise = 1E-2;gndfusionnodetection.zerovelocityConstraintNoise = 1E-2;％流程噪音。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 = 1e-9 *那些（16）;gndfusionnodetection.statecovariance = 1e-9 *那些（16）;

modingview = true;打开3D姿势查看器如果UpplyView [ViewerWithDetection，Viewernodetection，AnnoHandle]......= helperCreatePoseViewers (initialState multipathAngles);结尾

numsamples =元素个数(trueOrientation);numGPSSamples = numsamples / imuSamplesPerGPS;%记录最终度量计算的数据。estpositionnocheck =零（numsamples，3）;estorientationnocheck = Quaternion.zeros（NumSamples，1）;estposition = zeros（numsamples，3）;estorientation = Quaternion.zeros（Numsamples，1）;异常值残差的％阈值。ResealThreshold = 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（ABS（INSORMIZIZERS）<= REDILUNTHESHOLD））%根据GPS数据更新过滤器状态。Fusegps（GNDFusionNodetection，LLA（SampleIdx，:)，RPO，......gpsvel（sampleidx，:)，rvel）;如果ModingView Set（AnnoHandle，'细绳'那异常状态:没有的那......'Edgecolor'那“k”）;结尾其他的如果ModingView Set（AnnoHandle，'细绳'那异常状态:检测到的那......'Edgecolor'那'r'）;结尾结尾结尾

％计算位置错误。posdNoCheck = estPositionNoCheck - true;posd = estPosition - true;%的阴谋的结果。t =（0：大小（POSD，1）-1）。'./ imufs;数字（“单位”那'标准化'那“位置”，[0.2615 0.2833 0.4552 0.3700]）子图（1,2,1）图（T，POSDNOCHECK）AX = GCA;ylims = get（斧头，'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);标题(“位置误差(没有异常值移除)”)包含(“时间(s)”) ylabel (“错误(m)”） 传奇（“x”那“y”那“z”，Sprintf（“离群值\ nregion”)) subplot(1,2,2) plot(t, posd) ax = gca;ylims = get（斧头，'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);标题('位置错误（删除异常）')包含(“时间(s)”) ylabel (“错误(m)”） 传奇（“x”那“y”那“z”，Sprintf（“离群值\ nregion”）)