扩展卡尔曼滤镜(EKF)同步本地化和绘图(SLAM)是机器人在构建未知环境地图的同时同时确定机器人在其中位置的一种技术. EKF-SLAM的关键组成部分是共变矩阵,它代表了机器人估计状态和地图特征的不确定性. 本条为计算EKF-SLAM内部的共变矩阵提供了一步步的过程.

共变矩阵初始化

过程从初始化共变矩阵开始,一般表示为P. 这个矩阵结合了机器人姿势和地图特征的不确定性,初始共变反映了对机器人起始位置和已知特征的初始信心.

预设步骤

在预测阶段,机器人的运动模型被用来估计新状态. 共变矩阵使用运动模型的雅各比亚语进行更新,表示为F,过程噪声共变,Q. 更新如下: 共变矩阵是使用动模型的雅各比亚语进行更新的,表示为F,过程噪声共变,Q.

P pred =FP F]T+Q]]]

更新带有测量的步数

收到新的传感器测量后,会更新共变矩阵以纳入这些信息. 测量模型的雅各比亚,H,和测量噪声共变R,用于计算卡尔曼增益,K:

K = P pred H ] (H P ]] pred ] + R] ]-1 ]]]]]]]]

共变矩阵随后更新为:

P]]]]]]] =(I-K H) P]]]] pred ]]]

包含地图特征

共变矩阵扩展至包括地图特征,随着新增特征的加入,大小会增加,每次更新都会调整机器人姿势和特征相关的不确定性,保持对地图和本地化中整体不确定性的一致估计.