现在我们正在尝试创建一个 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 更重要,如果您愿意的话钦哲基金会工作。
