[英]Tracking a robot in circular motion using Kalman Filter
我正在尝试做的是:
我遇到的问题是:
我的代码实现
我认为这里有两个问题。 一种是您缺少过程协方差矩阵Q。如果您的状态转换模型不完美,这将为算法提供一个提示,表明预测的不确定性。 较大的Q将使算法更多地依赖于测量。 尝试初始化
self.q = 0.001*self.f.dot(self.f.transpose())
然后在预测功能中
self.p = self.f.dot(self.p).dot(self.f.transpose()) + self.q
另一个问题是您正在测量笛卡尔平面中的圆周(极)运动。 旋转会在X和Y方向产生加速度,而在F矩阵中会消失。 我将更新F矩阵以包括完整的物理模型,包括加速度。 时间步长(dT)也丢失了,可以作为参数添加。
class KalmanFilter(Filter):
def __init__(self, sigma, dT):
...
self.f = np.array([[1, 0, dT, 0, dT*dT/2, 0],
[0, 1, 0, dT, 0, dT*dT/2],
[0, 0, 1, 0, dT, 0],
[0, 0, 0, 1, 0, dT],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
最后是你的主要功能
KF = KalmanFilter(sigma=1,dT=0.1)
我还将sigma增加到1以得到更平滑的预测,并将P初始化从999减小到1以可视化初始过冲。
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.