Question

请看一下：

def kalman(mu,P,F,Q,B,u,z,H,R):
# mu, P : current state and its uncertainty
# F, Q  : Dynamic system and its noise
# B, u  : control model and the entrance
# z     : observation
# H, R  : Observation model and its noise

mup = F @ mu + B @ u;
pp  = F @ P @ F.T + Q;

zp = H @ mup

# if there is no observation we only do prediction

if z is None:
    return mup, pp, zp

epsilon = z - zp

k = pp @ H.T @ la.inv(H @ pp @ H.T +R)

new_mu = mup + k @ epsilon
#print(new_mu)
new_P  = (np.eye(len(P))-k @ H) @ pp
return new_mu, new_P, zp

我在这段代码上使用了这个kalman过滤器：

mu,P,pred = kalman(mu,P,F,Q,B,a,np.array([x,y]),H,R)


    xe.append(mu[0])
    ye.append(mu[1])
    xu.append(2 * np.sqrt(P[0, 0]))
    yu.append(2 * np.sqrt(P[1, 1]))



    P2 = P
    mu2 = mu
    res2 = []

    for __ in range (fps * 2):
        mu2, P2, pred2 = kalman(mu2, P2, F, Q, B, a, None, H, R)
        xp.append(mu2[0])
        yp.append(mu2[1])
        xpu.append(2 * np.sqrt(P[0, 0]))
        ypu.append(2 * np.sqrt(P[1, 1]))



    for n in range(len(xp)):

        uncertainity_in_state = (xpu[n] + ypu[n]) / 2
        cv2.circle(image, (int(xp[n]), int(yp[n])),  int(uncertainity_in_state), (0,0,255))

        cv2.circle(image, (int(x), int(y)), int(radius), (0, 255, 255), -2) 


    cv2.imshow('tracking', image)

np.array([x, y])是篮球的中心线，我试图预测它的轨迹。

矩阵也是：

a = np.array([0, 900])
F = np.array(
[1, 0, dt, 0,
0, 1, 0, dt,
0, 0, 1, 0,
0, 0, 0, 1 ]).reshape(4,4)


B = np.array(
[dt**2/2, 0,
0, dt**2/2,
dt, 0,
0, dt ]).reshape(4,2)


H = np.array(
[1,0,0,0,
0,1,0,0]).reshape(2,4)


mu = np.array([0,0,0,0])
P = np.diag([1000,1000,1000,1000])**2


sigmaM = 0.0001
sigmaZ = 3*noise

Q = sigmaM**2 * np.eye(4)
R = sigmaZ**2 * np.eye(2)

问题在于，似乎预测的产生方向与篮球运动有关。

Check out this image

您可以看到预测的圆圈被绘制为好像它们从屏幕突出。此外，我可以正确跟踪球，因为你可以看到他们的黄色圆圈。

请帮帮我。我一直在努力工作几个小时。

感谢。

本土卡尔曼过滤器的问题

0 个答案: