卡尔曼滤波的尝试，python实现 -- 参考了众多大佬的文章

以下以python代码为线索，写出来自己的思考，没有真正的公式和理论推导。因为我还是喜欢代码，不喜欢理论的…
代码copy的这位大佬：https://blog.csdn.net/varyshare/article/details/95065650

先模拟数据，从数据出发

# 我们先弄点模拟数据 假设这东西跑了100秒，按照 0.5 m/s 的加速度跑的，这是完全正确的实际位置
import numpy as np
time = np.linspace(1,100,100)
position = 1/2 * 0.5 * time**2

# 我还需要一点噪声，毕竟观察测量的不准的 normal正态分布的噪声，均值为0，标准差为120
noise = np.random.normal(0,120,size=(t.shape[0]))
position_noise = position+noise

看下数据可视化效果

import matplotlib.pyplot as plt
plt.plot(t,position,label='truth position')
plt.plot(t,position_noise,label='only use measured position')

plt.legend()
plt.show()

接下来计算卡尔曼滤波对每个点的估计值
由于起点无法由测量值和估计值计算得到，因此由position_noise的第一个值即观测值作为估计值

#一个数组存储估计值
predicts = [postion_noise[0]]
#一个变量，存储当前的估计值
position_predict = predicts[0]

设置参数：

#估计值的方差
predict_var = 0
#这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
odo_var = 120 ** 2
#测量值的方差
v_std = 50

迭代计算

# 迭代99次
for i in range(1,t.shape[0]):
	# 利用实际值与前一时刻的实际值的位置差，加入噪声来得到下一个位置的估计值
    dv =  (position[i]-position[i-1]) + np.random.normal(0,50) 
    position_predict = position_predict + dv 
    # 更新估计值的方差
    predict_var += v_std**2 
    # 下面是Kalman滤波，是 position_predict (估计值) 和 position_noise (观测值) 取加权平均的过程
    position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)
    # 再次更新估计值的方差
    predict_var = (predict_var * odo_var)/(predict_var + odo_var)**2
    predicts.append(position_predict)

结果可视化出来

plt.plot(t,predicts,label='kalman filtered position')
plt.legend()
plt.show()