Currently, I am trying to navigate a small robot car to point A from my current position. The car has a GPS sensor and a BNO055 IMU(Gyro + Mag + Acc). I know the GPS co-ordinates of point A. Using the GPS co-ordinates of my car, I can calculate the bearing angle and the distance. From the fusion mode in the BNO055, I get the tilt compensated heading again(It gives me heading, roll & pitch). Based on the heading angle and the bearing angle, my car makes its turns. However, here is what I want to build on this:

  1. The GPS readings sometimes are a bit far from my actual position. How to get rid of these outliers? I've read of the use of EKF. But they seem to be all mathematical formulas and it's tough to keep up with them!
  2. For when the GPS signal is lost, I've read about Dead Reckoning methods being used. I'm not sure how to implement those. Can someone shed some light on that?
  3. Can I calculate the speed with just the GPS and an IMU? Any help is appreciated, thanks!

3 Answers 3


You should use a Kalman Filter. Here are two nice tutorials that explain how Kalman Filter algorithm works and the working principle of IMU/GPS sensors.

short: https://www.navlab.net/Publications/Introduction_to_Inertial_Navigation.pdf

extended: https://www.navlab.net/Publications/Introduction_to_Inertial_Navigation_and_Kalman_Filtering.pdf

vectors, coordinate systems: https://www.navlab.net/nvector/

In summary, the Kalman Filter works in two steps:

1) prediction: - uses IMU measurements - propagates the belief (mean, covariance) based on the motion model

2) update step - uses GPS measurements - fuses the predicted belief and measurements to get a better estimate

The Kalman Filter algorithm implementation is very straightforward. There are hundreds of papers about how to code the filtering algorithm. The challenging parts are

  • modeling the sensors (error characteristics)

    for IMU, you need to come up with drift, bias, random noise terms

    for GPS, you need to come up with confidence intervals for global coordinate measurements (https://www.mathworks.com/help/fusion/gs/model-imu-gps-and-insgps.html)

  • understanding when the estimates are not consistent

  • modeling the motion of your vehicle (motion model)

  • modeling the relation between measurements and the state of the system (measurement model)


The simplest answer to all those questions, is to use an EKF. But since you are not familiar with the mathematical formulas of EKF here are some possible methods which might be useful for your problems. Here I'm trying to explain some steps of the EKF in a simple manner.

Let's start with your second question. Dead Reckoning method can be used to estimate the current position using raw odometry data such as wheel encoder readings. Assume that you know the current heading and the velocity of the robot at time t. Then you can calculate it's position in the next second (t+1) assuming that velocity is constant. But the calculated position may differs from the actual position due to various reasons such as drifts, velocity changes etc. This is what basically happens in dead reckoning. You can use the linear acceleration values from IMU to calculate the X and Y coordinates of the robot, using S = ut + 0.5at^2. But the problem in this method is, the error accumulates with the time. In that case we fuse GPS readings with IMU readings using an EKF.

For the third question, you can calculate the speed only with the IMU readings. First calculate the linear acceleration values in world coordinate frame, then use v = u +at to calculate the current velocity.

v - current velocity u - starting velocity a - acceleration t- time

In this equation we assume that the acceleration is constant during the time step t. Therefore use a value for the time step as small as possible (around 0.01).

For the first question, I can suggest this method. usually GPS readings has a variance about 3.5m. You can measure the difference between two consecutive readings and if the difference is higher than a threshold (let's say 5m) that reading should be ignored. Until you receive a good measurement you can estimate your position with dead reckoning.

I strongly recommend you to use EKF since it provides the best solution for all these questions. No need to understand what is happening inside. MATLAB has an inbuilt function for EKF.

  • $\begingroup$ Tharindu, Thank you so much for the brief response. About the second question, my plan to use dead reckoning is when the GPS signal is lost. I understand using just accelerometer, the errors accumulate over time and it's not effective. BUT since I'm using it when GPS signal is lost(i.e. No more GPS data - maybe tunnels or something), I am not sure what you meant by fusing GPS data with IMU using EKF as there won't be GPS data. Can you please shed some light on that? $\endgroup$
    – Anony
    Mar 22, 2019 at 13:46
  • $\begingroup$ Its not relevant for dead reckoning. Fusing GPS data with IMU measurement is the solution for error accumulation and it cannot be performed where your GPS signal is lost. $\endgroup$ Mar 25, 2019 at 4:13

One way to improve GPS accuracy is RTK GPS. RTK GPS is what some of these robot lawnmowers use because of its centimeters accuracy. https://learn.sparkfun.com/tutorials/what-is-gps-rtk/all


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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