主要内容

紧密耦合IMU和GNSS的地面车辆姿态估计

打开实时脚本

这个例子展示了如何通过建立一个紧密耦合的扩展卡尔曼滤波器来估计地面车辆的位置和方向，并使用它来融合传感器测量。一个紧密耦合的滤波器将惯性测量单元(IMU)读数与原始全球导航卫星系统(GNSS)读数融合在一起。相反，松散耦合滤波器将IMU读数与过滤后的GNSS接收机读数融合在一起。

松耦合滤波图

松耦合过滤图

紧耦合滤波器图

尽管紧密耦合滤波器需要额外的处理，但当可用的GNSS卫星信号少于四个，或者某些卫星信号受到多径噪声等干扰时，可以使用它。这种类型的噪声可能发生在地面车辆通过有许多障碍物的部分道路时，例如在城市峡谷中，许多表面将信号组合反射回接收器，并干扰了直接信号。由于地面车辆处于城市环境中，有很多多路径噪声的机会，本例使用紧密耦合滤波器。

定义过滤器输入

为传感器模拟指定以下参数:

  1. 轨迹运动量

  2. IMU采样率

  3. GNSS接收机采样率

  4. RINEX导航消息文件

RINEX导航电文文件包含用于计算卫星位置和速度的GNSS卫星轨道参数。卫星位置和速度是生成GNSS伪距和伪距率的重要依据。

负载(“routeNatickMATightlyCoupled”，“pos”，“东方”，”或者“，“acc”，“angvel”，“lla0”，“imuFs”，“gnssFs”）;navfilename =“GODS00USA_R_20211750000_01D_GN.rnx”；

创建IMU传感器对象。设置轴偏差和恒定偏置值为0模拟校准。

imu = imussensor (SampleRate=imuFs);loadparams (imu,“generic.json”，“GenericLowCost9Axis”）;imu.Accelerometer.AxesMisalignment = 100*eye(3);imu.Accelerometer.ConstantBias = [0 0 0];imu.Gyroscope.AxesMisalignment = 100*eye(3);imu.Gyroscope.ConstantBias = [0 0 0];

方法读取RINEX文件rinexread函数。只使用该文件中的第一组GPS卫星，并将模拟的初始时间设置为该文件中的第一个时间步骤。

数据= rinexread(navfilename);[~，idx] = unique(data. gps . satellite iteid);navmsg = data.GPS(idx，:);t0 = navmsg.Time(1);

加载噪声参数，paramsTuned，为IMU传感器和GNSS接收机提供的tunedNoiseParameters垫文件。

负载(“tunedNoiseParameters.mat”，“paramsTuned”）;accelNoise = paramstund . accelerometernoise;gyroNoise = paramstune . gyroscopenoise;pseudorangeNoise = paramstund . examplehelperinsgnssnoise (1);pseudorangeRateNoise = paramstund . examplehelperinsgnssnoise (2);

设置RNG种子以产生可重复的结果。

rng默认的

创建过滤器和过滤器传感器模型

属性来创建紧密耦合筛选器insEKF对象，则必须定义过滤器状态到原始GNSS测量值的转换。使用exampleHelperINSGNSSHelper函数定义此转换。

gnss = exampleHelperINSGNSS;gnss。reference elocation = lla0;

方法创建加速度计和陀螺仪模型，从而定义IMU模型insAccelerometer而且insGyroscope对象,分别。

accel = ins加速度计;陀螺= ins陀螺仪;

通过使用IMU传感器模型、原始GNSS传感器模型和表示为的三维姿态运动模型来创建过滤器insMotionPose对象。

filt = insEKF(加速度，陀螺，gnss,insMotionPose);

的调优参数设置过滤器的初始状态paramsTuned．

filt。State = paramstund . initialstate;filt。StateCovariance = paramstund . initialstatecovariance;filt。AdditiveProcessNoise = paramstund .AdditiveProcessNoise;

估计车辆姿态

创建一个图，在其中查看过滤过程中地面车辆的位置估计。

figure posLLA = ned2lla(pos,lla0，“椭球体”）;geoLine = geoplot(posLLA(1,1)，posLLA(1,2)，“。”posLLA (1,1), posLLA(1、2),“。”）;Geolimits ([42.2948 42.3182]，[-71.3901 -71.3519]) geobasasemap地形传奇(“地面实况”，“过滤器”估计）

分配一个矩阵来存储位置估计结果。

numSamples = size(pos,1);estPos = NaN(numSamples,3);estOrient = ones(numSamples,1，“四元数”）;imuSamplesPerGNSS = imuFs/gnssFs;numGNSSSamples = numSamples/imuSamplesPerGNSS;

设置当前模拟时间为指定的初始时间。

T = t0;

融合IMU和原始GNSS测量值。在每次迭代中，将加速度计和陀螺仪测量值分别融合到GNSS测量值，以更新滤波器状态，使用由先前加载的噪声参数定义的协方差矩阵。在更新过滤器状态之后，记录新的位置和方向状态。最后，预测到下一个时间步骤的过滤器状态。

为ii = 1:numGNSSSamples为jj = 1:imuSamplesPerGNSS imuIdx = (ii-1)*imuSamplesPerGNSS + jj;[accelMeas,gyroMeas] = imu(acc(imuIdx，:)，angvel(imuIdx，:)，orient(imuIdx，:));保险丝(filt、accel accelMeas accelNoise);保险丝(filt、陀螺、gyroMeas gyroNoise);estPos(imuIdx，:) = stateparts(filt，“位置”）;estOrient(imuIdx，:) =四元数(stateparts(filt，“定位”));t = t +秒(1/imuFs);预测(filt 1 / imuFs);结束

更新卫星位置和原始GNSS测量值。

gnssIdx = ii*imuSamplesPerGNSS;recPos = posLLA(gnssIdx，:);recVel = vel(gnssIdx，:);[satPos,satVel,satIDs] = gnssconstellation(t，“RINEXData”, navmsg);[az,el,vis] = lookangles(recPos,satPos);[p,pdot] =伪橙子(recPos,satPos(vis，:)，recVel,satVel(vis，:));Z = [p;pdot];

更新GNSS模型上的卫星位置。

gnss。卫星位置= satPos(vis，:);gnss。satellite evelocity = satVel(vis，:);

融合原始GNSS测量值。

保险丝(gnss filt, z,.．.[pseudorangeNoise *的(元素个数(p)), pseudorangeRateNoise *的(1,元素个数(pdot))));estPos(gnssIdx，:) = stateparts(filt，“位置”）;estOrient(gnssIdx，:) =四元数(stateparts(filt，“定位”));

更新位置估计图。

estPosLLA = ned2lla(estPos,lla0，“椭球体”）;集(凡士林(1)LatitudeData = posLLA (1: gnssIdx, 1), LongitudeData = posLLA (1: gnssIdx, 2));集(凡士林(2)LatitudeData = estPosLLA (1: gnssIdx, 1), LongitudeData = estPosLLA (1: gnssIdx, 2));drawnowlimitrate结束

图中包含一个轴对象。axis对象包含2个line类型的对象。这些对象代表Ground truth, Filter估计。

验证结果

计算均方根误差以验证结果。紧耦合滤波器使用所提供的传感器读数来估计地面车辆姿态，具有相对较低的均方根误差。

posDiff = estPos - pos;posRMS =√(mean(posDiff.^2));disp (['三维位置RMS错误- X: 'num2str (posRMS (1)),”,Y:“，.．.num2str (posRMS (2)),'， z: 'num2str (posRMS (3)),“(m)”])
三维位置均方根误差- X: 0.8522, Y:0.62864, Z: 0.9633 (m)
orientDiff = rad2deg(dist(estOrient,orient));orientRMS =√(mean(orientDiff.^2));disp ([方位RMS错误-num2str (orientRMS),“(度)”])
方向均方根误差- 4.0385(度)