卡尔曼滤波（Kalman filtering）算法学习小记

动画和视频

/question/23971601

一个例子

import numpy as np# 模拟数据t = np.linspace(1, 100, 100)a = 0.5position = (a * t ** 2) / 2position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))import matplotlib.pyplot as pltplt.plot(t, position, label='truth position')plt.plot(t, position_noise, label='only use measured position')# 初试的估计导弹的位置就直接用GPS测量的位置predicts = [position_noise[0]]position_predict = predicts[0]predict_var = 0odo_var = 120 ** 2 # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低v_std = 60 # 测量仪器的方差for i in range(1, t.shape[0]):dv = (position[i] - position[i - 1]) + np.random.normal(0, 50) # 模拟从IMU读取出的速度position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置predict_var += v_std ** 2 # 更新预测数据的方差# 下面是Kalman滤波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) ** 2predicts.append(position_predict)plt.plot(t, predicts, label='kalman filtered position')plt.legend()plt.show()