在EKF-SLAM（基于特征的地图）中，一旦机器人感应到新的地标，它就会被增强为状态向量。结果，状态向量和协方差矩阵的大小被扩展。我的问题是关于新界标的不确定性及其与其他协方差矩阵对的相关性。我应该如何分配它们？当我将它们指定为零时，随着时间的流逝，此界标的估计误差不会改变。如果我给它们分配非常大的值，则每当机器人重新观察此界标时，估计值都会越来越好，但是，误差接近固定值而不是零。我假设问题ID与分配不确定性有关。有什么建议吗？

#1 楼

您可以使用逆观察模型的雅可比行列式来初始化协方差矩阵的新行/列。

假设您的观察模型是$ g（\ mathbf {x}）$，它将您的状态$ \ mathbf {x} $映射到预测的观察值$ \ hat {\ mathbf {z}} $ 。逆观测模型$ g ^ {-1}（\ mathbf {x}，\ tilde {\ mathbf {z}}）$将观测$ \ tilde {\ mathbf {z}} $映射到您的州的新条目。例如，如果$ \ tilde {\ mathbf {z}} $是范围和方位测量值，则在给定当前位置的情况下，$ g ^ {-1} $可能会确定新观测到的地标的全局$（x，y）$坐标。状态估计。

让$ \ mathbf {P} $为您州的协方差，并让$ \ mathbf {G} _x $和$ \ mathbf {G} _z $为$ g的雅可比行列式关于$ \ mathbf {x} $和$ \ tilde {\ mathbf {z}} $的^ {-1} $。然后，在添加新条目后的更新状态和协方差矩阵为

$ \ mathbf {x} \ gets \ begin {bmatrix} \ mathbf {x} \\ g ^ {-1}（\ mathbf {x}，\ tilde {\ mathbf {z}}）\ end {bmatrix}，\ qquad \ mathbf {P} _ {k + 1} = \ begin {bmatrix} \ mathbf {P} _k和\ mathbf {P } _k \ mathbf {G} _x ^ \ top \\ \ mathbf {G} _x \ mathbf {P} _k＆\ mathbf {G} _x \ mathbf {P} _k \ mathbf {G} _x ^ \ top + \ mathbf {G} _z \ mathbf {R} _k \ mathbf {G} _z ^ \ top \ end {bmatrix} $，

其中$ \ mathbf {R} _k $是方差（1D）或协方差$ \ mathbf {z} $。

评论

$ \ begingroup $
谢谢您的回答。我实际上在这里joansola.eu/JoanSola/eng/course.html中找到了解决方案。我认为您的答案应该修改为与上述链接中所述的pdf一致。
$ \ endgroup $
– Croco
2014-12-16 22:10


#2 楼

我没有专门的SLAM经验，但是我有很多EKF经验，所以这是我的意见。

坚持使用第二种方法。误差接近非零稳态值是正常的。可以通过分析得出名义上的数字，但是我现在不记得数学了，一旦开始将协方差与所有其他界标包括在内，它可能太复杂了。

不必过多担心应将协方差初始化为哪个特定值。在最简单的情况下，只需根据您的传感器噪声水平将方差设置为某个标称值，并与其他界标使用0协方差即可。随着时间的流逝，EKF更新步骤应将其调整为“适当的”值（假设您的初始值至少在标准范围内）。