估计地面车辆的位置和方向

打开实时脚本

这个例子展示了如何通过融合来自惯性测量单元(IMU)和全球定位系统(GPS)接收器的数据来估计地面车辆的位置和方向。

仿真设置

设置采样率。在一个典型的系统中，IMU中的加速度计和陀螺仪以相对较高的采样率运行。融合算法处理这些传感器数据的复杂度相对较低。相反，GPS运行的采样率相对较低，处理它的复杂性很高。在该融合算法中，GPS样本以较低的速率被处理，加速度计和陀螺仪样本以同样高的速率被一起处理。

为了模拟这种配置，IMU(加速度计和陀螺仪)的采样频率为100 Hz, GPS的采样频率为10 Hz。

imuFs = 100;gpsFs = 10;使用纬度定义模拟在地球上的位置，%%经度和高度(LLA)坐标。localOrigin = [42.2825 -71.343 53.0352];%验证|gpsFs|除|imuFs|。这允许传感器采样%的速率，使用嵌套的for循环模拟，没有复杂的采样率%匹配。imuSamplesPerGPS = (imuFs/gpsFs);assert(imuSamplesPerGPS == fix(imuSamplesPerGPS)，.．.GPS采样率必须是IMU采样率的整数因子。）;

融合滤波器

创建过滤器来融合IMU + GPS测量。融合滤波器使用扩展卡尔曼滤波器来跟踪方向(作为四元数)、位置、速度和传感器偏差。

的insfilterNonholonomic对象有两个主要方法:预测而且fusegps．的预测方法以IMU上的加速度计和陀螺仪样本作为输入。调用预测方法每次对加速度计和陀螺仪进行采样。该方法基于加速度计和陀螺仪向前一时间步预测状态。这一步更新了扩展卡尔曼滤波器的误差协方差。

的fusegps方法以GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波器状态，该卡尔曼增益根据各种传感器输入的不确定性进行加权。在这一步中还更新了误差协方差，这一次也使用了卡尔曼增益。

的insfilterNonholonomic对象有两个主要属性:IMUSampleRate而且DecimationFactor．地面飞行器有两个速度限制，假设它不会从地面反弹或在地面上滑动。这些约束是应用扩展卡尔曼滤波更新方程。这些更新以的速率应用于筛选器状态IMUSampleRate / DecimationFactor赫兹。

gndFusion = insfilterNonholonomic(“ReferenceFrame”，“ENU表示”，.．.“IMUSampleRate”imuFs,.．.“ReferenceLocation”localOrigin,.．.“DecimationFactor”2);

创建地面车辆轨迹

的waypointTrajectory对象根据指定的采样率、路径点、到达时间和方向计算姿态。指定地面车辆的圆形轨迹参数。

%弹道参数R = 8.42;% (m)速度= 2.50;%(米/秒)中心= [0,0];% (m)initialYaw = 90;%(度)numRevs = 2;定义角和相应的到达时间t。revTime = 2*pi*r / speed;theta = (0:pi/2:2*pi*numRevs).';t = linspace(0, revTime*numRevs, nummel (theta)).';定义位置。X = r .* cos(theta) + center(1);Y = r .* sin(theta) + center(2);Z = 0(大小(x));位置= [x, y, z];定义方向。偏航= theta + deg2rad(initialYaw);偏航= mod(偏航，2*pi);俯仰=零(大小(偏航));滚=零(大小(偏航));方位=四元数([偏航，俯仰，横摇]，“欧拉”，.．.“ZYX股票”，“帧”）;%生成轨迹。groundTruth =航路点“SampleRate”imuFs,.．.“锚点”、位置、.．.“TimeOfArrival”t.．.“定位”、方向);初始化用于模拟传感器噪声的随机数生成器。rng (“默认”）;

GPS接收器

在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。

gps = gpsSensor(“UpdateRate”gpsFs,“ReferenceFrame”，“ENU表示”）;gps。ReferenceLocation = localOrigin;gps。衰减因子= 0.5;%随机游走噪声参数gps。HorizontalPositionAccuracy = 1.0;gps。VerticalPositionAccuracy = 1.0;gps。速度精度= 0.1;

IMU传感器

通常，地面车辆使用6轴IMU传感器进行姿态估计。为IMU传感器建模，定义一个包含加速度计和陀螺仪的IMU传感器模型。在实际应用中，这两个传感器可以来自同一个集成电路，也可以来自不同的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。

imu = imsensor (“accel-gyro”，.．.“ReferenceFrame”，“ENU表示”，“SampleRate”, imuFs);%加速度计imu.Accelerometer.MeasurementRange = 19.6133;imu.Accelerometer.Resolution = 0.0023928;imu.Accelerometer.NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope.MeasurementRange = deg2rad(250);imu.Gyroscope.Resolution = deg2rad(0.0625);imu. gyroscope . noisendensity = deg2rad(0.025);

的状态初始化`insfilterNonholonomic`

这些状态是:

状态单位指数方向(四元数部件)1:4陀螺仪偏差(XYZ) rad/s 5:7位置(NED) m 8:10速度(NED) m/s 11:13加速度计偏差(XYZ) m/s^2 14:16

Ground truth用于帮助初始化过滤器状态，因此过滤器可以快速收敛到好的答案。

从轨迹的第一个样本中获得初始的真实姿态%，并释放地面真相轨迹，以确保第一个样本不是%在模拟过程中跳过。[initialPos, initialAtt, initialVel] = groundTruth();重置(groundTruth);初始化筛选器的状态gndFusion.State(1:4) = compact(initialAtt).';gndFusion.State(5:7) = imu.Gyroscope.ConstantBias;gndFusion.State(8:10) = initialPos.';gndFusion.State(11:13) = initialVel.';gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;

的方差初始化`insfilterNonholonomic`

测量噪声描述了有多少噪声是破坏GPS读数基于gpsSensor车辆动力学模型的参数和不确定性。

过程噪声描述了滤波方程对状态演化的描述程度。过程噪声通过参数扫描来确定，从而联合优化来自滤波器的位置和方向估计。

%测量噪声精度= gps.VelocityAccuracy.^2;Rpos = gps.HorizontalPositionAccuracy.^2;该滤波器的地面车辆动态模型假设存在。运动时无侧滑或打滑。。这意味着速度是%仅约束到身体的前轴。另外两个速度轴%的读数用加权的零测量来校正% |ZeroVelocityConstraintNoise|参数。gndFusion。ZeroVelocityConstraintNoise = 1e-2;%进程噪声gndFusion。陀螺仪= 4e-6;gndFusion。GyroscopeBiasNoise = 4e-14;gndFusion。加速度计噪声= 4.8e-2;gndFusion。加速度计biasnoise = 4e-14;初始误差协方差gndFusion。StateCovariance = 1e-9*eye(16);

初始化范围

的HelperScrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势中的错误。的HelperPoseViewerscope允许三维可视化的滤波器估计和地面真实姿态。瞄准镜会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为假．

usererrscope = true;打开流错误图usePoseView = true;打开3D姿势查看器如果useErrScope errscope = HelperScrollingPlotter(.．.“NumInputs”4.．.“时间间隔”10.．.“SampleRate”imuFs,.．.“YLabel”, {“度”，.．.“米”，.．.“米”，.．.“米”}，.．.“标题”, {“四元数距离”，.．.“X位置错误”，.．.“位置Y错误”，.．.'位置Z错误'}，.．.“YLimits”，.．.[- 1,1 - 1,1 - 1,1 - 1,1]);结束如果usePoseView查看器= helpposeviewer (.．.“XPositionLimits”， [- 15,15]，.．.“YPositionLimits”， [- 15,15]，.．.“ZPositionLimits”， [- 5,5]，.．.“ReferenceFrame”，“ENU表示”）;结束

模拟循环

主要模拟循环是一个带有嵌套for循环的while循环。while循环在gpsFs，为GPS测量速率。嵌套的for循环在imuFs，为IMU采样率。作用域以IMU采样率更新。

totalSimTime = 30;%秒用于最终度量计算的日志数据。numsamples = floor(min(t(end)， totalSimTime) * gpsFs);truePosition = 0 (numsamples,3);trueOrientation = quaterion .zero (numsamples,1);estPosition = 0 (numsamples,3);estOrientation = quaterion .zero (numsamples,1);Idx = 0;为sampleIdx = 1:numsamples在IMU更新频率预测循环。为i = 1:imuSamplesPerGPS如果~isDone(groundTruth) idx = idx + 1;从当前姿态模拟IMU数据。: [truePosition idx), trueOrientation (idx:).．.trueVel, trueAcc, trueAngVel] = groundTruth();[accelData, gyroData] = imu(trueAcc, trueAngVel，.．.trueOrientation (idx:));使用预测方法来估计滤波器的状态在accelData和gyroData数组上。predict(gndFusion, accelData, gyroData);记录估计的方向和位置。[estPosition(idx，:)， estOrientation(idx，:)] = pose(gndFusion);计算误差并绘制。如果usererrscope orientErr = rad2deg(.．.dist (estOrientation (idx:), trueOrientation (idx,:)));posErr = estPosition(idx，:) - truePosition(idx，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束更新姿态查看器。如果usePoseView查看器(estPosition(idx，:)， estOrientation(idx，:)，.．.truePosition (idx:), estOrientation (idx:));结束结束结束如果~结束(groundTruth)下一步发生在GPS采样率。根据当前姿态模拟GPS输出。[lla, gpsVel] = gps(truePosition(idx，:)， trueVel);根据GPS数据更新过滤器状态。fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);结束结束

误差度量计算

在整个模拟过程中记录位置和方向。现在计算位置和方向的端到端的均方根误差。

posd = estPosition - truePosition;对于方向，四元数距离是一个更好的选择%减去欧拉角，欧拉角有不连续。四元数的%距离可以用|dist|函数计算，该函数给出%以弧度为单位的方向角差。转换为学位%显示。quatd = rad2deg(dist(estOrientation, trueOrientation));在命令窗口显示RMS错误。流(端到端仿真位置RMS误差\n'）;

端到端仿真位置RMS误差

Msep =√(mean(posd.^2));流(“\ tX: %。2f, Y: %。2f, Z: %。2 f(米)\ n \ n”msep (1).．.msep msep (2), (3));

X: 1.16, Y: 0.98, Z: 0.03(米)

流('端到端四元数距离RMS误差(度)\n'）;

端到端四元数距离RMS误差(度)

流(' \ t %。2 f(度)\ n \ n ', sqrt(平均(quatd ^ 2)));

0.11(度)