这就是无噪音版本的运动模型了。也就是我们在卡尔曼滤波代码时要用到的模型,用于上一时刻到下一时刻的状态估计。加上噪音的运动模型比较复杂,我们就不写在这里了。通过上一讲我们应该了解到在实施卡尔曼滤波的时候我们写入代码的运动模型就是没有噪音的版本。
有了运动模型,即有了扩展卡尔曼滤波五步中的第一步,接下来就需要进行卡尔曼滤波的第二步,获得运动模型的雅各比矩阵,通过下面的式子(第二篇的式(6))获得。
对应于此处
那么你带入求导就可以得到
其中是3x3的单位矩阵。在扩展卡尔曼滤波的第二步我们还有另外两个量需要确定,一个是误差协方差矩阵,另一个是过程噪音矩阵。在设置了之后,就可以根据算法迭代一直更新下去,我们则根据卡尔曼滤波的表现来设置为一个值固定的矩阵。正如前一讲所说,这两个值都靠猜(根据滤波器的表现,比如根据真实值和滤波器的估计值之间的差别来调整这两个矩阵,这是很麻烦的事情)。我们后面写代码时直接给出这两个值。
接下来是卡尔曼滤波的第三步更新卡尔曼增益,这一步里涉及到三个变量来自于第二步,测量噪音协方差矩阵根据传感器提供的测量误差等信息进行更合理一些的猜测。而观察(测量)模型的雅各比矩阵则需要我们先对观察模型有一个了解。我们测量的是路标相对于机器人的角度和距离。
路标为固定在环境中的点,维度为2,即它的横纵坐标,用表示,为坐标点的序号。根据机器人自己的位置和角度,我们可以很容易获得它相对于某一个路标的角度和距离
矩阵的第一行明显就是距离了,第二行其实就是直角三角形两个非斜边的比值再求个逆三角函数得到角度,最后减去机器人原来的角度得到相对角度。至于为什么用(arctan2)而不是自行百度。
无噪音版本的测量模型对应到卡尔曼滤波中,就应该是
式子中的来自于式4。注意这些符号都加了帽子符号''是因为他们都是估计值。
为了书写简便,令
代入6式则有
接下来就是要求测量的雅各比矩阵了。我们需要把它同时对机器人的状态和路标(landmark)的位置求导。这几个是我们需要知道的变量。即和分别对这5个量求导,求得一个2X5的矩阵。过程就不写了,用链式法则很容易求
前三列为对机器人状态量求导,后两列为对路标求导。
至此,有运动模型,测量模型以及雅各比矩阵,回顾第二讲最后的完整EKF的式子,只要我们设置了误差协方差矩阵的初始值,卡尔曼滤波便可以根据那5个公式一步一步迭代下去了。
但是呢,其实还有一些事情没做。在SLAM里,我们想要测量的量不仅仅是机器人的位置和角度,我们也想要测量路标的位置。所以说真正的需要更新的状态量是
其中代表目前观察到的路标的总数量。所以其实状态量不是3个而是。注意probabilistic的书里(2005版本)状态量维度是,这是因为它给每个路标都考虑了一个额外的变量(177页,6.62以及313页10.2.2),比如路标的颜色不同,形状不同等等,这里为了简便我们不考虑那个量。这里符号的标注有些复杂了,望大家弄懂每个角标的意思。这里冗余地重复一下。以为例,表示当前路标的标号,是当前路标的横坐标,表示当前路标在卡尔曼滤波中第步最终得到的更新值。
这样EKFSLAM中的过程模型就该写为
简写为
过程模型的前三个量也就是机器人的状态模型的更新就是式子4。其余的路标的位置在这一步不更新,因为我们的假设是路标不会动的。与此对应的雅各比矩阵为
其中为式子5,表示行,列的单位矩阵。求导数的过程仍然按照第二篇文章的式子6。读者可以自行求导,不会的可以私信。其实就是在原来的基础之上多了路标的求导,路标位置本身和机器人位置没有关系,对求导为0,某个路标位置和其他路标没有关系,只有对自身求导才得到1,对其他路标求导都是0.
接下来同样,由于我们当前观察到的路标有个,所以类似于式子7的雅各比矩阵应该有个。式6的测量模型更完整的写法就应该是
该式子有2n行。把上式对状态量式8求导,可以得到完整的测量雅各比矩阵,维数为。
这个式子看起来贼复杂。其实就是公式12对式8求导。表示1行列的0向量。每一个路标的测量对机器人状态都是有导数的,所以式13的前三列都有数值,而路标只有对自身才有导数,对其他路标导数为0(他们没有任何关系)。
至此,我们完成了EKF-SLAM所需要的测量模型,过程模型以及相关的雅各比矩阵。
(未完待续....这公式写地怀疑人生)
评论(0)
您还未登录,请登录后发表或查看评论