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中用于速度估计的卡尔曼滤波器实现的主要内容,如果未能解决你的问题,请参考以下文章