估算地面车辆的位置和定向
该示例示出了如何通过从惯性测量单元（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）；assert（imuSamplesPerGPS==fix（imuSamplesPerGPS），...'GPS采样率必须是IMU采样率的整数因素。）;
融合滤波器
创建滤波器以保险为IMU + GPS测量。融合过滤器使用扩展的卡尔曼滤波器跟踪方向（作为四元数），位置，速度和传感器偏差。
这个insfilternonholoromic.具有两种主要方法的对象：预测和fusegps.这个预测方法将来自IMU的加速度计和陀螺仪样本作为输入预测每次采样加速度计和陀螺仪时，方法都有方法。该方法预测了基于加速度计和陀螺的一个时间步长的状态。在此步骤中更新了扩展卡尔曼滤波器的错误协方差。
这个fusegps该方法将GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波器状态，该卡尔曼增益根据各种传感器输入的不确定性对其进行加权。在该步骤中还更新了误差协方差，这一次还使用了卡尔曼增益。
这个insfilternonholoromic.对象有两个主要属性:IMUSampleRate和decimationFactor.。地面车辆有两个速度约束，假设它不会从地面反弹或在地面上滑动。这些约束使用扩展卡尔曼滤波器更新方程应用。这些更新以imusamplerate / decimationfactor.赫兹。
gndFusion = insfilterNonholonomic (“ReferenceFrame”,'enu',...“IMUSampleRate”imuFs,...“参考定位”，本地来源，...“决策因素”, 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 (“采样器”imuFs,...“航路点”、位置、...“到达时间”T...'方向'，方向）；％初始化用于模拟传感器噪声的随机数生成器。RNG（'默认'）;
GPS接收器
在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
gps=gps传感器('updaterate'，GPSFS，“ReferenceFrame”,'enu')；gps.ReferenceLocation=localOrigin；gps.DecayFactor=0.5；%随机游动噪声参数gps.HorizontalPositionAccurance=1.0；gps.VerticalPositionAccurance=1.0；gps.VelocityAccuracy=0.1；
IMU传感器
通常，地面车辆使用6轴IMU传感器进行姿态估计。要建模一个IMU传感器，定义一个包含加速度计和陀螺仪的IMU传感器模型。在实际应用中，这两个传感器可以来自一个集成电路，也可以来自单独的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。
imu=imu传感器(“加速陀螺仪”,...“ReferenceFrame”,'enu',“采样器”, imuFs);%加速度计imu.Accelerator.MeasurementTrange=19.6133；imu.Accelerator.Resolution=0.0023928；imu.Accelerator.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
地面真实值用于帮助初始化过滤器状态，因此过滤器可以快速收敛到正确答案。
%从轨迹的第一个样本中获取初始地面真实姿势％并释放地面真相轨迹，以确保第一个样品不是仿真期间跳过％。[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；Rpos=gps.HorizontalPositionAccuracy.^2；%该过滤器的地面车辆动态模型假定运动过程中无侧滑或打滑。这意味着速度是%仅约束到前车身轴。其他两个速度轴%读数通过称重的零测量值进行校正% | ZeroVelocityConstraintNoise |参数。gndFusion.ZeroVelocityConstraintNoise=1e-2；%过程噪声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姿势查看器如果UseerScope errscope=HelperScrollingPlotter(...“NumInputs”4...“时间跨度”10...“采样器”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循环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更新频率下％预测循环。对于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阵列上。预测(gndFusion accelData gyroData);%记录估计的方向和位置。[estPosition(idx，:)， estOrientation(idx，:)] = pose(gndFusion);%计算误差并绘图。如果usererrscope orientErr = rad2deg(...dist（estOrientation（idx，：），trueOrientation（idx，：）；posErr=estPosition（idx，：）-truePosition（idx，：）；errscope（orienter，posErr（1），posErr（2），posErr（3））；结束%更新姿势查看器。如果UpployView Viewer（estposition（idx，:)，estorientation（idx，:)，...Trueposition（IDX，:)，estorientation（IDX，:)）;结束结束结束如果~isDone（地面真相）％这次下一步发生在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错误。fprintf('\ n \ n \ n末端模拟位置rms错误\ n'）;msep =√意味着(posd ^ 2));fprintf('\tX:%.2f，Y:%.2f，Z:%.2f（米）\n\n'，msep（1），...msep（2），msep（3））；fprintf('端到端四元数距离RMS误差(度)\n'）;fprintf(' \ t %。2 f(度)\ n \ n '，sqrt（平均值（quatd ^ 2））））;
端到端模拟位置均方根误差X:1.16，Y:0.99，Z:0.03（米）端到端四元数距离均方根误差（度）0.09（度）
你也可以从以下列表中选择一个网站: