第一次听说卡尔曼滤波已经是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()