# Error with simple Extended Kalman Filter simulation in Python

I am trying to write a Python simulation for a bearing-only EKF tracking problem. I wish to estimate the $$x$$ and $$y$$ position and velocity vectors for an object, so my state vector is $$\mathbf{x} = [x,y,\dot{x},\dot{y}]^T.$$ I am using a very simple constant-velocity dynamic model so my Jacobian $$J_f$$ is simply $$\mathbf{J}_f = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}.$$

Meanwhile there are two sensors whose, positions are known, which give bearing only information. Thus the measurement model is \begin{align*} \theta_n &= \mathrm{arctan2}\left( \frac{y-c_{n,y}}{x-c_{n,x}} \right) + w_n \\ \mathbf{Y} &= \begin{bmatrix} \theta_1 & \theta_2 \end{bmatrix}^T \end{align*} where $$c_{n,x}$$ and $$c_{n,y}$$ give the coordinates of the sensors. There are only two sensors. Then I believe the Jacobian of the measurement model is given by

$$\mathbf{J}_h = \begin{bmatrix} \frac{c_{1,y}-y}{(x-c_{1,x})^2+(y-c_{1,y})^2} & \frac{x-c_{1,x}}{(x-c_{1,x})^2+(y-c_{1,y})^2} & 0 & 0 \\ \frac{c_{2,y}-y}{(x-c_{2,x})^2+(y-c_{2,y})^2} & \frac{x-c_{2,x}}{(x-c_{2,x})^2+(y-c_{2,y})^2} & 0 & 0 \end{bmatrix}.$$

I tried coding this up in Python for ten iterations but this is the result I get.

Here the red crosses represent the true path of the object while the blue dots represent the estimated path, and clearly it is very far off. So far what I've been able to identify is that the motion model is accurate but somewhere between estimating the error covariance, the Kalman gain, and the update step there is some underlying bug, but I just can't find it. Any help would be hugely appreciated! (and please let me know if I am posting to the wrong community) Code below:


import numpy as np
from numpy import sin, cos, arctan2, arctan
import matplotlib.pyplot as plt
get_ipython().magic(u'pylab inline')

cx1 = 1.0
cy1 = 1.0
cx2 = 5.0
cy2 = 5.0
dt = 1.0
vx = 1.0
vy = -1.0
sigu = 0.1
sigw = 0.1

def FJacobian():
return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])

def HJacobian(xstate):
x = xstate[0]
y = xstate[1]
denom1 = (x-cx1)**2 + (y-cy1)**2
denom2 = (x-cx2)**2 + (y-cy2)**2
return np.array([[(cy1-y)/denom1, (x-cx1)/denom1, 0, 0],[(cy2-y)/denom2, (x-cx2)/denom2, 0, 0]])

def walk(xstate):
xstate_new = xstate
xstate_new[0] += xstate[2]*dt
xstate_new[1] += xstate[3]*dt
xstate_new[2] += np.random.normal(0,sigu)
xstate_new[3] += np.random.normal(0,sigu)
return xstate_new

def get_bearing(xstate):
x = xstate[0]
y = xstate[1]
theta1 = arctan2(y-cy1,x-cx1)
theta2 = arctan2(y-cy2,x-cx2)
# introduce noise
theta1 += np.random.normal(0,sigw)
theta2 += np.random.normal(0,sigw)
return np.array([theta1, theta2])

def get_bearing_noiseless(xstate):
x = xstate[0]
y = xstate[1]
theta1 = arctan2(y-cy1,x-cx1)
theta2 = arctan2(y-cy2,x-cx2)
return np.array([theta1, theta2])

def predict_state(xstate):
return FJacobian() @ xstate

def predict_covariance(S):
J_F = FJacobian()
Q = np.array([[0,0,0,0],[0,0,0,0],[0,0,sigu**2,0],[0,0,0,sigu**2]])
return J_F @ S @ J_F.T + Q

def get_kalman_gain(S_pre,J_H):
R = np.eye(2) * sigw**2
return S_pre @ J_H.T @ np.linalg.inv(J_H @ S_pre @ J_H.T + R)

def update_state(xstate_pre, K, Y, h):
return xstate_pre + K @ (Y - h)

def update_covariance(K, J_H, S_pre):
return (np.eye(4) - K @ J_H) @ S_pre

def init():
xstate=np.zeros(4)
xstate[:2] = 10*np.random.rand(2)-5
xstate[2:] = 4*np.random.rand(2)-2
mu = 0
S = np.outer((xstate-mu),(xstate-mu))
return xstate, S

xstate_true = [-2,7,vx,vy]
xstate, S = init()
for t in np.arange(3):
xstate_pre = predict_state(xstate)
S_pre = predict_covariance(S)
J_H = HJacobian(xstate_pre)

xstate_true = walk(xstate_true)
Y = get_bearing(xstate_true)
h = get_bearing_noiseless(xstate_pre)

K = get_kalman_gain(S_pre,J_H)
xstate_up = update_state(xstate_pre,K,Y,h)
S_up = update_covariance(K,J_H,S_pre)
xstate = xstate_up
S = S_up
$$$$
`