2d Kalman filter with acceleration
<p>I would like to implement a kalman filter in Python for some tracking software I'm working on. It is a 2D coordinate system using a single vector x for position, velocity and acceleration of x and y coordinates,</p>

<p><a href="https://i.stack.imgur.com/XUckm.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/XUckm.png" alt="enter image description here"></a></p>

<p>I am using the following update and predict method:</p>

<pre><code># 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
</code></pre>

<p>Using other resources I have come up with the following state transition matrix and measurement matrix but am not sure how correct they are:</p>

<pre><code>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.'''),
</code></pre>

<p>Basically it doesn't work and makes my tracked path more jittery and I have no clue where I'm going wrong. Can anyone help?</p>