卡尔曼 滤波器 （Kalman Filter） 滤波的基本概念 l滤波是什么？ 所谓滤波，就是从混合在一起的诸多信号中提取出所需要的信号。 l信号的分类（数学关系）？ （1）确定性信号：可以表示为确定的时间函数，可确定其在任何 时刻的量值。（具有确定的频谱） （2）随机信号：不能用确定的数学关系式来描述的，不能预测其 未来任何瞬时值，其值的变化服从统计规律。（频谱不确定，功 率谱确定） 滤波的基本概念 l确定性信号的滤波 可采用低通、高通、带通、带阻等模拟滤波器或者计算机通 过算法实现常规滤波 l随机信号的滤波 根据有用信号和干扰信号的功率谱设计滤波器维纳滤波（ Wiener Filtering）或卡尔曼滤波（Kalman Filter） 随机信号的滤波也可以看做是估计问题。 卡尔曼滤波的由来 卡尔曼滤波的由来 卡尔曼，全名Rudolf Emil Kalman， 匈牙利数学家，1930年出生于匈牙利 首都布达佩斯。1953，1954年于麻省 理工学院分别获得电机工程学士及硕 士学位。1957年于哥伦比亚大学获得 博士学位。我们在现代控制理论中要 学习的卡尔曼滤波器，正是源于他的 博士论文和1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预 测问题的新方法）。 卡尔曼滤波的由来 卡尔曼滤波的由来 卡尔曼滤波理论作为最优估计的一种，它的创立是科学技术和 社会需要发展到一定程度的必然结果。在1795年，高斯为测定行星 运动轨道而提出最小二乘估计法。为了解决火力控制系统精度跟踪 问题，维纳于1942年提出了维纳滤波理论，利用有用信号和干扰信 号的功率谱确定线性滤波器的频率特性，首次将数理统计理论与线 性理论有机的联系在一起，形成了对随机信号做平滑、估计或者预 测的最优估计新理论。但是采用频域设计法是造成维纳滤波器设计 困难的根本原因。于是，人们逐渐转向寻求在时域内直接设计最优 滤波器的方法，而卡尔曼研究的卡尔曼滤波理论很好的解决了这个 问题 卡尔曼滤波器是什么？ 卡尔曼滤波器是什么 简单的说，卡尔曼滤波器是一个“optimal recursive data processing algorithm (最优化自回归数据处理算法)”。从形 式上，卡尔曼滤波器是5条公式。 对于解决很大部分的问题，他是最优，效率最高甚至 是最有用的。他的广泛应用已经超过了30年，包括机器人 导航、控制，传感器数据融合甚至在军事方面的雷达系统 以及导弹追踪等等。而近年来更被应用于计算机图像处理 ，例如头脸识别、图像分割、图像边缘检测等等。 卡尔曼滤波的特点 卡尔曼滤波的特点 （1）卡尔曼滤波处理的对象是随机信号； （2）被处理的信号无有用和干扰之分，滤波的目的是要估计出 所有被处理的信号(区别于维纳滤波)； （3）系统的白噪声激励和测量噪声并不是需要滤除的对象，它 们的统计特性是估计过程中需要利用的信息；（区别最小二乘） （4）算法是递推的，且使用状态空间法在时域内设计滤波器， 适用于对多维随机过程的估计； （5）被估计量既可以是平稳的，也可以是非平稳的； （6）估计过程中，只需要考虑过程噪声和测量噪声及当前时刻 系统状态的统计特性。（计算机计算时，所占空间小） 思路 思路 Part 1 线性系统的卡尔曼滤波方程 I.线性离散系统 II. 线性连续系统 Part 2 非线性系统的卡尔曼滤波方程 I.扩展卡尔曼滤波器 EKF II. 无迹卡尔曼滤波器 UKF 卡尔曼滤波的基本思想 卡尔曼滤波的基本思想 在海图作业中，航海长通常以前一时刻的船位为基准，根据航向 、船速和海流等一系列因素推算下一个船位，但是他并不轻易认 为船位就一定在推算船位上，还要选择适当的方法，通过仪器得 到另一个推算船位。观测和推算这两个船位一般不重合，航海长 需要通过分析和判断选择一个可靠的船位，作为船舰当前的位置 。 以 时刻的最优估计 为准，预测 时刻 的状态变量 ，同时又对该状态进行观测，得到 观测变量 ，再在预测与观测之间进行分析，或者 说是以观测量对预测量进行修正，从而得到 时刻的 最优状态估计 。 卡 尔 曼 滤 波 思 想 例子 假设我们要研究一个房间的温度，以一分钟为时间单位。根 据我们的经验判断，这个房间的温度是恒定的，但是对我们 的经验不是完全相信，可能存在上下几度的偏差，我们把该 偏差看做是高斯白噪声。另外，我们在房间里放一个温度计 ，温度计也不准确，测量值会与实际值存在偏差，我们也把 这偏差看做是高斯白噪声。现在，我们要根据我们的经验温 度和温度计的测量值及它们各自的噪声来估算出房间的实际 温度。 卡尔曼滤波的基本方程 例子 假如我们要估算 k 时刻的实际温度值。首先你要根据 k-1 时刻 的温度值，来预测 k 时刻的温度（K时刻的经验温度）。因为 你相信温度是恒定的，所以你会得到 k 时刻的温度预测值是跟 k-1 时刻一样的，假设是 23 度（*公式一），同时该值（预测 值）的高斯噪声的偏差是 5 度（5 是这样得到的：如果 k-1 时 刻估算出的最优温度值的偏差是 3，你对自己预测的不确定度 是 4 度，他们平方相加再开方，就是 5（*公式二） 。然后 ，你从温度计那里得到了 k时刻的温度值，假设是25 度，同时 该值的偏差是 4 度。 卡尔曼滤波的基本方程 例子 卡尔曼滤波的基本方程 现在，我们用于估算K时刻房间的实际温度有两个温度值：估计值 23度和测量值25度。究竟实际温度是多少呢？是相信自己还是相信 温度计？究竟相信谁多一点？我们需要用他们的均方误差来判断。 因为， （*公式三），所以我们可以估算出K时 刻的最优温度值为： 度（*公式四）。 得到了K时刻的最优温度，下一步就是对K+1时刻的温度值进行最 优估算，需要得到K时刻的最优温度（24.56）的偏差，算法如下： （*公式五） 就这样，卡尔曼滤波器就不断的把均方误差递归，从而估算出最优 的温度值，运行速度快，且只保留上一时刻的协方差。 无控制离散型卡尔曼滤波器的基本公式 系统的状态方程 ： 系统的测量方程 ： 为过程噪声 ； 为测量噪声 ; 如果满足 为过程噪声的协方差，其为非负定阵 ； 为测量噪声的协方差，其为正定阵。 系统测量方程的输出量是可以实际测量的量。 为噪声驱动阵 1 基于离散系统模型的卡尔曼滤波的基本公式 1.1 无控制的离散型卡尔曼滤波基本方程 无控制离散型卡尔曼滤波的基本方程 （1）状态的一步预测方程： （2）均方误差的一步预测： （3）滤波增益方程（权重）： （4）滤波估计方程（K时刻的最优值）： （5）均方误差更新矩阵（K时刻的最优均方误差）： 1 基于离散系统模型的卡尔曼滤波的基本公式 1.1 无控制的离散型卡尔曼滤波基本方程 带有控制的离散型卡尔曼滤波基本方程 系统的状态方程 ： 系统的测量方程 ： 如果满足 为过程噪声的协方差，其为非负定阵 ； 为测量噪声的协方差，其为正定阵。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.2 带有控制的离散型卡尔曼滤波基本方程 （1）状态的一步预测方程： （2）均方误差的一步预测： （3）滤波增益方程（权重）： （4）滤波估计方程（K时刻的最优值）： （5）滤波均方误差更新矩阵（K时刻的最优均方误差）： 1 基于离散系统模型的卡尔曼滤波的基本公式 1.2 带有控制的离散型卡尔曼滤波基本方程 带有控制的离散型卡尔曼滤波基本方程 线性离散型卡尔曼滤波方程的一般形式 系统方程和测量方程的一般形式： 如果满足 为过程噪声的协方差，其为非负定阵 ； 为测量噪声的协方差，其为正定阵。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.3 离散型卡尔曼滤波方程的一般形式 引入矩阵 ，对状态方程进行等效变换： 其中： 为过程噪声的协方差，其为非负定阵 ； 为测量噪声的协方差，其为正定阵。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.3 离散型卡尔曼滤波方程的一般形式 一般形式的卡尔曼滤波方程 （1）状态的一步预测方程： （2）均方误差的一步预测： （3）滤波增益方程（权重）： （4）滤波估计方程（K时刻的最优值）： （5）滤波均方误差更新矩阵（K时刻的最优均方误差）： 1 基于离散系统模型的卡尔曼滤波的基本公式 1.3 离散型卡尔曼滤波方程的一般形式 离散型卡尔曼滤波基本方程使用要点 （1）滤波初值的选取 卡尔曼滤波是一种递推算法，启动时必须先给初值 情况一：一般情况下，取 ，卡尔曼滤 波器是无偏的，即滤波稳定，但是实际上这样的初值很难得到 ； 情况二：如果系统是一致完全随机可控和一致完全随机可观测 的，则卡尔曼滤波器一定是一致渐近稳定的，此时盲目的选取 滤波初值不影响最终估计值（大多数情况下）。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.4 离散型卡尔曼滤波基本方程使用要点 离散型卡尔曼滤波基本方程使用要点 （2）估计均方误差的等价形式及选用 公式（1）形式简单，计算量小，但是积累误差容易使协 方差矩阵失去非负定性甚至对称性，所以实际中常使用 公式（2）； 如果在滤波初值对被估计量的统计特性缺乏了解，选取 滤波初值盲目，则宜采用公式（3）。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.4 离散型卡尔曼滤波基本方程使用要点 离散型卡尔曼滤波基本方程使用要点 （3）连续系统离散化 卡尔曼滤波的基本方程只适用于系统方程和测量方程均为 离散的情况，但实际的物理系统一般都是连续的，动力学特 性用连续微分方程来描述，所以在使用基本方程之前，需要 对系统方程和测量方程进行离散化处理。 连续系统的离散化处理包括对过程白噪声的等效离散化处 理。 1 基于离散系统模型的卡尔曼滤波的基本公式 1.4 离散型卡尔曼滤波基本方程使用要点 连续系统的卡尔曼滤波基本方程 通过对实际的物理系统进行分析后得到的系统模型一般为 连续型的。连续型卡尔曼滤波方程可在离散型卡尔曼滤波器基 本方程的基础上推导出来。基本思路：将连续系统离散化，应 用离散型卡尔曼滤波器的基本方程和导数概念推导出连续型滤 波方程。 采用递推算法是离散型卡尔曼滤波的最大优点，算法可由 计算机执行，不必存储时间过程中得大量测量信息。连续型卡 尔曼滤波则根据连续时间过程中的测量值，采用求解矩阵微分 方程的方法估计系统状态变量的时间连续值，因此算法失去了 递推性。 2 基于连续系统模型的卡尔曼滤波的基本公式 连续系统的状态空间表达式为： 为非负定矩阵 ； 为正定阵 其中： 2 基于连续系统模型的卡尔曼滤波的基本公式 连续系统模型的卡尔曼滤波基本方程 2 基于连续系统模型的卡尔曼滤波的基本公式 与连续系统模型等效的离散系统的数学模型： 连续系统模型的卡尔曼滤波基本方程 其中： 是零均值分段常值白噪声过程，其协方差为 ： 连续系统模型的卡尔曼滤波基本方程 引入矩阵J来去除过程噪声与测量噪声的相关性 （1）状态的一步预测方程： （2）均方误差的一步预测： （3）滤波增益方程（权重）： 连续系统模型的卡尔曼滤波基本方程 2 基于连续系统模型的卡尔曼滤波的基本公式 （4）滤波估计方程（K时刻的最优值）： （5）滤波均方误差更新矩阵（K时刻的最优均方误差）： 将其变形求极限 3 非线性系统的卡尔曼滤波方程 普通卡尔曼滤