卡尔曼滤波器的推导

参考资源

【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili

【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题_哔哩哔哩_bilibili

【卡尔曼滤波器】3_卡尔曼增益超详细数学推导 ～全网最完整_哔哩哔哩_bilibili

【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式_哔哩哔哩_bilibili

【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibili

说明

我是跟着DR_CAN老师几篇视频的讲解，自己推导了一遍，加深理解，也方便查阅。老师讲得很好。

Kalman filter is an optimal recursive data processing algorithm.

卡尔曼滤波器是一个最优的、递归的数据处理算法，可以对输入数据进行时间递归处理。它是一个线性滤波器。

数据预测具有不确定性，数据测量也有不确定性，卡尔曼滤波是根据不那么准确的预测模型、和不那么准确的观测，来拟合出一个最优值。

递归算法

假设用一把尺子对一个物体进行测量，测了k次，测量值分别是现在求估计值。

测量了k次的估计值为：

上式表达的意思是：

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

在卡尔曼滤波器中， 是卡尔曼增益（Kalman Gain）。

定义估计误差和测量误差：

估计误差：代表估计值和真实值的差距

测量误差：代表测量值和真实值的差距

手工计算前面几次：

用excel计算：

根据excel计算结果画的图：

数学基础：数据融合、协方差矩阵、状态空间方程

数据融合

例如用两个称称一个物体，两个称都不准。

协方差矩阵

协方差矩阵，将方差、协方差在一个矩阵中表现出来，体现了变量间的联动关系。

例子：

球员	身高x（厘米）	体重y（公斤）	年龄z
哈兰德	194	87	23
孙兴憨	183	78	31
萨拉赫	175	71	31
平均	184	78.7	28.3

协方差：

协方差矩阵：

定义矩阵：

则，协方差矩阵：

更多球员的数据：

球员	身高x（厘米）	体重y（公斤）	年龄z
哈兰德	194	87	23
孙兴憨	183	78	31
萨拉赫	175	71	31
因莫比莱	184	78	33
伊瓜因	184	72	36
莫拉塔	189	85	31
贝林厄姆	186	81	20
莱万多夫斯基	185	81	34

求出的协方差矩阵是：

	身高	体重	年龄
身高	25.5	23.5	-11.25
体重	23.5	27.86	-14.48
年龄	-11.25	-14.48	26.61

状态空间表达

将上面四个方程，用矩阵形式表示出来，作为状态空间方程：

卡尔曼增益的数学推导

假设噪声符合正态分布：

其中上式中E表示期望。

下面证明：

如果X的期望为0，即E(X)=0，那么

因为假设噪声符合期望为0的高斯分布，所以：

定义后验估计误差：

下面计算协方差矩阵P：

进一步提取：

定义先验估计误差：

可得：

得到后验估计误差的协方差矩阵：

目标是使后验估计误差协方差矩阵的迹（trace）最小。

的迹：

因为一个矩阵和它的转置矩阵的迹相同，所以：

上式就是卡尔曼增益的公式，是卡尔曼滤波中的一个核心公式。

其中R是测量误差的协方差矩阵。

当R很大的时候，即测量误差很大的时候，

由上面的公式：

可知，此时的后验估计值等于先验估计：

当R很小，即测量误差很小的时候：

此时的估计值等于测量值：

误差协方差矩阵

卡尔曼滤波器的5个公式：

在卡尔曼增益公式推导过程中，我们已经得出：

通过一个二维实例直观理解卡尔曼滤波器

 

考虑一个人走路，假设匀速行驶。分别设定两个状态变量：

：表示位置

：表示速度

此时，速度和位置状态方程变为：

因为加上了不确定性，所以就无法直接通过上面两个式子求出k时刻的位置、速度。但是可以估计，根据噪声的误差来估计。

卡尔曼滤波器分两个步骤：预测和校正：

用excel计算：

为了简化起见，设Q矩阵、R矩阵的协方差项为0，即各误差项之间是相互独立的。所以Q矩阵和R矩阵只有对角线上的值不等于0。矩阵的值自己可以修改