我認為這里有兩個問題。一個是你缺少過程協方差矩陣Q.如果你的狀態轉移模型不完美，這將給算法一個暗示預測有多不確定的暗示。大Q將使算法更多地依賴于測量。

嘗試初始化

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

以及后來的預測函數

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

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

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以獲得更平滑的預測，并將P初始化從999減少到1，以顯示初始過沖。

結果如下：

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。