我認(rèn)為這里有兩個(gè)問(wèn)題。一個(gè)是你缺少過(guò)程協(xié)方差矩陣Q.如果你的狀態(tài)轉(zhuǎn)移模型不完美，這將給算法一個(gè)暗示預(yù)測(cè)有多不確定的暗示。大Q將使算法更多地依賴于測(cè)量。

嘗試初始化

self.q = 0.001*self.f.dot(self.f.transpose())

以及后來(lái)的預(yù)測(cè)函數(shù)

self.p = self.f.dot(self.p).dot(self.f.transpose()) + self.q

另一個(gè)問(wèn)題是你在笛卡爾平面上測(cè)量圓形(極地)運(yùn)動(dòng)。旋轉(zhuǎn)在X和Y中給出加速度，并且在F矩陣中缺失。我會(huì)更新F矩陣以包括完整的物理模型，包括加速度。時(shí)間步長(zhǎng)(dT)也缺失，可以作為參數(shù)添加。

class KalmanFilter(Filter):

def __init__(self, sigma, dT):

...

self.f = np.array([[1, 0, dT, 0, dT*dT/2, 0],

[0, 1, 0, dT, 0, dT*dT/2],

[0, 0, 1, 0, dT, 0],

[0, 0, 0, 1, 0, dT],

[0, 0, 0, 0, 1, 0],

[0, 0, 0, 0, 0, 1]])

最后在你的主要功能

KF = KalmanFilter(sigma=1,dT=0.1)

我還將sigma增加到1以獲得更平滑的預(yù)測(cè)，并將P初始化從999減少到1，以顯示初始過(guò)沖。

結(jié)果如下：

總結(jié)

以上是生活随笔為你收集整理的python卡尔曼滤波跟踪_使用卡尔曼滤波器以圆周运动跟踪机器人的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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