動(dòng)畫和視頻

一個(gè)例子

import numpy as np

# 模擬數(shù)據(jù)

t = np.linspace(1, 100, 100)

a = 0.5

position = (a * t ** 2) / 2

position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))

import matplotlib.pyplot as plt

plt.plot(t, position, label='truth position')

plt.plot(t, position_noise, label='only use measured position')

# 初試的估計(jì)導(dǎo)彈的位置就直接用GPS測(cè)量的位置

predicts = [position_noise[0]]

position_predict = predicts[0]

predict_var = 0

odo_var = 120 ** 2 # 這是我們自己設(shè)定的位置測(cè)量?jī)x器的方差，越大則測(cè)量值占比越低

v_std = 60 # 測(cè)量?jī)x器的方差

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ù)測(cè)當(dāng)前位置

predict_var += v_std ** 2 # 更新預(yù)測(cè)數(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) ** 2

predicts.append(position_predict)

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

plt.legend()

plt.show()

總結(jié)

以上是生活随笔為你收集整理的java 二维卡尔曼滤波_卡尔曼滤波（Kalman filtering）算法学习小记的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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