Kalman卡尔曼滤波

步骤：
1、计算\(y = 20 e^{0.24x}\)计算出\(x\) 在0.1~4区间内的y值作为真值：\(Y\)；

2、在 Y 的基础上加入一个高斯分布的误差作为观测量，观测量：Y_OBS；

3、初始化P、Q、R矩阵，P代表初始状态精度，Q代表预测精度，R观测精度；

4、初始化状态值 state、结果存储空间 Y_reselt ，协方差存储空间 P_reselt.

# step 1
X = np.arange(0.1,4,0.01)
Y = 20 * np.exp(0.24*X)
# step  2
rand_error = 2
Y_obs = Y + rand_error * (2* np.random.random(len(X))-1)
# step  3
Q = 0.1
R = 5
P = 1
# step 4
state = 20
Y_reselt = np.zeros(len(X))
P_reselt = np.zeros(len(X))

Y_reselt[0] = state
P_reselt[0] = P

for i in range(1,len(X)):
    P = P + Q
    K = P / (P + R)
    state = state + K * (Y_obs[i]-state)
    P = (1-K) * P 
    P_reselt[i] = P 
    Y_reselt[i] = state


plt.plot(X,Y_obs,label = 'Noisy Signal')
plt.plot(X,Y_reselt,label = 'Estimated Signal')

plt.legend()
plt.show()