用于速度估计的python中的Kalman滤波器实现

Question

我尝试实现卡尔曼滤波器来预测速度提前一个步骤。 在python中实现H = np.diag（[1,1]）H

结果：array（[[[1，0]，[0，1]]）对于测量矢量数据文件是csv文件，其中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矩阵应为行向量。