基于ekf的Landmark SLAM

打开实时脚本

这个例子展示了如何使用ekfSLAM使用扩展卡尔曼滤波器(EKF)算法和最大似然算法进行数据关联，实现可靠的地标同步定位和映射(SLAM)。在本例中，您将创建车辆周围环境的地标地图，并同时跟踪车辆的路径。通过使用噪声控制命令移动车辆来生成轨迹，并使用它在路径上遇到的地标形成地图。通过再次观察地标来修正车辆轨迹和地标估计。

负载数据集

加载维多利亚公园数据集的修改版本，其中包含控制器输入、测量、GPS纬度和经度，以及使用控制器输入和运动模型生成的航位推算。

负载(“victoriaParkDataset.mat”，“controllerInput”，.．.“测量”，“gpsLatLong”，“deadReckoning”）;

设置参数

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

initialState = [gpsLatLong(1,2) gpsLatLong(1,1) deg2rad(37)]';initialCovar = eps*eye(3);

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

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

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

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

指定检查关联地标的最大范围。

maxSensorRange = 30;% [m]

指定车辆移动的时间步长。

timeStep = 0.025;%(秒)

指定数据关联功能的阈值，nav.algs.associateMaxLikelihood．地标性拒绝阈值为卡方( $χ^{2}$ )分布表的值为95%正确的关联。

landmarkRejectionThres = 5.991;%最大关联距离landmarkAugmentationThres = 200;%新建地标的最小距离validationGate = [landmarkRejectionThres landmarkAugmentationThres];

设置一个标志，该标志决定在运行期间是否绘制映射。

plotOnTheRun = false;

从数据集中得到样本的数量。

numSamples = size(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。

为count = 1:numSamples predict(ekfSlamObj,controllerInput(count，:)，timeStep);

具有里程碑意义的提取

本例使用从数据集中观察到的地标列表，因此不必使用传感器从环境中提取地标。这些标志来自车辆前方的半圆形区域。半圆形区域的半径由传感器的最大范围定义。

observedLandmarks =测量{计数};

修正

的ekfSLAM对象根据给定的测量值纠正状态，返回匹配的地标列表和新地标列表。的正确的方法中指定的函数DataAssociationFcn的属性ekfSlamObj将观测到的地标或测量值与已知的地标联系起来。数据关联函数返回关联列表和新地标列表。的正确的方法使用关联来更新车辆和地图的信念，使用观测到的地标和已知地标之间的相关性。此外,正确的方法增加任何新地标的位置和协方差状态而且StateCovariance向量,分别。

如果启用了实时绘图选项，则使用当前时间的扫描更新图形。

如果~ isempty (observedLandmarks)正确(ekfSlamObj、observedLandmarks measCovar, validationGate);更新图中车辆的修正位置如果plotOnTheRun exampleHelperUpdateScans (ekfSlamObj。状态,.．.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数据中创建一个真实的地理图。在相同的地理图上绘制车辆轨迹，以及所有检测到的地标及其相关的协方差。

得到修正和预测的姿势[corrpose, predpose] = poseHistory(ekfSlamObj);%显示地图exampleHelperShowMap (ekfSlamObj.State ekfSlamObj.StateCovariance,.．.gpsLatLong、corrPoses predPoses);