轻松理解Kalman Filter

原文来自视频

什么是卡尔曼滤波？

理解过程

问题提出

其中，p:位置，u:速度，$P_k$：协方差矩阵
如何直观地理解「协方差矩阵」？
浅谈协方差矩阵

在统计学中，方差是用来度量单个随机变量的离散程度，而协方差则一般用来刻画两个随机变量的相似程度，
协方差的结果有什么意义呢？
如果结果为正值，则说明两者是正相关的（从协方差可以引出“相关系数”的定义），也就是说一个人越猥琐越受女孩欢迎。如果结果为负值， 就说明两者是负相关，越猥琐女孩子越讨厌。如果为0，则两者之间没有关系，猥琐不猥琐和女孩子喜不喜欢之间没有关联，就是统计上说的“相互独立”。

从方差/协方差到协方差矩阵

当没有外力作用时：

当有外力作用时：

上面的两个情况，公式可以写成下面的矩阵形式，其中$x=[p_k,u_k{]}^T$

怎么估计协方差呢？
先看看协方差的性质：

引入噪声后

状态预测模型推出k时刻的状态，因为状态始终满足正态分布，所以只用均值和协方差矩阵就可以表示这个转换

X表示预测，Y表示实际测量，如下


怎样由预测X,和测量Y，这两个正态分布得出最优估计呢？相乘即可
两个正态分布相乘的性质如下

写成矩阵形式，则如下

所以针对前面的预测X和测量Y，

得出最优的状态估计为

引入卡尔曼增益K后，可以写成如下

编程实现的步骤：
第一步：预测

第二步：测量

第三步：卡尔曼增益

第四步：返回第一步，不断地迭代