估算地面车辆的位置和定向
该示例示出了如何通过从惯性测量单元（IMU）和全球定位系统（GPS）接收器融合数据来估计地面车辆的位置和方向。
仿真设置
设置采样率。在典型的系统中，IMU中的加速度计和陀螺仪以相对较高的采样率运行。从融合算法中的那些传感器处理数据的复杂性相对较低。相反，GPS以相对较低的采样率运行，并且与处理相关的复杂性很高。在该融合算法中，GPS样品以低速率处理，加速度计和陀螺仪样品以相同的高速加工在一起。
为了模拟这个配置，IMU(加速度计和陀螺仪)以100hz采样，GPS以10hz采样。
imuFs = 100;gpsFs = 10;使用纬度定义模拟在地球上发生的位置，%经度和高度(LLA)坐标。localorigin = [42.2825 -71.343 53.0352];％验证| GPSFS |分开| imufs |。这允许传感器样本使用嵌套循环模拟的％速率而无需复杂的采样率%匹配。imuSamplesPerGPS = (imuFs / gpsFs);断言(imuSamplesPerGPS = =修复(imuSamplesPerGPS),．..'GPS采样率必须是IMU采样率的整数因素。）;
融合滤波器
创建滤波器以保险为IMU + GPS测量。融合过滤器使用扩展的卡尔曼滤波器跟踪方向（作为四元数），位置，速度和传感器偏差。
的insfilternonholoromic.具有两种主要方法的对象：预测和fusegps．的预测方法以来自IMU的加速度计和陀螺仪样本作为输入。调用预测每次采样加速度计和陀螺仪时，方法都有方法。该方法预测了基于加速度计和陀螺的一个时间步长的状态。在此步骤中更新了扩展卡尔曼滤波器的错误协方差。
的fusegps方法以GPS样本为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波状态，卡尔曼增益根据传感器输入的不确定性对其进行加权。误差协方差也在这一步更新，这一次使用卡尔曼增益。
的insfilternonholoromic.对象有两个主要属性: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。retime = 2*pi*r / speed;θ=(0:π/ 2:2 *π* numRevs)。”;t = linspace(0, revTime* numrev, nummel (theta)); ';%定义的位置。X = r .* cos + center(1);Y = r * sin() + center(2);z = 0(大小(x));位置= [x, y, z];%定义取向。偏航= theta + deg2rad(initialYaw);Yaw = mod(Yaw, 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 = deg2rad（250）;imu.gyroscope.resolution = deg2rad（0.0625）;imu.gyroscope.noisedensity = deg2rad（0.025）;
对象的状态初始化insfilternonholoromic.
美国是:
状态单位指数方向(四元数部分)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）=初始vel。';gndfusion.state（14:16）= imu.accelerometer.constantbias;
初始化变量的方差insfilternonholoromic.
测量噪声描述了有多少噪声损坏了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 = 1e-9 *那些（16）;
初始化范围
的HelperScrollingPlotter范围使得能够随着时间的推移绘制变量。这里用于跟踪姿势的错误。的HelperPoseViewer范围允许过滤器估计和地面真实姿态的三维可视化。示波器会减慢模拟的速度。若要禁用范围，请将相应的逻辑变量设置为假．
umererrscope = true;%打开流错误图usePoseView = true;打开3D姿势查看器如果usererrscope errscope = HelperScrollingPlotter(．..“NumInputs”4．..“时间间隔”10．..“SampleRate”imuFs,．..'ylabel', {“度”，．..“米”，．..“米”，．..“米”}，．..'标题', {“四元数距离”，．..'位置x错误'，．..“Y位置错误”，．..“Z位置错误”}，．..“YLimits”，．..[- 1,1 - 1,1 - 1,1 - 1,1]);结束如果UpplyView Viewer = HelperposeViewer（．..“XPositionLimits”(-15年,15),．..'ypositionLimits'(-15年,15),．..“ZPositionLimits”, 5, 5],．..“ReferenceFrame”，'enu'）;结束
模拟循环
主模拟循环是一个带有嵌套的for循环的while循环。的处执行while循环gpsFs，即GPS测量速率。嵌套的for循环在imuFs，这是IMU采样率。该范围以IMU采样率更新。
totalSimTime = 30;%秒%记录最终度量计算的数据。numsamples =楼层（min（t（终端），totalsimtime）* gpsfs）;trueposition = zeros（numsamples，3）;Trueorientation = Quaternion.zeros（NumSamples，1）;estposition = zeros（numsamples，3）;estorientation = Quaternion.zeros（Numsamples，1）;Idx = 0;为sampleIdx = 1: numsamplesIMU更新频率下％预测循环。为我= 1:imuSamplesPerGPS如果~isDone(groundTruth) idx = idx + 1;%从当前姿势模拟IMU数据。: [truePosition idx), trueOrientation (idx:)．..true, true, true] = groundTruth();[accelData, gyroData] = imu(trueAcc, trueAngVel, true)．..trueOrientation (idx:));%使用预测方法估计基于滤波器的状态%在accelData和gyroData数组上。预测(gndFusion accelData gyroData);%记录估计的方向和位置。[estPosition(idx，:)， estOrientation(idx，:)] = pose(gndFusion);%计算误差并绘图。如果usererrscope orientErr = rad2deg(．..dist (estOrientation (idx:), trueOrientation (idx,:)));posErr = estPosition(idx，:) - true (idx，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束%更新姿势查看器。如果UpployView Viewer（estposition（idx，:)，estorientation（idx，:)，．..Trueposition（IDX，:)，estorientation（IDX，:)）;结束结束结束如果~结束(groundTruth)％这次下一步发生在GPS采样率。%根据当前姿态模拟GPS输出。[lla, gpsVel] = gps(true (idx，:)， true);%根据GPS数据更新过滤器状态。fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);结束结束
错误度量计算
在整个仿真过程中记录位置和方向。现在计算端到端均方根误差的位置和方向。
posd = estPosition - true;对于方向，四元数距离是一个更好的选择%减去欧拉角，欧拉角有不连续。四元数的%距离可以用|dist|函数计算，该函数给出以弧度表示的方向的角差。转换为度%显示在命令窗口中。quatd = rad2deg(dist(estOrientation, trueOrientation));%在命令窗口中显示RMS错误。流('\n\ n端到端仿真位置均方根误差\n'）;msep =√意味着(posd ^ 2));流(“\ tX: %。2f, Y: %。Z: % 2 f。2 f(米)\ n \ n”，msep（1），．..msep msep (2), (3));流('端到端四元数距离RMS误差(度)\n'）;流(' \ t %。2 f(度)\ n \ n '，sqrt（平均值（quatd ^ 2））））;
端到端仿真位置RMS误差X: 1.16, Y: 0.99, Z: 0.03(米)端到端四元数距离RMS误差(度)0.09(度)
你也可以从以下列表中选择一个网站:
