My Algorithm to Calculate Position of Smartphone - Gps and Sensors

My Algorithm to Calculate Position of Smartphone - GPS and Sensors

As some of you mentioned you got the equations wrong but that is just a part of the error.

1. Newton - D'Alembert physics for non relativistic speeds dictates this:

   // init values
   double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
   double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
   double  x=0.0, y=0.0, z=0.0; // position [m]
   
   // iteration inside some timer (dt [seconds] period) ...
   ax,ay,az = accelerometer values
   vx+=ax*dt; // update speed via integration of acceleration
   vy+=ay*dt;
   vz+=az*dt;
    x+=vx*dt; // update position via integration of velocity
    y+=vy*dt;
    z+=vz*dt;
   

2. the sensor can rotate so the direction must be applied:

   // init values
   double gx=0.0,gy=-9.81,gz=0.0; // [edit1] background gravity in map coordinate system [m/s^2]
   double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
   double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
   double  x=0.0, y=0.0, z=0.0; // position [m]
   double dev[9]; // actual device transform matrix ... local coordinate system
   (x,y,z) <- GPS position;
   
   // iteration inside some timer (dt [seconds] period) ...
   dev <- compass direction
   ax,ay,az = accelerometer values (measured in device space)
   (ax,ay,az) = dev*(ax,ay,az);  // transform acceleration from device space to global map space without any translation to preserve vector magnitude
   ax-=gx;    // [edit1] remove background gravity (in map coordinate system)
   ay-=gy;
   az-=gz;
   vx+=ax*dt; // update speed (in map coordinate system)
   vy+=ay*dt;
   vz+=az*dt;
    x+=vx*dt; // update position (in map coordinate system)
    y+=vy*dt;
    z+=vz*dt;
   

   • gx,gy,gz is the global gravity vector (~9.81 m/s^2 on Earth)
   • in code my global Y axis points up so the gy=-9.81 and the rest are 0.0

3. measure timings are critical

   Accelerometer must be checked as often as possible (second is a very long time). I recommend not to use timer period bigger than 10 ms to preserve accuracy also time to time you should override calculated position with GPS value. Compass direction can be checked less often but with proper filtration

4. compass is not correct all the time

   Compass values should be filtered for some peak values. Sometimes it read bad values and also can be off by electro-magnetic polution or metal enviroment. In that case the direction can be checked by GPS during movement and some corrections can be made. For example chech GPS every minute and compare GPS direction with compass and if it is constantly of by some angle then add it or substract it.

5. why do simple computations on server ???

   Hate on-line waste of traffic. Yes you can log data on server (but still i think file on device will be better) but why to heck limit position functionality by internet connection ??? not to mention the delays ...

[Edit 1] additional notes

Edited the code above a little. The orientation must be as precise as it can be to minimize cumulative errors.

Gyros would be better than compass (or even better use them both). Acceleration should be filtered. Some low pass filtering should be OK. After gravity removal I would limit ax,ay,az to usable values and throw away too small values. If near low speed also do full stop (if it is not a train or motion in vacuum). That should lower the drift but increase other errors so an compromise has to be found between them.

Add calibration on the fly. When filtered acceleration = 9.81 or very close to it then the device is probably stand still (unless its a flying machine). Orientation/direction can be corrected by actual gravity direction.

See if the user looks at a GPS position

As per your question, in your case GPS receiver is only collecting absolute location with margin (inaccurate) of 3-10 meter depending on your receiver type.
When your device returns the location of latitude and longitude, it does not know your orientation, if we assume you only use GPS, then the only way to calculate orientation is by movement the receiver (movement mean walking for while constantly).

To get your orientation typically using Magnetic Compass and/or Gyroscopes build in your device.

There are no final rules how to use this or that, what you need to do is to find out what is your requirement and make some tests to find the best results.

One disadvantage of using GPS constantly, is that it is power consuming (it drain the battery) so you need to make a strategy of combining differing sensors and turn on/off GPS programmatically to collecting data periodically.

And make algorithm logic to give the user the orientation regarding to the position.

I have provided some useful links:

• https://www.geolounge.com/absolute-relative-location/
• https://www.quora.com/Sensors/What-is-the-difference-between-accelerometers-gyroscopes-and-magnetometers
• http://www.explainthatstuff.com/howgpsworks.html
• http://www.tracklogs.co.uk/cgi-bin/publ/user.cgi?obj=support&mod=kbase&view=15885925
• How do I programmatically find direction on a blackberry?

GPS/GIS Calculations: Algorithm to predict future position based on movement/mph?

Here is the complete parametric answer :

variables :

• heading : heading (i.e. backwards angle from azimuth 0°, in degrees)
• speed : velocity (i.e. norm of the speed vector, in miles/hour)
• lat0, lon0 : initial coordinates in degrees
• dtime : time interval from the start position, in seconds
• lat, lon : predicted coordinates in degrees
• pi : the pi constant (3.14159...)
• Rt : Earth radius in miles (6378137.0 meters which makes 3964.037911746 miles)

In an (East, North) local frame, the position after the time interval is :

x = speed * sin(heading*pi/180) * dtime / 3600;
y = speed * cos(heading*pi/180) * dtime / 3600;

(with coordinates in miles)

From there you can compute the new position in the WGS84 frame (i.e. latitude and longitude) :

lat = lat0 + 180 / pi * y / Rt;
lon = lon0 + 180 / pi / sin(lat0*pi/180) * x / Rt;

Edit : corrected the last line : *sin(phi) to /sin(phi)

The angle of direction without using GPS

If your device is fixed relative to the car, you can extrapolate your coordinates after the last known fix using the previous known speed and input from the magnetometer.

As you updated your question, if the device is not fixed then it's more difficult. You'd have to use some some sensor fusion algorithm that could adjust the speed calculation based on the accelerometer and gyro inputs, and (have a look at how Kalman filters work for example).

The information from that algorithm could be correlated with previous GPS fixes in order to estimate the position of the device in relation to the car (think about what the magnetometer would read during, let's say, a right turn on the road if the phone is in the left pocket vs. if it's in the right pocket). Unfortunately, you don't have the comfort of knowing that the phone is always pointing straight forward through the windshield.

---

Related Topics