估算地面车辆的位置和定向
该示例示出了如何通过从惯性测量单元（IMU）和全球定位系统（GPS）接收器融合数据来估计地面车辆的位置和方向。
模拟设置
设置采样率。在典型的系统中，IMU中的加速度计和陀螺仪以相对较高的采样率运行。从融合算法中的那些传感器处理数据的复杂性相对较低。相反，GPS以相对较低的采样率运行，并且与处理相关的复杂性很高。在该融合算法中，GPS样品以低速率处理，加速度计和陀螺仪样品以相同的高速加工在一起。
为了模拟这种配置，IMU（加速度计和陀螺仪）在100Hz时采样，并且GPS在10 Hz上采样。
imufs = 100;GPSFS = 10;％定义地球上的位置使用纬度进行此模拟，％经度和高度（LLA）坐标。localorigin = [42.2825 -71.343 53.0352];％验证| GPSFS |分开| imufs |。这允许传感器样本使用嵌套循环模拟的％速率而无需复杂的采样率％匹配。imusamplespergps =（imufs / gpsfs）;断言（imusamplespergps == fix（imusamplespergps），......'GPS采样率必须是IMU采样率的整数因素。）;
融合过滤器
创建滤波器以保险为IMU + GPS测量。融合过滤器使用扩展的卡尔曼滤波器跟踪方向（作为四元数），位置，速度和传感器偏差。
这insfilternonholoromic.具有两种主要方法的对象：预测和Fusegps.。这预测方法采用IMU的加速度计和陀螺仪样品作为输入。打电话给预测每次采样加速度计和陀螺仪时，方法都有方法。该方法预测了基于加速度计和陀螺的一个时间步长的状态。在此步骤中更新了扩展卡尔曼滤波器的错误协方差。
这Fusegps.方法将GPS样本作为输入。该方法通过计算根据其不确定性来基于GPS样本来基于GPS样本更新滤波器状态。在此步骤中还更新了错误协方差，这次也使用Kalman Goin。
这insfilternonholoromic.对象有两个主要属性：imusamplevere.和decimationFactor.。地面车辆有两个速度约束，假设它不会从地面上反弹或在地面上滑动。使用扩展卡尔曼滤波器更新方程来应用这些约束。这些更新以速率应用于过滤器状态imusamplerate / decimationfactor.赫兹。
gndfusion = insfilternonholoromic（'参考范围'那'enu'那......'imusamplerate'，imufs，......'参考'，局部素，......'decimationfactor'，2）;
创造地面车辆轨迹
这WayPointTrajectory.对象根据指定的采样率，航点，到达时间和方向来计算姿势。指定地面车辆的圆形轨迹的参数。
％轨迹参数r = 8.42;％（m）速度= 2.50;％ （多发性硬化症）中心= [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）;间距=零（尺寸（偏航））;卷=零（尺寸（偏航））;定向=四元数（[偏航，俯仰，卷]，'euler'那......'Zyx'那'框架'）;％生成轨迹。地面= WayPointTrajectory（'采样率'，imufs，......'waypoints'， 位置，......'到达的时间'，t，......'方向'， 方向）;％初始化用于模拟传感器噪声的随机数生成器。RNG（'默认'）;
GPS接收器
在指定的采样率和参考位置设置GPS。另一个参数控制输出信号中噪声的性质。
GPS = GPSSensor（'updaterate'，GPSFS，'参考范围'那'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'那......'参考范围'那'enu'那'采样率'，imufs）;％加速度计Imu.accelerometerm.measurementRange = 19.6133;Imu.accelerometer。效果= 0.0023928;IMU.Accelerometer。不生系= 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 ^ 214:16
地面真理用于帮助初始化过滤器状态，因此过滤器会很快收敛到良好的答案。
％从轨迹的第一个样本获得初始地面真相姿势％并释放地面真相轨迹，以确保第一个样品不是仿真期间跳过％。[initialpos，initialatt，initialvel] = toundtruth（）;重置（地面）;％初始化过滤器的状态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.vps.velocityAccuracy。^ 2;rpos = gps.horizoNtalpositionAccacalacy。^ 2;该滤波器的地面车辆的动态模型假定存在百分比在运动期间没有侧滑或滑动。这意味着速度是％约束仅对前体轴。另外两个速度轴用零测量校正％读数被加权％| ZerovelocityConstraintNoise |范围。gndfusion.zerovelocityConstraintNoise = 1E-2;％流程噪音gndfusion.gyroscopenoise = 4e-6;gndfusion.gyroscopybiasnoise = 4e-14;gndfusion.acceleromernoise = 4.8e-2;gndfusion.accelerometerbiasnoise = 4e-14;％初始错误协方差gndfusion.statecovariance = 1e-9 *那些（16）;
初始化范围
这救助人员范围使得能够随着时间的推移绘制变量。这里用于跟踪姿势的错误。这HelperpositeViewer.范围允许3-D滤波器估计和地面真理姿势的可视化。范围可以减慢模拟。要禁用范围，请将相应的逻辑变量设置为错误的。
umererrscope = true;％打开流媒体错误绘图movingsview = true;％打开3D姿势查看器如果useerrscope errscope = evermercrollingplotter（......'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]，......'参考范围'那'enu'）;结尾
仿真环路
主模拟循环是一小时的循环，带有嵌套循环。while循环执行GPSFS.，这是GPS测量率。嵌套循环执行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（ToundTruth）IDX = IDX + 1;％模拟来自当前姿势的IMU数据。[trueposition（idx，:)，tryerverientation（idx，:)，......Truevel，TrueaCC，TrueanGel] = Tounttruth（）;[Acceldata，Gyrodata] = IMU（Trueacc，TrueanGel，......Triace主顾（IDX，:)）;％使用预测方法来估计基于滤波器状态的方法AcceldAta和Gyrodata阵列的百分比。预测（GNDFusion，Accelda，Gyrodata）;％记录估计的方向和位置。[estposition（idx，:)，estorientation（idx，:)] =姿势（gndfusion）;％计算错误和绘图。如果USERRSCOPE Orienterr = RAD2DEG（......dist（estorientation（idx，:)，triace主管（Idx，:)）;poserr = estposition（idx，:) - trueposition（idx，:);errscope（东方，poserr（1），poserr（2），poserr（3））;结尾％更新姿势查看器。如果UpployView Viewer（estposition（idx，:)，estorientation（idx，:)，......Trueposition（IDX，:)，estorientation（IDX，:)）;结尾结尾结尾如果〜ISDONE（Tountertruth）％这次下一步发生在GPS采样率。％基于当前姿势模拟GPS输出。[lla，gpsvel] = gps（Trueposition（Idx，:)，Truevel）;％基于GPS数据更新过滤器状态。Fusegps（GNDFusion，LLA，RPO，GPSVel，rvel）;结尾结尾
错误度量计算
在整个模拟中记录位置和方向。现在计算一个位置和方向的端到端根均匀误差。
POSD = estthosition - Trueposition;取向的％，四元距离是一个更好的替代方案减去具有不连续性的欧拉角度。四元数可以使用| dist |计算％距离|功能，它给了弧度方向的百分比差异。转换为程度％在命令窗口中显示。quatd = rad2deg（dist（estorientation，traiderientation））;％在命令窗口中显示RMS错误。fprintf（'\ n \ n \ n末端模拟位置rms错误\ n'）;MSEP = SQRT（平均值（POSD。^ 2））;fprintf（'\ tx：％.2f，y：％.2f，z：％.2f（米）\ n \ n'，msep（1），......MSEP（2），MSEP（3））;fprintf（'端到端四元数距离rms误差（度）\ n'）;fprintf（'\ t％.2f（度）\ n \ n'，sqrt（平均值（quatd ^ 2））））;
端到端仿真位置rms误差x：1.16，y：0.99，z：0.03（米）端到端四元数距离（度）0.09（度）
您还可以从以下列表中选择一个网站：