卡尔曼滤波器理论推导

type

status

date

slug

summary

Part 0: 参考内容

参考B站UP主：Math talk、DR_CAN

参考链接：

21. 卡尔曼滤波器详解--从零开始(1) Kalman Filter from Zero

接下来的几篇从零开始介绍 Kalman filter系列的文章花费了我许多时间整理。这个话题我一直不想开始讲，是因为真的要讲好这一块，并且从基础开始讲，是非常不容易的。这就是控制的难学之处，内容庞杂！ Kalman Filter既是控制设计方面学习者的需知常识，也是传感器系统设计和优化，信号处理，系统辨识等领域的实践者的必备知识。理解Kalman filter需要大概四块领域的知识：概率和随机过程理论(Probability and Random Process Theory) 统计学和最优估计理论(Statistics and optimal estimation) 信号处理（Signal Proccessing）动力学系统常识(Dynamic Systems) 我会以 5门本书的内容（见reference）为基础从最基础的知识开始讲，把必须知道的一些东西讲清楚，而不是上来便抛出一系列公式和符号。主要基于前4本书来讲解，第5本中文教材可以说是一个以上4本教材内容的一些简短总结。同样的，名词我会用中英文交替书写，以便你习惯以后阅读其他英文资料。说是从0开始，你需要对控制和信号的基本知识有个大概了解。这一系列文章本来是作为 LQG 控制(Linear Quadratic Gaussian Control) 的一点前奏，后来想好好写一写也算是给读者们的一点福利。由于filtering是一块非常庞大的知识体系，我只挑基础的必要知识来讲，以及基础的Kalman filter理论。 Kalman filter有很多改进版本，这里作为关于控制的系列文章，我就不花时间列举了。作为现代控制理论系列文章的一块核心内容，但愿能帮助到你们学习控制理论。多点赞和推荐给你挣扎中的朋友，也算支持我抽空出来创作吧。关于私信和咨询啊，做题写paper什么的术业有专攻，我就不回应这些诉求了。老实说很多知友抛出的问题我也不是一下子就能完全解答的，也得花费时间去找资料阅读和思考。我也没打算靠那点咨询费去赚钱，要的高了，知道很多人都是学生，也没那个经济能力。以我目前的工作状态，只能满足抽空来总结一些控制基础知识了，一方面是给自己的阅读留下一点笔记，另一方面给自己的一些要学习控制的学术界朋友和学生留点干货。我能保证的就是，既然写了，就一定要写好，不是截个图，连自己都没搞懂就来误人子弟。本篇目录： What is Kalman Filter and its use?

https://zhuanlan.zhihu.com/p/166342719

参考书籍：

概率机器人

机器人学中的状态估计

Part 1: Recursive Algorithm 递归算法

Optimal Recursive Data Processing Algorithm——最优化递归数字处理算法

更像是观测器而不是一般的滤波器

系统的不确定性：

不存在完美的数学模型

系统的扰动不可控，也很难建模

测量传感器存在误差

例：

对某个物体进行测量，记第k次测量结果为

估计真实数据 → 取平均值

其中，为k-1次时的平均值

则原式可合并为如下

可得到

随着k的增加，测量结果不再重要

k值小的时候，较大，作用较大

令，则上式化为

当前的估计值 = 上一次的估计值 + 系数 *（当前测量值 - 上一次的估计值）

：Kalman Gain 卡尔曼增益/因数

估计误差：

测量误差：

则卡尔曼滤波器的核心公式为：

讨论：在k时刻：

k-1时的估计误差远大于k时的测量误差

计算步骤如下：

计算卡尔曼增益：

计算：

更新：

例：长度测量

实际长度：

第一次估计长度：

第一次估计误差：（假设估计误差不变）

第一次的测量值：

第一次的测量误差：

测量次数	z_{k}	e_{MEA_{k}}	\hat{x_{k}}	k_{k}	e_{EST_{k}}
0	ㅤ	ㅤ	40	ㅤ	5
1	51	3	46.88	0.625	1.875
2	48	3	47.31	0.385	1.154
3	ㅤ	ㅤ	ㅤ	ㅤ	ㅤ

上述表格画为曲线

递归思想：随着数据的不断迭代，测量值会逐渐靠近真实值

Part 2: Data Fusion, Covariance Matrix, State Space, Observation

参考如下链接：

卡尔曼滤波（二）--三种数学思想_Patrick_zl的博客-CSDN博客

本文为卡尔曼滤波个人学习文档，主要资料来源B站UP主DR_CAN相关视频（【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题_哔哩哔哩_bilibili），欢迎各路大佬交流指正。本节并没有直接讨论卡尔曼滤波，而是为卡尔曼滤波器的推导进行一些有关于数学基础的铺垫，包括以下三个方面：1、数据融合（Data Fusion）2、协方差矩阵（Covariance ...

https://blog.csdn.net/Patrick_zl/article/details/121276887

Data Fusion 数据融合

为标准差，服从正态分布

估计真实值

求k使得最小方差最小

由于独立：

由上式可知当时，最小，因此有：

最后解得：

表示当K取此值时，估计值最小。至此可以看到卡尔曼增益的雏形了。

卡尔曼增益——数据融合结果

Covariance Matrix 协方差矩阵

协方差描述的是两个变量之间的相关性，对于两个变量x，y 可将二者的协方差表达为：

上式可理解为当高于平均值时，如果相应的每个也高于平均值，则，表示二者成正相关；反之当高于平均值时，相应的每个低于平均值，则，表示二者负相关。

并且我们将变量x的方差表示为。因此规定对于两个变量x, y，可将二者的协方差表达为：

对于三个变量x，y，z的协方差矩阵则表示为：

对于任意变量的协方差矩阵也可以此类推。

对于协方差矩阵的求取可用如下方法（以求取拥有n个样本的三个变量x，y，z的协方差矩阵为例）：

Step1：设样本矩阵s：

Step2：求过渡矩阵（注意式中第二项为元素全为1的方阵，而不是单位阵）：

Step3：最后求协方差矩阵P：

注：MATLAB函数

方差函数：varp

协方差函数：covar

State Space, Observation 状态空间方程以及观测器

现代控制理论中，除了有传统的输入输出作为描述系统的概念意外，还引入状态变量这一概念标识系统的状态。由输入、输出和状态标量组成，描述系统变化的方程称为状态空间方程（也叫状态空间表达式）。状态空间方程由状态方程和输出方程组成。对于离散系统和连续系统，状态空间方程有着各自的表达方法。由于卡尔曼滤波较多适用于离散系统，所以本节主要讲解离散系统的状态空间方程。