Kalman滤波及其在SLAM中应用-25页PPT文档

Kalman滤波及其在SLAM中应用康丹Outline•Kalman滤波•简单实例•扩展kalman滤波•SLAM问题准备知识•状态空间模型：在控制领域中，用输入向量、状态向量和输出向量描述系统的一种方法，叫状态空间模型。如下式••表示时刻的状态向量，表示时刻的输入向量，表示时刻的输出向量，•A表示状态矩阵，B为控制矩阵或者输入矩阵，C为观测矩阵，D为输入输出矩阵。(1)()()()()()xkAxkBukykCxkDuk)(kx)(ku)(kykkkKalman滤波•背景介绍：•Kalman，匈牙利数学家。•卡尔曼滤波器源于他的博士论文和1960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）。kalman滤波•引入一个离散控制系统的控制模型：••定义系统的观测变量为z，得到测量方程为：••其中，表示时刻的状态向量，表示时刻的输入向量，表示时刻的观测向量，•分别为过程激励噪声和观测噪声，它们为相互独立、正态分布的白噪声，协方差分别是Q，R（这里假设它们不随系统状态变化而变化）)(kx)()()()1(kwkBukAxkx)()()(kvkHxkzk)(kuk)(kzk)()(kvkw、kalman滤波•kalman滤波第一步：•用系统的过程模型来预测下一状态的系统。假设现在的系统状态是，可以预测下一时刻k+1出现的状态：••更新状态的协方差矩阵：•其中，为过程激励噪声协方差。(1|)(|)()xkkAxkkBuk)|1(kkx)(kxQAkkAPkkPT)|()|1(Qkalman滤波•结合观测值更新状态估计，得到k+1时刻状态为：•其中，称为新息。•为kalman增益，其计算公式为：•式中，为观测噪声协方差。))|1()1(()|1()1|1(kkHxkzKkkxkkxggK))|1(()|1(RHkkHPHkkPKTTg)|1()1()1(kkHxkzkVRkalman滤波•为了令kalman不断迭代下去，需要更新K时刻状态的协方差：•时间更新和测量更新不断重复迭代，就是kalman最吸引人的特性。•)1|1(kkx)|1()()1|1(kkPHKIkkPg)|1()()1|1()3())|1()1(()|1()1|1()1()2())|1(()|1(11kkPHKIkkPkkHxkzKkkxkkxkzRHkkHPHkkPKkalmanggTTg更新测量误差更新估计由观测变量增益）计算（测量更新（校正）QAkkAPkkPkBukkAxkkxT)|()|1()2()()|()|1()1(向前推算误差协方差向前推算状态变量时间更新（预测）为初始估计和)|()|(kkPkkx简单实例•考虑房间内温度对象过程:•即过程的状态不随时间变化，没有控制输入；包含噪声的观测值是状态变量的直接体现。•那么，kalman滤波的5个方程分别为：••(1)()()()()()xkxkwkzkxkvk)|1()()1|1())|1()1(()|1()1|1())|1()(|1()|()|1()|()|1(1kkPKIkkPkkxkzKkkxkkxRkkPkkPKQkkPkkPkkxkkxggg简单实例•假设房间温度不变，k时刻为23度（估计值），该值的协方差假设为5度（上一时刻更新的协方差为3度，噪声为2度）。•房间内有一温度计，其值显示25度，该值的偏差（噪声）为2度。•则kalman增益为：•K+1时刻的真实估计值为：•该时刻更新协方差为（即为上边3度的计算式）•71.0)25/(5gK42.24)2325(*71.023)1|1(kkx45.15)71.01()1|1(kkP扩展卡尔曼滤波•可以看出，卡尔曼滤波估计是一个线性随机系统的状态。然而实际中，很多系统是非线性的，处理这些系统时，用扩展卡尔曼滤波(EKF),它是将期望和方差线性化的卡尔曼滤波器。•控制对象的状态空间表述为:))(),(),(()1(kwkukxfkx))(),(()(kvkxhkz扩展卡尔曼滤波•扩展卡尔曼滤波的5个方程为：)|1()()1|1()3()))|1(()1(()|1()1|1()1()2())|1(()|1(11kkPHKIkkPkkxhkzKkkxkkxkzVRVHkkHPHkkPKkalmanggTTTg更新测量误差更新估计由观测变量增益）计算（测量更新（校正）TTWQWAkkAPkkPkukkxfkkx)|()|1()2()0),(),|(()|1()1(向前推算误差协方差向前推算状态变量时间更新（预测）为初始估计和)|()|(kkPkkx扩展卡尔曼滤波•A是f对x的偏导的雅可比矩阵，即•W是f对w的偏导的雅可比矩阵，即•H是h对x的偏导的雅可比矩阵，即•V是h对v的偏导的雅可比矩阵，即••][][],[iijixfA][][],[iijiwhH][][],[iijiwfW][][],[iijivhVSLAM问题•SLAM：Simultaneouslocalizationandmapping自主定位和地图创建。有时也称为CML（concurrentmappingandlocalization）•问题陈述：自主移动机器人从未知环境中一个未知点开始运动，经过一系列未知特征点，并得到这些点到机器人距离的一组测量数据。这组测量数据由机器人头部的测距传感器测得。•SLAM处理的目标是根据这些测量数据得到环境的估计地图，并用这张地图实现机器人的定位。SLAM问题•在SLAM中，机器人运动模型已知，即运动角速度和线速度，可以估计机器人的位置；机器人与特征点之间的测量值可以测出，即知道测量值。在SLAM中，根据EKF，用每一状态的测量值来更新机器人的运动位置，然后由机器人位置和测量值求出特征点的坐标。SLAM问题•K时刻，机器人状态可表示为•Xv(k)表示机器人的状态，Xlm（k）为路标的状态，它不随时间变化而变化,可以写成Xlm。•观测值是描述机器人和特征点距离的函数，则•其中，为特征点i与机器人之间的距离，•为机器人与特征点位置的夹角。)(ki])()([))()()(()(])()([kykxXkkykxkXkXkXXlmlmlmTVehlmv其中，TiiikkkZ)](),([)()(kiSLAM问题•EKF-SLAM算法：•(1)初始化机器人的位置Xv(0)和协方差阵P(0)，可以令其都为0.•(2)预测•(3)观测值•路标特征值的预测值的新息为•新息的误差协方差矩阵为：•)()|()|1())(),|(()|1(kQAkkAPkkPkukkXfkkXTVV))|1(()1()1(kkxhkzkVRHkkHPkST)|1()1(SLAM问题•(4)数据关联•检验k+1时观测到的特征点与0,1,…k时观测到的特征点是否关联上，根据下式证明时刻观测到的一组特征点确实是观测到的，其中G为常数；否则，舍弃观测值。••(5)更新••GkVkSkVT)1()1()1()|1()()1|1()))|1(()1(()|1()1|1())|1(()|1(1kkPHKIkkPkkxhkzKkkxkkxRHkkHPHkkPKkalmanggTTg协方差矩阵更新机器人状态：增益SLAM问题•(6)返回(2)递归执行(2)-(5).••该算法在matlab上仿真执行结果为:SLAM问题•目前问题：•EKF-SLAM一致性和算法有效性:•当环境特征较多时，出现偏移现象，且计算时间太长。0100200300400500050100150200250300350400450500metresmetres0100200300400500050100150200250300350400450500metresmetres•谢谢大家！谢谢你的阅读知识就是财富丰富你的人生