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:
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.
- predict when IMU fires event
- 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:
- 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).
- Is it OK to use formulas from Linear motion? Because I have Curvilinear motion in my case.
- 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)
- What if after all manipulations velocity is still inaccurate? Should I apply additional instruments after KF?
- 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!