用于速度估计的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矩阵应该是一个行向量。