卡尔曼滤波

import random
import matplotlib.pyplot as plt
x_last = 0;
p_last = 0.05; // 初始值不能为0，值大小无所谓，会随着迭代次数增加，值的变化和初始值无关。
Q = 0.01; //预测误差，越大表示信任测量值，结果会更加随测量值的波动而波动。 越小表示信任预测模型，波动会更小。
          //这个值一般设置小一点，否则感觉意义不大。

R = 0.6; //测量误差

kg = 0.0;
x_mid = 0.0;
x_now = 0.0;
p_mid = 0.0; //p为系统的反馈值，每次迭代都会修正p值。
p_now = 0.0;
z_real = 1;
z_measure = 0.0;

x_last = z_real + (random.random()-0.5)*1;
x_mid = x_last;

for i in range(500):
    x_mid = x_last;

    p_mid = p_last + Q;  
    kg = p_mid / (p_mid + R);
    z_measure = z_real + (random.random()-0.5)*1;
    x_now = x_mid + kg*(z_measure - x_mid);
    p_now = (1 - kg)*p_mid;

    p_last = p_now;
    x_last = x_now;

    plt.plot(i, kg, 'ro', markersize = 1)
    plt.plot(i, p_mid, 'go', markersize = 1)
    plt.plot(i, z_measure, 'bo', markersize = 1)
    plt.plot(i, x_now, 'ro', markersize = 1)
    
plt.show()