Kalman filtering for position using GPS,accelerometer and speed sensors

Question

I am working on tracking a vehicle under tunnel when GPS is lost. Whenever the vehicle in on the road, the GPS works fine and gives good accuracy but when the vehicle is under tunnel, the GPS is lost and its difficult to track vehicle. This is where i have decided to use kalman filtering. I have a 9 axis IMU sensor(accelerometer,gyro,magnetometer) and speed value from Candata and would like to predict the location using kalman So far, this is wat i have done

1. Px(t+1) = Px + delta_t * vx + 0.5 * ax * delta_t 2
2. Py(t+1) = Py + delta_t * vy + 0.5 * ay * delta_t 2
3. Vx(t+1) = Vx + ax * delta_t
4. Vy(t+1) = Vy + ay * delta_t

where px,py are my positions and vx and vy are my velocities this is my statemodel my input is the abs acceleration in x and y direction calculated from 9 axis IMU using all 3 sensors. and in my update step i use the gps value i received. So my question here is, where do i use my speed value? Is accelerometer enough?

Answer 1

In my experience at the very least you need a nine or ten state Kalman (velocity, position, and angle -- that's nine states if you attempt to use Euler angles, ten if you use quaternions). Practically, you need to use IMU offset, so add six more states. If you use the IMU's compass function, add three more. It may not be a bad idea to monitor the acceleration due to gravity -- one more. If you use the vehicle's odometer function (from CAN), then add a state for proportionality error, and possibly offset error.

You need vehicle orientation. That's because the ground is accelerating upwards at $9.81 \mathrm{m/s^2}$ (if Einstein is to be believed), or the accelerometer's proof mass is being pulled down with a force equal to $9.81 \mathrm{m/s^2/kg}$ (if Sir Isaac Newton is to be believed). Either way, to get the actual acceleration with respect to the ground you need to null out the acceleration with respect to gravity, and you can only do that if you have the vehicle's orientation in 3D.

It's been over a decade since I worked on this last, but there has to be canned packages available open source -- have you done a search?

Answer 2

A couple of points I noticed when I was working on this exercise (commonly known as dead reckoning) near airport tunnels where my GPS is jumpy (or) completely lost(in this case , it is lagged to the previous position).

In my case, I was using u-blox GPS receiver (data coming at 1Hz) , vehicle state data from CAN and baseline reference from centimetre grade GPS receiver (data coming at 100 Hz)

• I would also add yaw rate in the set of equations (it's super noisy, so needs filtering.) Filtering on yaw rate depends on curvature of the road too (yaw rate from CAN data tends to erroneous while the vehicle is curving )

• using Kalman is good for like 30 seconds . the error builds up drastically more than that (that's what i observed in my DR and I had to add lots of if-else loops.)

• try using HDOP , VDOP and GDOP in your if-else loops to activate the algo