Python 基于卡尔曼滤波的机器人圆周运动跟踪
我想做什么:Python 基于卡尔曼滤波的机器人圆周运动跟踪,python,kalman-filter,Python,Kalman Filter,我想做什么: 使用卡尔曼滤波器或扩展卡尔曼滤波器定位以圆周运动的机器人 利用三角学和线性代数,我可以预测“圆周运动”,但我想知道我是否可以使用卡尔曼滤波器来定位机器人(不假设它是圆周运动) 机器人感知其坐标(x,y) 我遇到的问题: 卡尔曼滤波器的状态向量收敛到圆心 卡尔曼滤波器无法找到真实位置 截图: 我的代码实现 我认为这里有两个问题。一个是缺少过程协方差矩阵Q。如果状态转移模型不完美,这将提示算法预测的不确定性有多大。较大的Q值将使算法更依赖于测量值。 尝试初始化 self.q
- 使用卡尔曼滤波器或扩展卡尔曼滤波器定位以圆周运动的机器人
- 利用三角学和线性代数,我可以预测“圆周运动”,但我想知道我是否可以使用卡尔曼滤波器来定位机器人(不假设它是圆周运动)
- 机器人感知其坐标(x,y)
- 卡尔曼滤波器的状态向量收敛到圆心
- 卡尔曼滤波器无法找到真实位置
- 截图:
- 我认为这里有两个问题。一个是缺少过程协方差矩阵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并不重要,因为我认为它是一个小的“噪声”矩阵。我错了。再次感谢。我现在可以安心睡觉了。