什么是卡尔曼滤波
对于这个滤波器，我们几乎可以下这么一个定论：只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性。
因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。
形象解释
卡尔曼滤波就是以上个时刻的状态，推断下个时刻的状态。
比如一个有经验的船长，没有很先进的仪器能定位船的位置，他需要走一会判断一下位置，然后根据这个位置判断下一个位置，否则走的太远就迷路了；
首先他能知道起点的位置，然后每隔一定时间，就根据船速、航线、风向等预估自己的位置，这是经验，不一定准确，且肯定存在一定误差；
他还有个不太精确的仪器，能测量出自己的位置，也不一定准确，且也存在一定误差；
那现在有了经验值和测量值，到底哪个准确，如何参考这两个值；
如果船长很自信，更相信自己的经验，这样 经验值的权重就大一些，观测值权重小一些；
如果船长经验不足，肯定不能过分相信经验，只能更多参考测量值，此时测量值权重大一些；
最后就确定了 新的位置Xnew
x 为预测， z 为观测
显然 k 决定了各自的权重，那么 k 又是如何确定呢？
我们就看谁更准，怎么做呢？
比如现在真实的位置我们知道，然后拿仪器测，如果结果一样，那说明仪器准，此外由于存在系统误差，如仪器不灵光，外部温度不同，电量不足等等，总之会有一定的误差，每次测量结果可能不太一样，那就需要多测几次，看看结果准不准，也就是计算方差大小，方差大小决定了可信度；
同样的方法人也一样，可以获得人的可信度；
显然 k 就是可信度占比
但是要注意一点，可信度随着状态的转移会发生变化，如人在一个熟悉的环境中，他判断位置就会比较准确，在一个陌生的环境中，判断位置就不太可信；
此时 新的位置Xnew 似乎确定了，但是别忘了，仍然是 “推测” 的，也不完全准确，也存在一定的偏差，此时的偏差也跟 k 有关，因为 k 实际上决定了 Xnew 的精度。
数学建模
卡尔曼滤波有两个假设
1. 世界上噪声无处不在，状态本身有噪声，状态转移也有噪声 ====> 比如现在的测量温度是 30 度，实际上可能是29度，这就是状态噪声；你开空调1分钟，可能变成25度，也可能变成26度，这就是状态转移噪声
2. 一个系统会处于各种不同的状态，并且可以相互转换，我们无法获取当前状态，但是可以通过测量猜测出当前状态
场景描述
有一辆小车在行进，车上装有传感器获取 速度v，也装有定位装置获取 位置p，那么小车的状态就可以用 x=[p, v]’ 来表示；
在车行驶的过程中，存在内部控制，如踩油门加速；也存在外部影响，如风阻
u 可以理解为加速度；
######## 得到 某时刻的状态
误差表示
卡尔曼滤波假设噪声无处不在，仪器获取的数据当然也有噪声，所以我们只能认为 测量值是真实值的最优估计，并且误差服从正态分布，
我们用 最优估计 代表当前状态，但是实际上真实的状态是某个范围中的一点
xk就是最优估计，其实就是均值，这里记住状态是均值；
其周围的部分就是方差，由于是两个量，车速和位置，所以用协方差表示
这就是当前状态的方差，即当前状态的可信度
######## 得到 该时刻的状态 的方差，形成 该时刻状态 的 高斯分布
状态转移
根据物理公式可以得到 下一时刻的 位置pt 和 速度vt，并转换成矩阵的表示方式，在形式上做简化，就得到如下公式。
其中 Ft 代表状态转移矩阵；Bt 代表状态控制矩阵；ut 代表 状态控制向量
状态-均值发生变化，其方差也会相应转变
Pt = FtPt-1FtT，T表示转置 【协方差矩阵的性质 cov(Ax, Bx)=Acov(x, y)BT】
形成了新的高斯分布
考虑到外部影响因素，这里假设外部影响因素服从 高斯分布 wk~N(0, Qk) 【一会顺风，一会逆风，一会上坡，一会下坡，可以看做均值为0的正态分布】
至此得到完整的状态预测方程
状态是均值，由于 wk 均值为0，有些教程直接忽略了，具体使用时根据实际情况而定；
######## 得到 下一时刻的状态 的预测 和 对应的方差，形成 下一时刻状态 的 高斯分布，并带噪声
状态转换成指标
小车的状态是 车速 和 位置，但是状态可能不是我们需要的数学指标，所以需要进行转换，如观测到你出汗了，但我们需要你热不热，此时就把出汗转换成热；
假设这个转换矩阵为 Hk；
上步得到的 下一时刻状态的 预测和方差就要转换成
此时我们完成了下一状态的预测，结果服从正态分布
######## 得到 下一时刻指标 的预测 和 对应的方差，形成 下一个时刻指标的 高斯分布，带噪声
状态观测
用传感器观测小车的状态，车速和位置，也是带有误差的，转换成数学指标，
把 协方差 记作 Rk，把状态的数学指标记为 Zk，即均值
权衡分析
现在有了 下一时刻 的预测值和观测值，和对应方差，如下图
这就可以根据方差来确定 k (形象描述里面的k)，然后计算这个时刻的 最优估计状态，并更新方差
这个 k 是个很重要的概念，叫卡尔曼增益。
为了简化式子，把上面三个式子都 左乘 Hk-1
第三个式子再 右乘 Hk 的转置的逆，得到如下式子
这就得到了 下一个时刻 的状态和噪声
此时就可以继续预测下下时刻的状态 和噪声。
总结
卡尔曼滤波就是5个公式
预测，更新，更新完在预测
图形描述
实例
模拟预测小车速度和位置的一段程序
观测值是一段从0到99，速度为1的匀速行驶路径，加入高斯噪声
import numpy as np import matplotlib.pyplot as plt ### 创建有噪声的观测值 # 创建一个0-99的一维矩阵 z = [i for i in range(100)] z_watch = np.mat(z) # 创建一个方差为1的高斯噪声，精确到小数点后两位 noise = np.round(np.random.normal(0, 1, 100), 2) noise_mat = np.mat(noise) # 将z的观测值和噪声相加 z_mat = z_watch + noise_mat ### 定义初始状态 # 定义x的初始状态 x_mat = np.mat([[0,], [0,]]) # 定义初始状态协方差矩阵 p_mat = np.mat([[1, 0], [0, 1]]) ### 状态转移矩阵 # 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1 f_mat = np.mat([[1, 1], [0, 1]]) ### 外部影响因素方差 # 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高 q_mat = np.mat([[0.0001, 0], [0, 0.0001]]) #### 观测值 # 定义观测矩阵，状态转换指标 h_mat = np.mat([1, 0]) # 定义观测噪声协方差 r_mat = np.mat([1]) for i in range(100): x_predict = f_mat * x_mat p_predict = f_mat * p_mat * f_mat.T + q_mat kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat) x_mat = x_predict + kalman *(z_mat[0, i] - h_mat * x_predict) p_mat = (np.eye(2) - kalman * h_mat) * p_predict print(x_mat) plt.plot(x_mat[0, 0], x_mat[1, 0], 'r*', markersize = 3) plt.show()
由于我们的观测值只有里程，0-99，而观测状态是 [里程，车速]，所以转换矩阵 H 为 [1， 0]，乘完之后只有里程。
输出
横坐标为路程，纵坐标为车速。
参考资料：
https://zhuanlan.zhihu.com/p/39912633 更详尽请参考该教程
https://www.zhihu.com/question/22422121/answer/22877882
https://blog.csdn.net/guo1988kui/article/details/82778198