Kalman filter GPS + IMU fusion get accurate velocity with low cost sensors

Question

I'm new to all this robotics stuff. Especially to Kalman filter.

My initial goal is to have velocity as accurate as possible

Here is my case:

I have a phone which is mounted, for example in the car. So it has low cost GPS and IMU sensors. 2D

GPS gives me:

• position (longitude, latitude, altitude)
• position accuracy (error can't be split into east, north, up directions)
• speed
• speed accuracy (error can't be split into east, north, up directions)
• heading angle
• heading angle accuracy

IMU: (separated accelerometer, gyroscope and magnetometer). I fuse them myself

Actually it's needed to be mentioned that I can't use magnetometer in my case. Since "car" is faraday cage. So I only can fuse accelerometer and gyroscope. Both of them are outputs from Madgwick AHRS (can get from here rotation matrix, quaternion if needed) and represented in North, East, Up dimensions.

What I've done so far:

• I've implemented linear KF with position & velocity states. But I didn't achieve desired accuracy.

Get rid of IMU data from chart above.

It's IMU causes that drift. I have GPS updates every 1 second. And IMU with 13 Hz frequency. We can see here that every 13th iteration we have GPS updates and then IMU goes rogue.

Used approach:
Since I have GPS 1Hz and IMU upto 100Hz. But I took 13Hz in my case. Since I don't need to have so many updates.

1. predict when IMU fires event
2. When GPS fires event. I take latest IMU data. Do predict and then gps (position, velocity) update.

Since my primary goal is to have accurate velocity.
I don't care much about position and heading angle but... Since velocity correlates with them they can be added to Kalman Filter. Am I right?

So my Kalman states are position, velocity and heading angle.

Can I use something like?

$$ x = x_i + v_i\Delta t + \frac{a_i\Delta t}{2} $$ $$ v = v_i + a_i\Delta t $$ $$ \theta = \theta_i + w_i\Delta t $$ Questions:

1. Could velocity benefit from adding position and heading angle as states to Kalman Filter. Since there is some correlation between them. (Angular velocity impacts on velocity itself).
2. Is it OK to use formulas from Linear motion? Because I have Curvilinear motion in my case.
3. Almost all papers describe (position, velocity) model with KF. Can I take advantage in using EKF? I found some papers that mention odometry word. And seems like they have the same model. (pos, velocity, angle)
4. What if after all manipulations velocity is still inaccurate? Should I apply additional instruments after KF?
5. Can I somehow take advantage of current location and prev. location points? (For example, calculate velocity from two points. Of course it means that my unit moves linear and not by curve). Then somehow correct my predicted KF result with this velocity.

Please help me with modeling Kalman Filter. And give an advice how to achieve best velocity accuracy.

Thanks!

Answer 1

I would say, this site may helps you to understand where the power of the Kalman-filter lies. http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

In my humble opinion:

1. I think basic useful Kalman-filter requires position and speed at least, because as you've said they correlate to each other.

2. There is an important thing to do before even implementing anything. Doing an analysis of the motion. I mean by that, what sort of motion does your system produce? A simple Kalman-filter is best at linear motion prediction. If it is non-linear, you have to be clever on how to set up the process noise Q parameter. Let me give you and example: You have small vessel you are tracking with a radar. It is known, that your vessel has a non-linear speed (it is not known for the process, when the vessel slows down). In order to adjust the process noise to the situation, hence increasing accuracy, you have to have imply some knowledge in the algorithm, in what cases the speed turns more non-linear. Like, small vessel is usually more manouverable, than a 200 m long one, so Q shall set considering the size for example.

3. See four.

4. I would say, a 3 state (pos/speed/acc) KF, should do the job if you include angle, then it is even better. Although, if you motion is simple you could also consider an alpha-beta filter as well, which is less intelligent about the history of the motion, but quite fast.

5. Basically the first state of the KF shall make use of the previous and current position.

I hope this gives a little kick in your project.