I'm trying to get an extended Kalman Filter to work. My System Model is:
$ x = \begin{bmatrix} lat \\ long \\ \theta \end{bmatrix}$
where lat and long are latitude and longitude (in degree) and $\theta$ is the current orientation of my vehicle (also in degree). In my Prediction Step I get a reading for current speed v, yaw rate $\omega$ and inclination angle $\alpha$:
$z = \begin{bmatrix} v \\ \alpha\\ \omega \end{bmatrix}$
I use the standard prediction for the EKF with $f()$ being:
$ \vec{f}(\vec{x}_{u,t}, \vec{z}_t) = \vec{x}_{u,t} + \begin{bmatrix} \frac{v}{f} * \cos(\theta) * \cos(\alpha) * \frac{180 °}{\pi * R_0} \\ \frac{v}{f} * \sin(\theta) * \cos(\alpha) * \frac{180 °}{\pi * R_0} * \frac{1}{\cos(lat)} \\ \frac{\omega}{f} \end{bmatrix} $
$f$ being the prediction frequency, $R_0$ being the radius of the earth (modelling the earth as a sphere)
My Jacobian Matrix looks like this:
$ C = v \cdot \Delta t \cdot cos(\alpha) \cdot \frac{180}{\pi R_0} $
$ F_J = \begin{pmatrix} 1 & 0 & -C \cdot sin(\phi) \cdot \frac{1}{cos(lat)} \\ -C \cdot sin(\phi) \cdot \frac{sin(lat)}{{cos(lat)}^2} & 1 & C \cdot cos(\phi) \cdot \frac{1}{cos(lat)}\\ 0 & 0 & 1 \end{pmatrix} $
As I have a far higher frequency on my sensors for the prediction step, I have about 10 predictions followed by one update.
In the update step I get a reading for the current GPS position and calculate an orientation from the current GPS position and the previous one. Thus my update step is just the standard EKF Update with $h(x) = x$ and thus the Jacobian Matrix to $h()$, $H$ being the Identity.
Trying my implementation with testdata where the GPS Track is in constant northern direction and the yaw rate constantly turns west, I expect the filter to correct my position close to the track and the orientation to 355 degrees or so. What actually happens can be seen in the image attached (Red: GPS Position Measurements, Green/blue: predicted positions): 
I have no Idea what to do about this. I'm not very experienced with the Kalman filter, so it might just be me misunderstanding something, but nothing I tried seemed to work…
What I think:
I poked around a bit: If I set the Jacobian Matrix in the prediction to be the identity, it works really good. The Problem seems to be that $P$ (the covariance Matrix of the system model) is not zero in $P(3,1)$ and $P(3,2)$. My interpretation would be that in the prediction step the Orientation depends on the Position, which does not seem to make sense. This is due to $F_J(2,1)$ not being zero, which in turn makes sense.
Can anyone give me a hint where the overcorrection may come from, or what I should look at / google for?
