Skip to main content
 首页 » 编程设计

python之5D 卡尔曼滤波器不起作用,我们不确定哪里出了问题

2024年10月25日13zhwl

现在我们正在尝试创建一个 5 维卡尔曼滤波器,它接收在盒子里四处游荡的小 bug 的 x 和 y 坐标作为输入。我们观察这个错误,因为它做了大约 2000 次移动,然后预测它从那时起的位置。维度是 x 坐标、y 坐标、速度、航向和角加速度。以下是我们到目前为止的代码。

#x is a list of five variables - x, y, velocity, angularVelocity, angularAcceleration 
#currentmeasurement is the x and y that were observed 
def kalman_filter(x, P, currentmeasurement, lastMeasurement = None): 
        prevmeasurement = [] 
 
    #if there is a lastMeasurement argument, it becomes measurement 
    if lastMeasurement: 
        prevmeasurement = [lastMeasurement[0], lastMeasurement[1], x.value[3][0]] 
 
    #if there is no lastMeasurement argument, the current measurement becomes measurement. 
    else: 
        prevmeasurement = [x.value[0][0], x.value[1][0], x.value[3][0]] 
 
    #Prediction Step 
    a = x.value[3][0] 
    F = matrix([ 
        [1., 0., cos(a), 0., 0.], 
        [0., 1., sin(a), 0., 0.], 
        [0., 0., 1., 0., 0.], 
        [0., 0., 0., 1., dt], 
        [0., 0., 0., 0., 1.]]) 
 
    x = (F * x) + u 
    P = F * P * F.transpose() 
 
    #we can calculate the heading from our observations. 
    heading = atan2(currentmeasurement[0][1] - prevmeasurement[1], currentmeasurement[0][0] - prevmeasurement[0]) 
    while(abs(heading - prevmeasurement[2]) > pi): 
        heading = heading + 2*pi*((prevmeasurement[2]-heading)/abs(prevmeasurement[2]-heading)) 
 
    #perhaps the velocity should also be calculated?  
 
    # measurement update 
    dt = 1. 
    u = matrix([[0.], [0.], [0.], [0.], [0.]])      # external motion 
    H =  matrix([[1., 0., 0., 0., 0.],              #the measurement function 
             [0., 1., 0., 0., 0.], 
             [0., 0., 0., 1., 0.]]) 
 
    R =  matrix([[1., 0., .0],                      #measurement uncertainty 
             [0., 1., .0], 
             [0., 0., 1.]]) 
    I =  matrix([[]])                               #a 5x5 identity matrix 
    I.identity(5) 
 
 
    prevmeasurement = [currentmeasurement[0][0], currentmeasurement[0][1], heading] 
    Z = matrix([prevmeasurement]) 
    y = Z.transpose() - (H * x) 
    S = H * P * H.transpose() + R 
    K = P * H.transpose() * S.inverse() 
    x = x + (K * y) 
    P = (I - (K * H)) * P 
 
    return x,P 

这会导致极不正确的估计。我们不确定我们在这里做错了什么——我认为我们正确地遵循了所有步骤,但不确定我们是否拥有正确的所有必需矩阵。任何输入都会有所帮助!

请您参考如下方法:

置顶评论描述错误。您的状态必须是 x, y, velocity, angle, angularVelocity

您缺少 Q,过程协方差。它应该反射(reflect)您的状态在更新之间可以改变多少,并在预测步骤中添加到 P 的更新中。

您正在构建 EKF(因为您的更新需要触发,所以它是非线性的)。您已经构造了一个执行状态更新的矩阵 F,但是过程协方差更新所需的是更新函数的 Jacobian。在您的情况下,它看起来像:

J = matrix([ 
    [1., 0., cos(a), -sin(a), 0.], 
    [0., 1., sin(a), cos(a), 0.], 
    [0., 0., 1., 0., 0.], 
    [0., 0., 0., 1., dt], 
    [0., 0., 0., 0., 1.]]) 

如果您唯一的直接测量值是位置,则不应计算航向并将其用作测量值。让钦哲基金会来做吧。

您应该将R 设置为测量的实际不确定度。 Q、R 表示并在 P 中传播的噪声比您的状态 x 更重要,如果您愿意的话钦哲基金会工作。