5
$\begingroup$

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): 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?

$\endgroup$
3
$\begingroup$

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}$

With that new Jacobian I get results that look like:

GPS EKF results

$\endgroup$
  • $\begingroup$ Nice catch, and welcome to the Robotics SE! $\endgroup$ – Chuck Oct 15 '15 at 12:09
  • $\begingroup$ Thanks! Finally had to ask a question on Overflow so I signed up and am keen to contribute. :) $\endgroup$ – Brian Lynch Oct 15 '15 at 15:15
  • $\begingroup$ Welcome to Robotics Brian, great first post, I look forward to your future contributions. $\endgroup$ – Mark Booth Oct 16 '15 at 12:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.