惯性传感器融合中残差滤波的多径GPS读取误差检测

打开脚本

方法的使用residualgps对象函数和残差滤波用于检测新的传感器测量值何时可能与当前滤波器状态不一致。

负载轨迹和传感器数据

加载mat文件loggedDataWithMultipath.mat．该文件包含模拟IMU和GPS数据以及圆形轨迹的地面真实位置和方向。GPS数据由于轨迹某一段的多路径误差而包含误差。通过在GPS数据中添加白噪声来模拟这些误差，以模拟城市峡谷的影响。

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

融合滤波器

方法创建两个姿态估计过滤器insfilterNonholonomic对象。使用一个过滤器处理所有传感器读数。使用另一个过滤器只处理不被认为是异常值的传感器读数。

创建过滤器。使用此过滤器仅处理未检测到的传感器读数%离群值。gndFusionWithDetection = insfilterNonholonomic(“ReferenceFrame”，“ENU表示”，.．.“IMUSampleRate”imuFs,.．.“ReferenceLocation”localOrigin,.．.“DecimationFactor”2);使用此过滤器处理所有传感器读数，无论是否不，他们是异类。gndFusionNoDetection = insfilterNonholonomic(“ReferenceFrame”，“ENU表示”，.．.“IMUSampleRate”imuFs,.．.“ReferenceLocation”localOrigin,.．.“DecimationFactor”2);% GPS测量噪声。狂欢= 0.01;Rpos = 1;该滤波器的地面车辆动态模型假设存在。运动时无侧滑或打滑。。这意味着速度是%仅约束到身体的前轴。另外两个速度轴%的读数用加权的零测量来校正% |ZeroVelocityConstraintNoise|参数。gndFusionWithDetection。ZeroVelocityConstraintNoise = 1e-2;gndFusionNoDetection。ZeroVelocityConstraintNoise = 1e-2;%进程噪音。gndFusionWithDetection。陀螺仪= 4e-6;gndFusionWithDetection。GyroscopeBiasNoise = 4e-14;gndFusionWithDetection。加速度计噪声= 4.8e-2;gndFusionWithDetection。加速度计biasnoise = 4e-14;gndFusionNoDetection。陀螺仪= 4e-6;gndFusionNoDetection。GyroscopeBiasNoise = 4e-14; gndFusionNoDetection.AccelerometerNoise = 4.8e-2; gndFusionNoDetection.AccelerometerBiasNoise = 4e-14;初始过滤器状态。gndFusionWithDetection。State = initialState;gndFusionNoDetection。State = initialState;初始误差协方差。gndFusionWithDetection。StateCovariance = 1e-9*ones(16);gndFusionNoDetection。StateCovariance = 1e-9*ones(16);

初始化范围

的HelperPoseViewerscope允许三维可视化比较滤波器估计和地面真实值。使用多个作用域会降低模拟速度。若要禁用作用域，请将相应的逻辑变量设置为假．

usePoseView = true;打开3D姿势查看器如果usePoseView [viewerWithDetection, viewerNoDetection, annoHandle].．.= helpercreateposeviews (initialState, multipathAngles);结束

模拟循环

主要的模拟循环是为带有嵌套的循环为循环。第一个循环执行于gpsFs，为GPS测量速率。的位置执行嵌套循环imuFs，为IMU采样率。每个作用域都按照IMU采样率更新。

numsamples = numel(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 id = idx + 1;使用预测方法来估计滤波器的状态在accelData和gyroData数组上。predict(gndFusionWithDetection, accel(idx，:)， gyro(idx，:)));predict(gndFusionNoDetection, accel(idx，:)， gyro(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，:)， Rpos，.．.gpsVel (sampleIdx:), Rvel);检查当前GPS读数的归一化残差。如果%值太大，它被认为是一个异常值而不被重视。[res, resCov] = residualgps(gndFusionNoDetection, lla(sampleIdx，:)，.．.Rpos, gpsVel(sampleIdx，:)， Rvel);normalizedRes = res(1:3) ./ sqrt(diag(resCov(1:3,1:3))。”);如果(all(abs(normalizedRes) <= residualThreshold))根据GPS数据更新过滤器状态。fusegps(gndFusionNoDetection, lla(sampleIdx，:)， Rpos，.．.gpsVel (sampleIdx:), Rvel);如果usePoseView集(annoHandle,“字符串”，“异常值状态:无”，.．.“EdgeColor”，“k”）;结束其他的如果usePoseView集(annoHandle,“字符串”，“异常值状态:检测到”，.．.“EdgeColor”，“r”）;结束结束结束

误差度量计算

计算两个滤波器估计的位置误差。在不检查GPS测量中的任何异常值的过滤器中，位置误差有所增加。

计算位置误差。posdNoCheck = estPositionNoCheck - truePosition;posd = estPosition - truePosition;%绘图结果。T = (0:size(posd,1)-1)。' ./ imuFs;图(“单位”，“归一化”，“位置”， [0.2615 0.2833 0.4552 0.3700]) subplot(1,2,1) plot(t, posdNoCheck) ax = gca;yLims = get(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 = get(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”）)

结论

的residualgps对象函数可用于检测传感器测量中的潜在异常值，然后使用它们更新的滤波器状态insfilterNonholonomic对象。其他的姿态估计过滤对象，比如，insfilterMARG，insfilterAsync,insfilterErrorState也有类似的目标函数来计算传感器测量残差。