用於速度估計的python中的卡爾曼濾波器實現

Question

我嘗試實現卡爾曼濾波器來預測速度，提前一步。 Python實現 H = np.diag（[1,1]） ħ

結果： 陣列（[[1,0]， [0，1]]） 對於測量矢量 數據文件是包含時間一列和速度csv文件中的另一列

measurements=np.vstack((mx,my,datafile.speed)) 
#length of meassurement 
m=measurements.shape[1] 
print(measurements.shape) 

輸出：（3，1069）

卡爾曼

for filterstep in range(m-1): 
     #Time Update 
      #============================= 
     #Project the state ahead 
     x=A*x 

     #Project the error covariance ahead 
     P=A*P*A.T+Q 

     #Measurement Update(correction) 
     #=================================== 
     #if there is GPS measurement 
     if GPS[filterstep]: 
     #COmpute the Kalman Gain 
     S =(H*P*H).T + R 
     S_inv=S.inv() 
     K=(P*H.T)*S_inv 

     #Update the estimate via z 
     Z = measurements[:,filterstep].reshape(H.shape[0],1) 
     y=Z-(H*x) 
     x = x + (K*y) 

     #Update the error covariance 
     P=(I-(K*H))*P 


# Save states for Plotting 
    x0.append(float(x[0])) 
    x1.append(float(x[1])) 


    Zx.append(float(Z[0])) 
    Zy.append(float(Z[1])) 

    Px.append(float(P[0,0])) 
    Py.append(float(P[1,1])) 



    Kx.append(float(K[0,0])) 
    Ky.append(float(K[1,0])) 

錯誤當屬：

--------------------------------------------------------------------------- 
ValueError        Traceback (most recent call last) 
<ipython-input-80-9b15fccbaca8> in <module>() 
    20 
    21   #Update the estimate via z 
---> 22   Z = measurements[:,filterstep].reshape(H.shape[0],1) 
    23   y=Z-(H*x) 
    24   x = x + (K*y) 

ValueError: total size of new array must be unchanged 

我怎樣才能消除這種錯誤

Answer 1

這條線是不正確的：

S =(H*P*H).T + R 

正確的代碼是：

S =(H*P*H.T) + R 

我在測量結果出現問題時遇到問題。您聲明 「array（[[1,0]，[0，1]]）對於測量向量數據文件是csv文件，其中包含時間作爲一列，並且速度在另一列」

因此，讀取爲CSV具有兩列，一次和一次速度的文件。在這種情況下，每次只有一次測量，即速度。對於單次測量，您的H矩陣應該是一個行向量。