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

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

仿真设置

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

为了模拟这种配置，IMU(加速度计和陀螺仪)以100hz采样，GPS以10hz采样。

imuFs = 100;gpsFs = 10;用纬度定义这个模拟在地球上发生的位置，经度和高度(LLA)坐标。localOrigin = [42.2825 -71.343 53.0352];%验证| gppsfs |除|imuFs|。这允许传感器采样%的速率，使用嵌套的for循环模拟，没有复杂的抽样速率%匹配。imuSamplesPerGPS = (imuFs / gpsFs);断言(imuSamplesPerGPS = =修复(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;%(米/秒)Center = [0,0];% (m)initialYaw = 90;%(度)numRevs = 2;定义角度和相应的到达时间。revTime = 2*pi*r / speed;θ=(0:π/ 2:2 *π* numRevs)。”;t = linspace(0, revTime*numRevs, numel(theta)).';%定义的位置。X = r .* cos() + center(1);Y = r .* sin() + center(2);z = 0(大小(x));位置= [x, y, z];%定义取向。偏航= theta + deg2rad(initialYaw);偏航= mod(偏航，2*pi);距= 0(大小(偏航));滚= 0(大小(偏航));方位=四元数([偏航，俯仰，横摇]，“欧拉”，.．.“ZYX股票”，“帧”）;%生成轨迹。groundTruth = waypointTrajectory (“SampleRate”imuFs,.．.“锚点”、位置、.．.“TimeOfArrival”t.．.“定位”、方向);初始化用于模拟传感器噪声的随机数生成器。rng (“默认”）;

GPS接收器

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

全球定位系统(gps) = gpsSensor (“UpdateRate”gpsFs,“ReferenceFrame”，“ENU表示”）;gps。ReferenceLocation = localOrigin;gps。DecayFactor = 0.5;随机游走噪声参数gps。HorizontalPositionAccuracy = 1.0;gps。VerticalPositionAccuracy = 1.0;gps。VelocityAccuracy = 0.1;

IMU传感器

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

imu = imuSensor (“accel-gyro”，.．.“ReferenceFrame”，“ENU表示”，“SampleRate”, imuFs);%加速度计imu.Accelerometer.MeasurementRange = 19.6133;imu.Accelerometer.Resolution = 0.0023928;imu.Accelerometer.NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope.MeasurementRange =函数(250);imu.Gyroscope.Resolution函数= (0.0625);imu.Gyroscope.NoiseDensity函数= (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) =紧凑(initialAtt)。”;gndFusion.State (7) = imu.Gyroscope.ConstantBias;gndFusion.State (8) = initialPos。”;gndFusion.State (11:13) = initialVel。”;gndFusion.State(十四16)= imu.Accelerometer.ConstantBias;

的方差初始化`insfilterNonholonomic`

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

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

%测量噪声Rvel = gps.VelocityAccuracy。^ 2;rpo = gps.HorizontalPositionAccuracy。^ 2;此过滤器的地面车辆的动力学模型假设有运动时无侧滑或打滑现象。。这意味着速度是%约束到只有身体前轴。另外两个速度轴%的读数被修正为零测量加权% | ZeroVelocityConstraintNoise |参数。gndFusion。ZeroVelocityConstraintNoise = 1飞行;%过程噪音gndFusion。GyroscopeNoise = 4 e-6;gndFusion。GyroscopeBiasNoise = 4 e-14;gndFusion。AccelerometerNoise = 4.8依照;gndFusion。AccelerometerBiasNoise = 4 e-14;初始误差协方差gndFusion。StateCovariance = 1 e-9 *眼(16);

初始化范围

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

useErrScope = 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 = quaternion.zeros (numsamples, 1);estPosition = 0 (numsamples, 3);estOrientation = quaternion.zeros (numsamples, 1);idx = 0;为sampleIdx = 1: numsamples在IMU更新频率预测循环。为我= 1:imuSamplesPerGPS如果~isDone(groundTruth) idx = idx + 1;从当前姿势模拟IMU数据。: [truePosition idx), trueOrientation (idx:).．.trueVel, trueAcc, trueAngVel] = groundTruth();[accelerdata, gyroData] = imu(trueAcc, trueAngVel，.．.trueOrientation (idx:));使用基于预测的方法估计滤波器状态在accelerdata和gyroData阵列上的%。预测(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 viewer (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|函数计算，它给出%以弧度表示的方向角差。转换为% display在命令窗口。quatd = rad2deg(dist(estOrientation, trueOrientation));在命令窗口中显示RMS错误。流('端到端模拟位置RMS错误\n'）;

端到端仿真位置RMS错误

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

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

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

端到端四元数距离均方根误差(度)

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

0.11(度)