import numpy as np# 模擬數(shù)據(jù) t = np.linspace(1, 100, 100) a = 0.5 position = (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')# 初試的估計(jì)導(dǎo)彈的位置就直接用GPS測量的位置 predicts = [position_noise[0]] position_predict = predicts[0]predict_var = 0 odo_var = 120 ** 2 # 這是我們自己設(shè)定的位置測量儀器的方差，越大則測量值占比越低 v_std = 50 # 測量儀器的方差 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 # 利用上個(gè)時(shí)刻的位置和速度預(yù)測當(dāng)前位置predict_var += v_std ** 2 # 更新預(yù)測數(shù)據(jù)的方差# 下面是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()

總結(jié)

以上是生活随笔為你收集整理的卡尔曼滤波估计导弹位置Python代码的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。