使用卡尔曼滤波器以圆周运动跟踪机器人

Question

我正在尝试做什么：

• 使用卡尔曼滤波器或扩展卡尔曼滤波器对机器人进行圆周运动的本地化
• 使用三角法和线性代数，我能够预测“圆周运动”，但我想知道是否可以使用卡尔曼滤波器来定位机器人（不假设它是圆周运动）< / em>的
• 机器人感知其坐标（x，y）。

我遇到了什么问题：

• 来自卡尔曼滤波器的状态向量收敛到圆的中心
• 卡尔曼滤波器无法找到真实位置
• 屏幕截图：Robot vs Kalman Filter

我的代码实施

• Gist Link

Answer 1

我认为这里有两个问题。一个是你缺少过程协方差矩阵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，以显示初始过冲。

结果如下：