python中用于速度估计的卡尔曼滤波器实现

Posted

技术标签:

【中文标题】python中用于速度估计的卡尔曼滤波器实现【英文标题】:Kalman filter implementation in python for speed estimation 【发布时间】:2016-04-23 11:58:10 【问题描述】:

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

结果: 数组([[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

我怎样才能消除这样的错误

【问题讨论】:

我想知道测量中的列是什么?你有位置 x 和 y 还是只有速度和时间?。 【参考方案1】:

此行不正确:

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

正确的代码是:

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

我无法跟踪测量结果。你说 " array([[1, 0], [0, 1]]) 测量矢量数据文件是 csv 文件,其中包含时间作为一列,速度在另一列"

因此,我将其作为 CSV 文件读取,其中包含两列、一个时间和一个速度。在这种情况下,您每次只有一个测量值,即速度。对于单次测量,您的 H 矩阵应该是行向量。

【讨论】:

以上是关于python中用于速度估计的卡尔曼滤波器实现的主要内容,如果未能解决你的问题,请参考以下文章

带加速度的卡尔曼滤波器。状态或控制向量?

时间序列分析----结合ARMA的卡尔曼滤波算法

python中的卡尔曼二维过滤器

Matlab:程序返回垃圾值,帮助正确执行卡尔曼滤波器和参数估计

适用于 iOS 的卡尔曼滤波器

更改 OpenCV 的卡尔曼滤波器中的增益以使其更具响应性