卡尔曼滤波器分类及基本公式

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