在上一篇文章中手把手推导了一遍卡尔曼增益，不熟悉的小伙伴可以看

养生的控制人：卡尔曼增益推导​zhuanlan.zhihu.com

这里再回顾一下重点。

问题重述

假设真实系统为

其中

我们对系统状态的估计（数据融合）为

其中卡曼尔增益为

我们可以看到卡尔曼增益中的估计协方差矩阵

估计协方差矩阵推导

根据定义

带入真实系统模型和状态估计的模型

再带入估计协方差矩阵的表达式

把转置放进去

把括号打开

由于四项之间是线性相加的，可以把期望的运算放进去，等于每一项的期望。

注意到

因此估计的协方差矩阵剩下两项

有了这个表达式后，我们就可以用卡尔曼滤波器来估计状态了！

卡尔曼滤波器两步法

目前为止卡尔曼增益中各项都是已知的了，因此可以用于估计了，卡尔曼滤波分为两个步骤：预测和校正。

Step1. 预测

Step2. 校正

数据融合即对状态的估计

至此，卡尔曼滤波（估计器）就推导完毕了，给定初值

数据融合、预测校正。

---

公式（5）的推导

在上一篇文章中我们推导得到协方差矩阵的表达式为

合并同类项

带入卡尔曼增益