EKF-Based里程碑式的大满贯

打开生活的脚本

这个例子展示了如何使用ekfSLAM可靠的具有里程碑意义的同时实现本地化和映射(大满贯)使用扩展卡尔曼滤波(EKF)算法和最大似然算法进行数据关联。在本例中,您创建一个具有里程碑意义的地图的实时环境的车辆,同时跟踪车辆的路径。生成一个轨迹通过移动车辆使用的控制命令,并形成地图使用地标遇到沿着路径。正确的车辆轨迹和具有里程碑意义的估计通过观察地标。

加载数据集

加载一个维多利亚公园的修改版本的数据集,其中包含控制器输入,测量,GPS经度和纬度,并使用控制器输入和航迹生成运动模型。

负载(“victoriaParkDataset.mat”,“controllerInput”,…“测量”,“gpsLatLong”,“deadReckoning”);

设置参数

指定初始车辆状态和状态协方差。

initialState = [gpsLatLong (1、2) gpsLatLong函数(1,1)(37)]';initialCovar =每股收益*眼(3);

指定过程噪声协方差速度和转向的命令。

sigmaVelocity = 2;%(米/秒)sigmaSteer =函数(6);% (rad)processNoise = [sigmaVelocity ^ 2 0;0 sigmaSteer ^ 2);

指定测量距离和方位的协方差。

sigmaRange = 1;% [m]sigmaBearing =函数(3);% (rad)measCovar = [sigmaRange ^ 2 sigmaBearing ^ 2];

指定的最大射程为协会检查地标。

maxSensorRange = 30;% [m]

指定的时间步大小车辆移动。

步伐= 0.025;%(秒)

指定数据关联函数的阈值,nav.algs.associateMaxLikelihood。具有里程碑意义的拒绝阈值是卡方( $χ^{2}$ )分布表值95%正确的协会。

landmarkRejectionThres = 5.991;%的最大距离landmarkAugmentationThres = 200;%最小距离为创造的新地标validationGate = [landmarkRejectionThres landmarkAugmentationThres];

设置一个标志,确定绘制地图在运行。

plotOnTheRun = false;

得到样本数据集的数量。

numSamples =大小(controllerInput, 1);

初始化图或者进度条

如果启用了生活情节选项,创建一个新的图,建立处理策划各种组件。否则,创建一个更新的酒吧等的数据集的比例执行。设置等待栏,以便点击取消停止正在进行的计算。关闭等酒吧停止执行。

如果plotOnTheRun [robotHandle covarianceHandle sensorHandle,…observationHandle landmarkHandle,…deadRecHandle estTrajHandle] =…exampleHelperInitializeVisualizationEKFSLAM (initialState gpsLatLong);其他的waitBarHandle = waitbar (0,“初始化例子…”,…“名字”,“ekfSLAM例子”,…“CreateCancelBtn”,“setappdata (gcbf,“取消”,1)');setappdata (waitBarHandle“取消”,0);结束

设置ekfSLAM

配置ekfSLAM对象使用车辆的初始状态,初始状态协方差,过程噪声协方差和车辆的运动模型。

ekfSlamObj = ekfSLAM (“状态”initialState,…“StateCovariance”initialCovar,…“StateTransitionFcn”,@exampleHelperVictoriaParkStateTransition);ekfSlamObj。ProcessNoise = ProcessNoise;ekfSlamObj。MaxAssociationRange = maxSensorRange;

主循环

主循环包含这些基本操作:

预测根据控制命令,预测下一个状态和当前状态。
具有里程碑意义的提取——环境中的地标。
修正——更新状态和状态协方差用观察到的地标。

预测

在这个例子中,车辆移动相对于以前的状态基于控制输入,而地标保持静止。因此,只有车辆状态的传播。使用车辆的运动模型车辆的状态传播到下一个时间步使用的数据集。

的预测方法调用中指定的函数StateTransitionFcn的属性ekfSlamObj预测车辆的状态。的预测方法通过控制输入、其他必要的输入和当前车辆构成的StateTransitionFcn。

为数= 1:numSamples预测(ekfSlamObj controllerInput(计数,:),步伐);

具有里程碑意义的提取

这个示例使用观察地标从数据集的列表,所以你不需要使用一个传感器从环境中提取的地标。前面的地标是一个半圆形的区域。半圆形的区域的半径定义的最大射程的传感器。

observedLandmarks ={}计数测量;

修正

的ekfSLAM对象纠正国家基于给定的测量,返回一个列表匹配的地标和新地标。的正确的方法使用函数中指定DataAssociationFcn的属性ekfSlamObj把观察到的地标或测量已知地标。关联的数据关联函数返回一个列表,列表的新地标。的正确的方法使用关联更新车辆的信念和地图使用之间的相关性观察到的地标和已知的地标。此外,正确的的位置和协方差方法增加任何新的里程碑状态和StateCovariance向量,分别。

如果启用了生活情节选项,更新图扫描的当前时间。

如果~ isempty (observedLandmarks)正确(ekfSlamObj、observedLandmarks measCovar, validationGate);%更新图中修正车辆的位置如果plotOnTheRun exampleHelperUpdateScans (ekfSlamObj.State…ekfSlamObj.MaxAssociationRange,…observedLandmarks,…sensorHandle observationHandle);结束结束

使用drawnow更新图与车辆的当前位置和所有已知的地标和它们的协方差。

如果生活情节没有启用并等待酒吧是活跃的,然后更新执行的数据集的一部分。

如果plotOnTheRun addpoints (deadRecHandle deadReckoning(计数,1),deadReckoning(统计,2));addpoints (estTrajHandle ekfSlamObj.State (1) ekfSlamObj.State (2));exampleHelperUpdateRobotAndLandmarks (ekfSlamObj.State ekfSlamObj.StateCovariance,…robotHandle、covarianceHandle landmarkHandle);drawnowlimitrate其他的%更新等酒吧waitbar(数/ numSamples waitBarHandle,…sprintf (“% 2.2 f % %执行”、数/ numSamples * 100));%检查点击取消按钮如果getappdata (waitBarHandle“取消”)打破结束结束

结束

使用删除函数来关闭等酒吧一次所有的计算都是完整的。

如果~ plotOnTheRun删除(waitBarHandle);结束

可视化地图

创建一个地理图从GPS数据与地面真理。车辆轨迹,连同所有检测到的地标和它们相关的协方差,在同一地理图。

%得到纠正和预测[corrPoses, predPoses] = poseHistory (ekfSlamObj);%显示地图exampleHelperShowMap (ekfSlamObj.State ekfSlamObj.StateCovariance,…gpsLatLong、corrPoses predPoses);