# Overcorrecting Kalman Filter

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?

There is an error in your posted equation for the Jacobian $F_J$, so that could be the source of the problem. It should look like this:
$F_J = \begin{bmatrix} 1 & 0 & -C \sin \theta \\ C \frac{\sin \theta \sin \lambda}{\cos^2 \lambda} & 1 & C \frac{\cos \theta}{\cos \lambda} \\ 0 & 0 & 1 \\ \end{bmatrix}$