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

Posted 2023-02-21

【中文标题】使用卡尔曼滤波器跟踪圆周运动的机器人

【英文标题】：Tracking a robot in circular motion using Kalman Filter

【发布时间】：2017-01-20 00:24:01

【问题描述】：

我想做的事：

使用卡尔曼滤波器或扩展卡尔曼滤波器定位以圆周运动移动的机器人 使用三角学和线性代数，我能够预测“圆周运动”，但我想知道是否可以使用卡尔曼滤波器来定位机器人（不假设它处于圆周运动中） 机器人感知其坐标 (x, y)。

我遇到的问题：

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

我的代码实现

Gist Link

【参考方案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 以可视化初始超调。

结果如下：

【讨论】：

谢谢。我不认为 Q 很重要，因为我认为它是一个小的“噪声”矩阵。我错了。再次感谢。我现在可以安心睡觉了。