Kalman滤波及其在SLAM中应用.ppt

1、卡尔曼滤波及其在SLAM中的应用，康丹、提纲、卡尔曼滤波的简单例子，扩展卡尔曼滤波SLAM问题，准备知识，状态空间模型：在控制领域，用输入向量、状态向量和输出向量描述系统的方法称为状态空间模型。下面的等式表示时间上的状态向量、时间上的输入向量和时间上的输出向量。a代表状态矩阵，B代表控制矩阵或输入矩阵，C代表观察矩阵，D代表输入和输出矩阵。卡尔曼滤波器，背景介绍：卡尔曼，匈牙利数学家。卡尔曼滤波源于他的博士论文和1960年发表的线性滤波和预测问题的新方法。介绍了一种离散控制系统的控制模型：将系统的观测变量定义为Z，得到测量方程：表示时间的状态向量、表示时间的输入向量和表示时间的观测向量分别为过

2、程激励噪声和观测噪声，它们是独立的正态分布的白噪声，协方差分别为Q，R(这里， 假设它们不随系统状态的改变而改变)，卡尔曼滤波器，假设当前系统状态是，则有可能在下一时刻预测k 1的状态：更新状态的协方差矩阵：其中它是过程激励噪声的协方差。卡尔曼滤波器，结合观测值来更新状态估计，k 1处的状态被获得如下：其中，它被称为新息。是卡尔曼增益，它的计算公式是：其中是观测噪声的协方差。为了使卡尔曼连续迭代，需要在k时刻更新状态协方差：时间更新和测量更新是重复迭代的，这是卡尔曼最吸引人的特性。举一个简单的例子，考虑房间中的温度对象过程：也就是说，过程的状态不随时间改变，并且没有控制输入；含有噪声的观测值是

3、状态变量的直接体现。然后，卡尔曼滤波的五个方程如下：简单的例子，假设室温是恒定的，k时间是23度(估计值)，并且这个值的协方差被假设为5度(在前一时间更新的协方差是3度，并且噪声是2度)。房间里有一个温度计，它的值显示25度，这个值的偏差(噪音)是2度。然后卡尔曼增益为：K 1处的实际估计值为：此时的更新协方差为(即，高3度的计算公式)，卡尔曼滤波器被扩展。可以看出，卡尔曼滤波器估计线性随机系统的状态。然而，在实践中，许多系统是非线性的。当处理这些系统时，使用扩展卡尔曼滤波器(EKF)，它使期望和方差线性化。控制对象的状态空间表示为：扩展卡尔曼滤波器有五个方程：扩展卡尔曼滤波器，A是F到X的偏

4、导数的雅可比矩阵，即W是F到W的偏导数的雅可比矩阵，即H到X的偏导数的雅可比矩阵，即V是H到V的偏导数的雅可比矩阵，即SLAM问题， 有时也被称为CML(并发映射和定位)问题陈述：自主移动机器人从未知环境中的未知点移动，经过一系列未知特征点，并获得从这些点到机器人的距离的一组测量数据。 这组测量数据由机器人头部的测距传感器测量。SLAM处理的目标是根据这些测量数据得到估计的环境地图，并利用该地图实现机器人定位。SLAM问题，在SLAM中，机器人的运动模型是已知的，即运动的角速度和线速度可以估计机器人的位置；可以测量机器人和特征点之间的测量值，也就是说，可以知道测量值。在SLAM中，根据EKF，

5、机器人的运动位置用每个状态的测量值更新，然后从机器人的位置和测量值获得特征点的坐标。SLAM问题，在时间k，机器人状态可以表示为Xv(k)，表示机器人状态，而Xlm(k)，不随时间变化，可以写成Xlm。观察值是描述机器人和特征点之间的距离的函数，其中它是特征点1和机器人之间的距离以及机器人和特征点之间的夹角。EKF-SLAM算法：(1)初始化机器人的位置Xv(0)和协方差矩阵P(0)，它们都可以是0。(2)预测(3)观测标志特征值预测值的新息误差协方差矩阵为：0，1，k SLAM问题，(4)观测特征点与数据关联检验中的k 1否则，观测值被丢弃。(5)更新，SLAM问题，(6)返回，(2)递归执行，(2)-(5)