第一次听说卡尔曼滤波已经是2017年了，那时候我25岁，卡尔曼却将近60岁了。尽管距离算法得诞生已经半个多世纪了，但是却历久弥新，永不过时。它仍然是当前使用最广泛的数据融合算法。
kalman核心就是两个过程，五个公式：
一、预测过程
二、更新过程
预测可以理解成根据公式进行计算得到的一个值，这样会存在一个协方差误差。Q
更新过程是一个纠正过程，就用另一个观测值来纠正预测值。
那我们应该更相信谁呢，预测还是更新？
这就需要比较他们之间的协方差误差大小（给出了一个kalman系数）
相当于找到了一个求权重的方法。
经典案例：
温度预测
室内温度恒定为23.5度。
根据经验预测时，温度是保持不变的，但是存在一个预测方差Q = 4e-4
但是测数时有一个测量误差，符合高斯分布，方差为R = 0.2^2
假定初始估计温度为25度
初始kalma系统方差为P = 1.0
python代码实现如下：
'''
Kalman filter example demo in Python
'''
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = (10,8)
#inital parameters
n_iter = 100
sz = (n_iter,)
x = 23.5 #真实温度
z = np.random.normal(x,0.2,size=sz) #测量值
#allocate apace for arrays
state_kalman = np.zeros(sz) # a posteri estimate of x 估计值
state_pre = np.zeros(sz) # a priori estimate of x 预测值
P = np.zeros(sz) # a posteri error estimate
Pminus = np.zeros(sz) # a priori error estimate 系统误差
K = np.zeros(sz) # gain or blending factor
R = 0.2**2 #温度测量时的方差
Q = 4e-4 #温度预测时的的方差
state_kalman[0] = 25 # 温度初始值
P[0] = 1.0 # 温度初始估计方差
for k in range(1,n_iter):
#time update
state_pre[k] = state_kalman[k-1] #根据上一个卡尔曼估计值，直接预测，就是原来的值保持不变
Pminus[k] = P[k-1] + Q # 存在预测误差
K[k] = Pminus[k]/(Pminus[k] + R) # kalman 增益
state_kalman[k] = state_pre[k] + K[k]*(z[k] - state_pre[k]) #估计值（权重不一样）
P[k] = (1-K[k])*Pminus[k]
plt.figure()
plt.plot(z,'k+',label = 'noisy measurements')
plt.plot(state_kalman,'b-',label = 'a posteri estimate')
plt.axhline(x,color = 'g',label = 'true value')
plt.legend()
plt.title('Estimate vs. iteration step',fontweight = 'bold')
plt.xlabel('Iteration')
plt.ylabel('Voltage')
plt.figure()
valid_iter = range(1,n_iter)
plt.plot(valid_iter,Pminus[valid_iter],label = 'a priori erroe estimate')
plt.title('Estimate $\it{\mathbf{a \ priori}}$ error vs. iteration step',fontweight = 'bold')
plt.xlabel('Iteration')
plt.ylabel('$(Voltage)^2$')
plt.setp(plt.gca(),'ylim',[0,.05])
plt.show()