机器人智能算法导论 课件第8章-线性卡尔曼滤波器

《机器人智能算法导论》

配套课件本节简介能力目标：能够构建机器人运动系统的高斯分布状态表示通过公式推导演示卡尔曼滤波迭代过程知识目标：掌握卡尔曼滤波器的基本假设及其数学表达理解预测步骤与更正步骤中均值/协方差计算公式的推导过程掌握线性观测模型与过程噪音建模方法本讲纲要引言联合高斯分布的边缘分布联合高斯分布的条件分布卡尔曼滤波器的假设线性卡尔曼滤波器线性卡尔曼滤波器的完整算法8.1引言贝叶斯滤波器：一种基于贝叶斯定理的概率滤波器，其在时间上递归地对先验信息和测量信息进行融合，从而实现对系统状态的在线估计。卡尔曼滤波器：贝叶斯滤波器的基础上，通过假设系统的状态方程和测量方程都是线性的，并且假设噪声服从高斯分布，从而推导出了一种特定的递归更新算法，用于估计具有线性状态方程和线性测量方程的线性系统的状态。这种算法通过预测步骤和更正步骤交替进行，通过状态估计和协方差估计来实现对系统状态的估计。贝叶斯滤波器和卡尔曼滤波器的概念8.1引言卡尔曼滤波器的主要目标：估计系统的隐藏状态通常被建模成多元高斯分布。在预测步骤：卡尔曼滤波器假设噪声服从高斯分布。在更正步骤：卡尔曼滤波器假设观测数据的误差服从高斯分布。作用：引入了高斯分布，并利用了高斯分布良好的数学性质，使得卡尔曼滤波器成为一个在时间和空间复杂度上可接受的实际滤波器。应用：这种基于高斯分布的状态估计方法使得卡尔曼滤波器在许多实际应用中表现出色，如导航、目标跟踪、机器人控制等。卡尔曼滤波器和高斯分布有密切的关系8.2联合高斯分布的边缘分布假设

是一个随机向量，其高斯分布为联合高斯分布s所服从的高斯分布的概率密度函数p(s)：联合高斯分布的边缘分布：随机向量sa和sb协方差矩阵：分布形式：8.2联合高斯分布的边缘分布联合高斯分布的边缘分布是高斯分布联合高斯边缘分布的均值和协方差公式为：推导目标是要证明以下结论：8.2联合高斯分布的边缘分布多变量高斯分布概率密度函数的一般形式：简化后：证明联合高斯分布的边缘分布是高斯分布：8.2联合高斯分布的边缘分布如果联合高斯边缘分布的概率密度函数的形式和这个形式一致，那么就可以说边缘分布是高斯分布。如果能证明联合高斯边缘分布是高斯分布，那么这个分布可由均值向量和协方差矩阵完全确定。一旦获得边缘分布的概率密度函数的一般形式，那么可以直接获得参数。第四步新定义的协方差逆矩阵恢复为协方差矩阵形式第二步定义联合高斯分布的协方差矩阵逆矩阵，并进行分块第三步将积分形式的高斯分布中的条件项消除第一步给出联合高斯条件分布的概率密度函数证明步骤随机变量sa的边缘分布的积分形式:根据随机向量一阶幂矩的定义，随机向量s的数学期望:把随机变量s拆分成两个子随机变量sa和sb，那么sa和sb的均值向量是s¯

相应子向量s¯a和s¯b:推导过程：8.2.1联合高斯边缘分布的积分形式随机向量

s

的协方差矩阵：联合高斯分布的分块形式：代入后：推导过程：8.2.1联合高斯边缘分布的积分形式8.2.2逆矩阵替代及分解为了便于推导，定义新矩阵形式：代入后：推导过程8.2.3边缘积分为了使推导过程比较简洁，首先定义：代入后：推导过程8.2.3边缘积分配方法具体公式：变换后：推导过程8.2.3边缘积分根据概率密度函数的性质，对这个概率密度函数进行积分，可以推导出如下公式代入后：推导过程8.2.4结论一旦断定这是一个高斯分布的概率密度函数，那么从函数可直接获得高斯分布的均值向量为，协方差矩阵为矩阵V为可逆矩阵，并且矩阵V与其逆矩阵B的分块矩阵满足如下形式：高斯边缘概率密度函数的归一化项Z：8.2.4结论联合高斯分布的边缘分布是高斯分布其均值向量和协方差矩阵分布为随机向量sa的均值向量和协方差矩阵联合高斯边缘分布的概率密度函数：8.3联合高斯分布的条件分布第四步新定义的协方差逆矩阵恢复为协方差矩阵形式第二步定义联合高斯分布的协方差矩阵逆矩阵，并进行分块第三步将积分形式的高斯分布中的条件项消除第一步给出联合高斯条件分布的概率密度函数将随机变量拆分成两个子随机向量：条件随机变量sa|sb和sb|sa

服从如下高斯分布：8.3.1联合高斯条件分布条件概率密度函数:推导过程8.3.2公式简化为了方便推导，我们将式分子中与sa无关的量归并到一个常数，即：进一步设定：推导过程8.3.2公式简化条件概率密度函数:其均值向量和协方差矩阵：8.3.3结论联合高斯分布的条件分布是高斯分布8.4卡尔曼滤波器的假设为了实现一个具体的贝叶斯滤波器，我们需要实现明确3个概率分布：系统状态的先验分布、系统状态的转移概率分布和观测数据概率分布。为此，卡尔曼滤波器在这3个概率分布上做了一定的限制和假设，假设先验分布和观测数据分布都是高斯分布，并且系统状态的转移和观测方程是线性的。在这些限制和假设的基础上，卡尔曼滤波器能够提供高效、准确的状态估计和预测。8.4.1变量定义重新定义系统状态、运动控制数据和观测数据的向量形式：假设任意时刻系统的状态是一个N×1的列向量，观测数据是一个M×1的列向量，运动控制是一个K×1的列向量定义系统从0时刻到t时刻的状态序列为

系统从1时刻到t时刻的观测数据序列为系统从1时刻到t时刻的运动控制数据序列为8.4.2先验分布假设系统状态的先验分布p(s0)蕴含我们对系统状态的先验知识和理解。卡尔曼滤波器假设系统的初始状态服从的高斯分布：由此也可以推出，先验分布的概率密度函数：通常，将系统在前一时刻状态的后验分布当作当前时刻的先验分布：8.4.3状态转移概率分布假设系统状态转移概率分布p(st|st−1,at)蕴含系统在受到一个运动驱动at后，从t−1时刻某个状态转移到t时刻某个状态的概率。状态转移过程通常由一个运动模型来表示：st的分布服从高斯分布：由此也可以推出，状态转移概率分布的概率密度函数：运动模型可以被认为是一个线性随机过程spt和sct分别表示t时刻预测步骤获得的状态和更正步骤获得的状态：8.4.4观测数据概率分布假设观测数据概率分布p(ot|st)蕴含系统在t时刻状态st已知的条件下观测到ot的概率。观测数据概率分布通常由一个观测模型表示：ot的分布服从高斯分布：由此也可以推出，观测数据概率分布的概率密度函数：观测模型可以认为是一个线性随机过程观测模型是基于预测步骤获得的系统状态来推测观测数据：8.4.5运动模型和观测模型构建实例在上述假设中，运动模型和观测模型的构建至关重要。为了便于理解上述概念，我们将设计一个配送机器人的实例来构建上述运动模型和观测模型。我们将配送机器人进行以下假设：1、配送机器人只在平面坐标系沿x轴运动；2、机器人的加速度为a，是常数；3、某个类似GPS的传感器可以随时测量机器人在x轴上的坐标位置；4、待估计的系统状态为t时刻机器人的位置和速度，即：8.4.5运动模型和观测模型构建实例定义配送机器人的运动学方程：引入噪声并做形式简化8.4.5运动模型和观测模型构建实例At定义了前一时刻系统状态对当前时刻系统状态的影响，这个影响是综合影响：前一时刻系统状态所有的分量都有可能对当前时刻系统状态特定分量有影响。综合影响也体现在任意一个外部力分量都可能对当前时刻系统状态的所有分量产生影响。结论：8.5线性卡尔曼滤波器预测步骤更正步骤初始化贝叶斯滤波器核心公式：8.5线性卡尔曼滤波器贝叶斯滤波器涉及5个概率分布：t−1时刻系统的后验分布，统在t时刻执行预测步骤时，该分布被作为先验分布使用：t−1时刻到t时刻系统状态的转移概率分布：当前时刻系统状态转移之后的概率分布：由t时刻系统状态产生t测量结果的概率分布：t时刻系统状态的后验分布：8.5线性卡尔曼滤波器不能编程实现的原因：贝叶斯滤波器没有具体定义这5个概率分布是什么在预测公式中，没有给出具体的求解方法在更新公式中，没有给出具体的求解方法为了构造一个可实现的贝叶斯滤波器，卡尔曼滤波器进行了一系列的假设，即先验分布假设、状态转移概率分布假设和观测数据概率分布假设。基于这些假设，卡尔曼滤波器构造了预测步骤和更正步骤的解析解。8.5.1预测步骤预测步骤核心公式：卡尔曼滤波器的预测步骤的目标是根据运动模型来从前一时刻系统状态分布和一个系统动作获得当前时刻系统状态的概率分布。卡尔曼滤波器的预测公式（t时刻预测的系统状态的均值和协方差）：8.5.1预测步骤预测步骤推导过程：预测步骤t时刻系统状态所属高斯分布的均值：预测步骤t时刻系统状态所属高斯分布的协方差矩阵：8.5.2更正步骤更正步骤核心公式：卡尔曼滤波器的更正步骤的目标是基于条件ot来更新spt的分布，即获得后验分布。卡尔曼滤波器的更正公式（t时刻预测的系统状态的均值和协方差）：完整公式：8.5.3线性卡尔曼滤波器图示ThePrediction-Correction-CyclePredictionCorrectionThePrediction-Correction-Cycle8.5.3线性卡尔曼滤波器图示CorrectionPrediction8.5.3线性卡尔曼滤波器图示8.6线性卡尔曼滤波器的完整算法运动模型是线性高斯模型，即当前时刻状态与前一时刻状态之间是线性变换关系。这个假设保证，如果状态sct−1是高斯随机变量，那么由状态sct−1经过线性变换得到的状态