估计地面车辆的位置和姿态
这个例子展示了如何估计地面车辆的位置和姿态融合数据从一个惯性测量单元(IMU)和全球定位系统(GPS)接收机。
仿真设置
设置采样率。在一个典型系统中,加速度计和陀螺仪IMU在相对较高的采样率。处理数据的复杂性从这些传感器的融合算法相对较低。相反,全球定位系统运行在一个相对较低的采样率和处理相关的复杂性高。在这种融合算法GPS样品加工速度低,加速度计和陀螺仪样品处理在高速率是相同的。
来模拟这个配置,乌兹别克斯坦伊斯兰运动(加速度计和陀螺仪)采样在100赫兹,和GPS是采样10赫兹。
imuFs = 100;gpsFs = 10;%定义在地球上这个模拟发生使用纬度,%经度和海拔(LLA)坐标。localOrigin = (42.2825 -71.343 53.0352);%验证| gpsFs | |分裂imuFs |。这使得传感器样品%的利率是模拟使用嵌套循环,没有复杂的采样率%匹配。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;%(米/秒)中心= (0,0);% (m)initialYaw = 90;%(度)numRevs = 2;%定义角度θ和相应的到达的时间t。revTime = 2 *π* r /速度;θ=(0:π/ 2:2 *π* numRevs)。”;t = linspace (0 revTime * numRevs元素个数(θ)。”;%定义的位置。x = r。* cos(θ)+中心(1);y = r。* sin(θ)+(2)中心;z = 0(大小(x));位置= [x, y, z];%定义取向。偏航=θ+函数(initialYaw);偏航=国防部(偏航,2 *π);距= 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传感器
通常情况下,地面车辆使用六轴IMU传感器姿态估计。一个IMU传感器模型,定义一个IMU传感器模型包含一个加速度计和陀螺仪。在现实的应用程序中,这两个传感器可以来自一个单一的集成电路或独立的。属性值设置下面是典型的低成本的MEMS传感器。
imu = imuSensor (“accel-gyro”,…“ReferenceFrame”,“ENU表示”,“SampleRate”,imuFs);%加速度计imu.Accelerometer。MeasurementRange = 19.6133;imu.Accelerometer。分辨率= 0.0023928;imu.Accelerometer。NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope。MeasurementRange =函数(250);imu.Gyroscope。解析函数= (0.0625);imu.Gyroscope。NoiseDensity函数= (0.025);
初始化的状态insfilterNonholonomic
美国是:
国家单位指数取向(四元数部分)1:4陀螺仪偏见(某某)rad / s 5:7位置(NED) m 8:10分速度(NED) m / s 11:13加速度计偏差(某某)十四16米/秒^ 2
地面真理是用于初始化滤波器,滤波器收敛迅速好的答案。
%得到初始地面真理构成从第一个样品的轨迹%和释放地面轨迹,确保第一个示例不是真理在模拟%跳过。[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 * 1 (16);
初始化范围
的HelperScrollingPlotter随着时间的推移使策划范围的变量。使用它来跟踪错误的姿势。的HelperPoseViewer允许范围的3 d可视化滤波器估计和地面真理构成。范围可以缓慢的模拟。禁用一个范围,设置相应的逻辑变量假。
useErrScope = true;%打开流错误情节usePoseView = true;%的3 d查看器如果useErrScope errscope = HelperScrollingPlotter (…“NumInputs”4…“时间间隔”10…“SampleRate”imuFs,…“YLabel”,{“度”,…“米”,…“米”,…“米”},…“标题”,{“四元数距离”,…“X位置错误”,…“Y位置错误”,…“Z位置错误”},…“YLimits”,…(1 1 1 1 1 1 1 1]);结束如果usePoseView观众= HelperPoseViewer (…“XPositionLimits”(-15年,15),…“YPositionLimits”(-15年,15),…“ZPositionLimits”,5,5],…“ReferenceFrame”,“ENU表示”);结束
模拟循环
主仿真循环是一个while循环的嵌套循环。while循环执行的gpsFs,这是GPS测量的速度。的嵌套循环执行的imuFsIMU的采样率。IMU的范围更新采样率。
totalSimTime = 30;%秒%日志数据进行最后度量计算。numsamples =地板(min (t(结束),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如果~结束(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:)] =姿势(gndFusion);%计算错误和阴谋。如果useErrScope 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 (lla gndFusion, rpo, gpsVel Rvel);结束结束
误差指标计算
位置和姿态是整个模拟记录。现在计算一个端到端的位置和姿态的根均方误差。
posd = estPosition - truePosition;%为取向,四元数距离是一个更好的选择%减去欧拉角,不连续。四元数的与| dist | %距离计算函数,给出了%角不同取向的弧度。转换为度%在命令窗口中显示。quatd = rad2deg (dist (estOrientation trueOrientation));%显示RMS命令窗口中的错误。流(' \ n \ nEnd-to-End模拟位置均方根误差\ n”);msep =√意味着(posd ^ 2));流(“\ tX: %。Y: % 2 f。Z: % 2 f。2 f(米)\ n \ n”msep (1)…msep msep (2), (3));流(“端到端四元数距离均方根误差(度)\ n ');流(' \ t %。2 f(度)\ n \ n ',sqrt(平均(quatd ^ 2)));
端到端模拟位置均方根误差X: 1.16, Y: 0.99, Z: 0.03(米)的端到端均方根误差四元数距离(0.09度)(度)
