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

我要做的是：
基于卡尔曼滤波或扩展卡尔曼滤波的机器人圆周运动定位
使用三角学和线性代数，我可以预测一个“圆周运动”，但我想知道我是否可以使用卡尔曼滤波器来定位机器人（不假设它在圆周运动中）
机器人感知它的坐标（x，y）。
我遇到的麻烦是：
卡尔曼滤波器的状态向量收敛到圆心
卡尔曼滤波器找不到真实位置
截图：Robot vs Kalman Filter
我的代码实现
Gist Link

最佳答案

我认为这里有两个问题。一是缺少过程协方差矩阵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以可视化初始超调。
结果如下：

关于python - 使用卡尔曼滤波器跟踪机器人的圆周运动，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/41754085/