估计地面车辆的位置和方向
这个例子展示了如何通过融合来自惯性测量单元(IMU)和全球定位系统(GPS)接收器的数据来估计地面车辆的位置和方位。
模拟设置
设置采样率。在一个典型的系统中，IMU中的加速度计和陀螺仪以相对较高的采样率运行。融合算法处理这些传感器数据的复杂度相对较低。相反，GPS运行在一个相对较低的采样率和与处理相关的复杂性高。该融合算法对GPS样本进行低速率处理，对加速度计和陀螺仪样本同时进行高速率处理。
为了模拟这种配置，IMU（加速度计和陀螺仪）在100Hz时采样，并且GPS在10 Hz上采样。
imufs = 100;GPSFS = 10;%利用纬度确定这个模拟发生在地球上的位置，％经度和高度（LLA）坐标。localOrigin = [42.2825 -71.343 53.0352];%验证|gpsFs|除|imuFs|。这允许传感器取样在没有复杂采样率的情况下，使用嵌套的for循环模拟%的采样率％匹配。imuSamplesPerGPS = (imuFs / gpsFs);断言(imuSamplesPerGPS = =修复(imuSamplesPerGPS),．..GPS采样率必须是IMU采样率的整数因子。）;
融合滤波器
创建过滤器来融合IMU + GPS测量。融合滤波器使用扩展卡尔曼滤波器跟踪方向(作为四元数)，位置，速度和传感器偏差。
的insfilterNonholonomic对象，该对象有两个主要方法:预测和fusegps．的预测方法从IMU中获取加速度计和陀螺仪样本作为输入。调用预测方法每次对加速度计和陀螺仪进行采样。该方法以加速度计和陀螺仪为基础，对状态进行时间步长预测。这一步更新了扩展卡尔曼滤波器的误差协方差。
的fusegps方法以GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波状态，该卡尔曼增益根据不同的传感器输入的不确定性来加权。误差协方差也在这一步更新，这一次也使用卡尔曼增益。
的insfilterNonholonomic对象有两个主要属性：imusamplevere.和DecimationFactor．地面车辆有两个速度约束，假设它不从地面反弹或在地面上滑行。这些约束是应用扩展卡尔曼滤波器更新方程。这些更新以如下的速率应用到过滤器状态IMUSampleRate / DecimationFactor赫兹。
gndfusion = insfilternonholoromic（“ReferenceFrame”，“ENU表示”，．..'imusamplerate'，imufs，．..“ReferenceLocation”localOrigin,．..“DecimationFactor”2);
创建地面车辆轨迹
的WayPointTrajectory.对象根据指定的采样率，航点，到达时间和方向来计算姿势。指定地面车辆的圆形轨迹的参数。
%轨迹参数r = 8.42;％（m）速度= 2.50;％ （小姐）Center = [0, 0];％（m）initialyaw = 90;%(度)numRevs = 2;％定义角度θ和到达的相应时间。Reptime = 2 * Pi * R /速度;θ=（0：PI / 2：2 * PI * NUMREVS）。';t = linspace（0，转发时间* numrevs，numel（theta））。';%定义的位置。x = r。* cos（theta）+中心（1）;y = r。* sin（theta）+中心（2）;z = zeros（尺寸（x））;位置= [x，y，z];％定义方向。yaw = theta + deg2rad（initialyaw）;yaw = mod（yaw，2 * pi）;间距=零（尺寸（偏航））;卷=零（尺寸（偏航））;定向=四元数（[偏航，俯仰，卷]，“欧拉”，．..“ZYX股票”，'框架'）;％生成轨迹。地面= 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位置(内德)m 8:10速度(内德)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 = 4e-6;gndfusion.gyroscopybiasnoise = 4e-14;gndfusion.acceleromernoise = 4.8e-2;gndfusion.accelerometerbiasnoise = 4e-14;%初始误差协方差gndFusion。StateCovariance = 1 e-9 * 1 (16);
初始化范围
的救助人员Scope允许随时间绘制变量。它在这里用来跟踪姿势中的误差。的HelperPoseViewer范围允许3-D滤波器估计和地面真理姿势的可视化。范围可以减慢模拟。要禁用范围，请将相应的逻辑变量设置为错误的．
useErrScope = true;%打开流错误图movingsview = true;％打开3D姿势查看器如果usererrscope errscope = HelperScrollingPlotter(．..'numinputs'，4，．..“时间间隔”10，．..“SampleRate”，imufs，．..“YLabel”，{“度”，．..'米'，．..'米'，．..'米'}，．..“标题”，{'四元距离'，．..“X位置错误”，．..'位置y错误'，．..“Z位置错误”}，．..'ylimits'，．..[-1,1 -1,1 -1,1-1,1]）;结尾如果使用poseview查看器= HelperPoseViewer(．..“XPositionLimits”，[-15,15]，．..“YPositionLimits”，[-15,15]，．..“ZPositionLimits”，[-5,5]，．..“ReferenceFrame”，“ENU表示”）;结尾
仿真环路
主模拟循环是一个带有嵌套for循环的while循环。时执行while循环GPSFS.，这是GPS测量率。嵌套循环执行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（ToundTruth）IDX = IDX + 1;%模拟当前姿态的IMU数据。: [truePosition idx), trueOrientation (idx:)．..truelevel, trueAcc, trueAngVel] = groundTruth();[accelData, gyroData] = imu(trueAcc, trueAngVel，．..Triace主顾（IDX，:)）;％使用预测方法来估计基于滤波器状态的方法%在accelData和gyroData阵列上。预测（GNDFusion，Accelda，Gyrodata）;％记录估计的方向和位置。[estposition（idx，:)，estorientation（idx，:)] =姿势（gndfusion）;%计算误差并绘图。如果USERRSCOPE 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，RPO，GPSVel，rvel）;结尾结尾
误差指标计算
在整个模拟中记录位置和方向。现在计算一个位置和方向的端到端根均匀误差。
POSD = estthosition - Trueposition;%对于定向，四元数距离是一个更好的选择减去具有不连续性的欧拉角度。四元数% distance可以用|dist|函数来计算，它给出了弧度方向的百分比差异。转换为程度% display命令窗口。quatd = rad2deg(dist(estOrientation, trueOrientation));％在命令窗口中显示RMS错误。流('\n\ n端到端仿真位置RMS错误\n'）;MSEP = SQRT（平均值（POSD。^ 2））;流(“\ tX: %。2f, Y: %。Z: % 2 f。2 f(米)\ n \ n”msep (1)．..msep msep (2), (3));流('端到端四元数距离rms误差（度）\ n'）;流('\ t％.2f（度）\ n \ n', sqrt(平均(quatd ^ 2)));
端到端四元数距离RMS误差X: 1.16, Y: 0.99, Z: 0.03(米)
您还可以从以下列表中选择一个网站：