卡尔曼滤波最完整公式推导

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。上面一段话来自百度百科，其实最核心的意思就是卡尔曼滤波可以很好地从带有噪声的数据过程中估计状态。而且卡尔曼滤波也是阿波罗登月中使用的突破性技术之一，正好我国嫦娥五号采土归来，正好回顾一下卡尔曼滤波的技术细节，对公式推导做一个完整的整理。

1、定义

• 状态方程：

• 观测方程：

• 符号定义：

首先定义状态估计误差

理解卡尔曼公式推导的核心，是一定要理解这里的符号标记。 核心是在第k时刻，对k+1时刻的估计！所以后面都要记住，在第k时刻的时候，对k+1时刻的任何值，都只能是估计（预测未来值）。具体怎么估计的，如下：

同理，我们也可以得到观测估计误差

基于上面的公式，我们定义两个重要的误差协方差矩阵

• 最终目的

其实卡尔曼的最终目的，是得到一个基于误差能够不断修正的迭代式估计表达式，其具体形式应该如下：

这个式子非常直观，就是基于误差去修正，怎么最优的去修正？就是我们怎么来算这个W，也就是卡尔曼增益（Kalman Gain）。

2、推导

为了得到不停的基于误差修正的一种计算模式，可能要能够得到一些关键的递推形式，如我之前发表过关于递推最小二乘的推导。首先我们推导关于状态误差协方差矩阵：

这里我们得到了P(k|k) 到P(k+1|k) 的一个递推形式，注意这里P(k|k) 的标记形式是为了更方便后面的推导，为什么这么说，其实是为了得到

这样一种能够不停向前迭代的形式。带着这个思考继续下面的推导，就会明白卡尔曼的巧妙之处。

我们先总结下，这里得到了P的一个递推形式：

同理，也是可以得到观测误差协方差的一个递推形式：

会发现其实S(k+1) 和S(k) 没啥直接的关系，这就是为什么不把S(k+1) 写成像P那样 S(K+1|K) 的形式，因为没有必要，只有P是需要这样来写的。

这里再次思考，我们的目的是求一个最优的W，就是卡尔曼增益，具体怎么来最优？这里就用到了状态估计误差，就是说，我们的目的是让状态误差的平方和最小，这里可以使用P的迹。具体推导如下：

然后使用P的迹对W求导，令其为0得到最优的W：

得到：

然后再把W带回P(k+1|k+1)的式子中：

到这里，推导完毕，得到了W的最优表达式，同时，也解决了我们上面提出的问题，一个完整的递推链：

建立完毕。那么通过这一套完整的递推链，给定P的一个初始估计P(0|0) 就可以按照下面的链来进行状态估计：

到这里卡尔曼滤波的公式推导完成了，这个版本是我在看过很多其他资料，反复提炼之后，标记最友好，推导最友好的一个版本，理解卡尔曼滤波一定要首先理解符号的定义，特别是下标，以及怎么样构建递推链。在掌握之后可以自行通过上面的推导总结出，其他教程反复提到的黄金五条公式，其实这些在我看来不是重点，重点是理解其本质原理。

3、资料

其实要弄懂卡尔曼滤波还真不简单，需要一些基本矩阵、统计、自控原理以及矩阵求导的知识，特别是对迹求导，这里推荐Matrix CookBook，里面详细列举了各种常用的求导方式。