使用扩展卡尔曼滤波器预测在turtlesim中绕圈移动的机器人的姿势

Question

我正在 ROS 中为turtlesim 进行模拟。

在 turtlesim 中，有一只乌龟“A”开始在 2D 环境中绕圈移动。还有一只乌龟“B”需要追赶 A 并捕捉，但 B 在 A 10 秒后生成，B 可以每 5 秒知道 A 的姿势（x，y，theta，线性速度，角度速度）。这意味着 B 不知道 A 的当前位置。我需要根据 A 的最后一个可用姿势在一定时间（例如 15 秒）后预测 A 的位置，以便在 A 到达其未来位置时，B可以提前到达那个位置并在那里等待A，当A到达那个位置时，B抓住A。

我正在尝试使用扩展卡尔曼滤波器来预测姿势，但它没有给出正确的预测姿势。

代码链接：我的要点

注意：step_vel 函数接受速度命令并将其发布。

我不考虑测量，因为我们没有任何测量。 我们只有以下 A 的信息：

1. x 坐标
2. y坐标
3. theta（A 的方向）
4. 线速度
5. 角速度

我哪里错了？

Answer 1

我正在阅读您的代码时的注释：

• 看起来您假设线速度和角速度是已知的且恒定的。 （您确定这是真的吗？）如果这是真的，您可以通过不将它们包含在状态向量中来简化过滤器。 EKF 对象仍然可以将它们存储为常量，但它们不需要位于 x
中
• 代替 F.dot(x) 你可以直接做 F @ x
• 为什么要更新 __x 两次？第 46 行是

  self.__x = predicted_x

  ，但随后你会在 49-51 上执行

  self.__x[0] += self.__linear_vel * np.cos(self.__x[2]) * dt

  以及一些类似的行。这很可能是过滤器的问题；请注意 49-51 正在执行的操作与 F 定义的动态之间的差异。
• 您的更新步骤出了问题。 H 是恒等式，因此您将获得所有五种状态的测量值，但您的测量值是 3x1。不确定你想做什么，但有些东西需要修复。
• 你调整过 Q 和 R 了吗？如果噪声比滤波器预期的多/少，这也很容易成为问题。