带加速度的2d卡尔曼滤波器

Question

我想在Python中为我正在使用的一些跟踪软件实现一个kalman过滤器。它是一个2D坐标系，使用单个向量x来表示x和y坐标的位置，速度和加速度，

我正在使用以下更新和预测方法：

# UPDATE
y = Z - (H * x)
S = H * P * H.T + R  # residual convariance
K = P * H.T * S.I    # Kalman gain
x = x + K*y
I = np.matrix(np.eye(F.shape[0])) # identity matrix
P = (I - K*H)*P

# PREDICT x, P based on motion
x = F*x
P = F*P*F.T + Q

return x, P

使用其他资源我已经提出了以下状态转换矩阵和测量矩阵，但不确定它们是多么正确：

F = np.matrix('''
      1. 0. 1. 0. 0.5 0.;
      0. 1. 0. 1. 0. 0.5;
      0. 0. 1. 0. 1. 0.;
      0. 0. 0. 1. 0. 1.;
      0. 0. 0. 0. 1. 0.;
      0. 0. 0. 0. 0. 1.
      '''),
 H = np.matrix('''
      1. 0. 0. 0. 0. 0.;
      0. 1. 0. 0. 0. 0.;
      0. 0. 0. 0. 1. 0.;
      0. 0. 0. 0. 0. 1.'''),

基本上它不起作用，让我的跟踪路径更加紧张，我不知道我哪里出错了。有人可以帮忙吗？