# Kalman filtering for position using GPS,accelerometer and speed sensors

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?

• you should use the angle too. The measurement of velocity is in the tangent direction of the sensor. How you estimate the variation of the direction of the sensor? May 12, 2020 at 12:46
• Are the velocity and position vectors in the car's frame of reference? How do you know the car's orientation with respect to the GPS frame of reference (which is most certainly ECEF)? How do you maintain an estimate of the car's orientation? May 12, 2020 at 15:24
• @GideonGenadiKogan. Yes for calculating velocity, im using the angle from the magnetometer (this is precalibrated and adjusted for declination) May 13, 2020 at 6:52

## 2 Answers

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?

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

• So u mean, u activate the kalman filtering only when the accuracy of GPS is bad(based on HDOP,VDOP etc) and not in the other scenarios cos kalman builds errors over time? May 13, 2020 at 6:49
• from my observations: GPS positions 1) can be jumpy 2) can linearly drift with time 3) can latch to its output(or) stop giving an output (I guess this is your case when Rx doesn't receive signal at all) .. Case 1 and 2 are when rx successfully decodes a GPS info, but due to high multipath (For Ex: Case 1 -> Downtowns, Case 2: As soon as you enter a short tunnel for like 5~10 seconds).. May 13, 2020 at 16:21
• Try to keep all info in same reference system (either in absolute position i.e ECEF or vehicle frame)You have two sets of position information: One from vehicle state data (position.speed,acceleration and yaw rate) , and other from GPS receiver itself... Kalman tries to use both these information to estimate the output.. and HDOP,VDOP,GDOP can help you for case 1 and case 2 to adjust the weight vector to trust the information. May 13, 2020 at 16:25
• my doubt is with respect to your 3rd point where u say to use HDOP in if else to activate the algo and u talk about kalman building errror after 30 seconds. Does that mean u use kalman not eveytime and activate this algorithm based on some if else condition May 13, 2020 at 18:53