My Algorithm to Calculate Position of Smartphone - GPS and Sensors

Question

I am developing an android application to calculate position based on Sensor's Data

1. Accelerometer --> Calculate Linear Acceleration

2. Magnetometer + Accelerometer --> Direction of movement

The initial position will be taken from GPS (Latitude + Longitude).

Now based on Sensor's Readings i need to calculate the new position of the Smartphone:

My Algorithm is following - (But is not calculating Accurate Position): Please help me improve it.

Note: My algorithm Code is in C# (I am sending Sensor Data to Server - Where Data is stored in the Database. I am calculating the position on Server)

All DateTime Objects have been calculated using TimeStamps - From 01-01-1970

    var prevLocation = ServerHandler.getLatestPosition(IMEI);
    var newLocation = new ReceivedDataDTO()
                          {
                              LocationDataDto = new LocationDataDTO(),
                              UsersDto = new UsersDTO(),
                              DeviceDto = new DeviceDTO(),
                              SensorDataDto = new SensorDataDTO()
                          };

    //First Reading
    if (prevLocation.Latitude == null)
    {
        //Save GPS Readings
        newLocation.LocationDataDto.DeviceId = ServerHandler.GetDeviceIdByIMEI(IMEI);
        newLocation.LocationDataDto.Latitude = Latitude;
        newLocation.LocationDataDto.Longitude = Longitude;
        newLocation.LocationDataDto.Acceleration = float.Parse(currentAcceleration);
        newLocation.LocationDataDto.Direction = float.Parse(currentDirection);
        newLocation.LocationDataDto.Speed = (float) 0.0;
        newLocation.LocationDataDto.ReadingDateTime = date;
        newLocation.DeviceDto.IMEI = IMEI;
        // saving to database
        ServerHandler.SaveReceivedData(newLocation);
        return;
    }


    //If Previous Position not NULL --> Calculate New Position
   **//Algorithm Starts HERE**

    var oldLatitude = Double.Parse(prevLocation.Latitude);
    var oldLongitude = Double.Parse(prevLocation.Longitude);
    var direction = Double.Parse(currentDirection);
    Double initialVelocity = prevLocation.Speed;

    //Get Current Time to calculate time Travelling - In seconds
    var secondsTravelling = date - tripStartTime;
    var t = secondsTravelling.TotalSeconds;

    //Calculate Distance using physice formula, s= Vi * t + 0.5 *  a * t^2
    // distanceTravelled = initialVelocity * timeTravelling + 0.5 * currentAcceleration * timeTravelling * timeTravelling;
    var distanceTravelled = initialVelocity * t + 0.5 * Double.Parse(currentAcceleration) * t * t;

    //Calculate the Final Velocity/ Speed of the device.
    // this Final Velocity is the Initil Velocity of the next reading
    //Physics Formula: Vf = Vi + a * t
    var finalvelocity = initialVelocity + Double.Parse(currentAcceleration) * t;


    //Convert from Degree to Radians (For Formula)
    oldLatitude = Math.PI * oldLatitude / 180;
    oldLongitude = Math.PI * oldLongitude / 180;
    direction = Math.PI * direction / 180.0;

    //Calculate the New Longitude and Latitude
    var newLatitude = Math.Asin(Math.Sin(oldLatitude) * Math.Cos(distanceTravelled / earthRadius) + Math.Cos(oldLatitude) * Math.Sin(distanceTravelled / earthRadius) * Math.Cos(direction));
    var newLongitude = oldLongitude + Math.Atan2(Math.Sin(direction) * Math.Sin(distanceTravelled / earthRadius) * Math.Cos(oldLatitude), Math.Cos(distanceTravelled / earthRadius) - Math.Sin(oldLatitude) * Math.Sin(newLatitude));

    //Convert From Radian to degree/Decimal
    newLatitude = 180 * newLatitude / Math.PI;
    newLongitude = 180 * newLongitude / Math.PI;

This is the Result I get --> Phone was not moving. As you can see speed is 27.3263111114502 So there is something wrong in calculating Speed but I don't know what

ANSWER:

I found a solution to calculate position based on Sensor: I have posted an Answer below.

If you need any help, please leave a comment

this is The results compared to GPS (Note: GPS is in Red)

Solution 1

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.

Solution 2

Acceleration sensors and gyros are not suited for position calculation.
After some seconds the errors become incredible high. (I hardly remember that the double integration is the problem).
Look at this Google tech talk video about sensor fusioning, he explains in very detail why this is not possible.

Solution 3

After solving the position I calculated using Sensors I would like to post my code here in case anyone needs in future:

Note: This was only checked on Samsung Galaxy S2 phone and only when person was walking with the phone, it has not been tested when moving in car or on bike

This is the result I got when compared when compared with GPS, (Red Line GPS, Blue is Position calculated with Sensor)

The code is not very efficient, but I hope my sharing this code will help someone and point them in the right direction.

I had two seperate classes:

1. CalculatePosition

2. CustomSensorService

   public class CalculatePosition {

           static Double earthRadius = 6378D;
           static Double oldLatitude,oldLongitude;
           static Boolean IsFirst = true;
   
           static Double sensorLatitude, sensorLongitude;
   
           static Date CollaborationWithGPSTime;
           public static float[] results;
   
   
   
           public static void calculateNewPosition(Context applicationContext,
                   Float currentAcceleration, Float currentSpeed,
                   Float currentDistanceTravelled, Float currentDirection, Float TotalDistance) {
   
   
               results = new float[3];
               if(IsFirst){
                   CollaborationWithGPSTime = new Date();
                   Toast.makeText(applicationContext, "First", Toast.LENGTH_LONG).show();
                   oldLatitude = CustomLocationListener.mLatitude;
                   oldLongitude = CustomLocationListener.mLongitude;
                   sensorLatitude = oldLatitude;
                   sensorLongitude = oldLongitude;
                   LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor",0.0F,TotalDistance);
                   IsFirst  = false;
                   return;
               } 
   
               Date CurrentDateTime = new Date();
   
               if(CurrentDateTime.getTime() - CollaborationWithGPSTime.getTime() > 900000){
                   //This IF Statement is to Collaborate with GPS position --> For accuracy --> 900,000 == 15 minutes
                   oldLatitude = CustomLocationListener.mLatitude;
                   oldLongitude = CustomLocationListener.mLongitude;
                   LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor", 0.0F, 0.0F);
                   return;
               }
   
               //Convert Variables to Radian for the Formula
               oldLatitude = Math.PI * oldLatitude / 180;
               oldLongitude = Math.PI * oldLongitude / 180;
               currentDirection = (float) (Math.PI * currentDirection / 180.0);
   
               //Formulae to Calculate the NewLAtitude and NewLongtiude
               Double newLatitude = Math.asin(Math.sin(oldLatitude) * Math.cos(currentDistanceTravelled / earthRadius) + 
                       Math.cos(oldLatitude) * Math.sin(currentDistanceTravelled / earthRadius) * Math.cos(currentDirection));
               Double newLongitude = oldLongitude + Math.atan2(Math.sin(currentDirection) * Math.sin(currentDistanceTravelled / earthRadius)
                       * Math.cos(oldLatitude), Math.cos(currentDistanceTravelled / earthRadius)
                       - Math.sin(oldLatitude) * Math.sin(newLatitude));
   
               //Convert Back from radians
               newLatitude = 180 * newLatitude / Math.PI;
               newLongitude = 180 * newLongitude / Math.PI;
               currentDirection = (float) (180 * currentDirection / Math.PI);
   
               //Update old Latitude and Longitude
               oldLatitude = newLatitude;
               oldLongitude = newLongitude;
   
               sensorLatitude = oldLatitude;
               sensorLongitude = oldLongitude;
   
               IsFirst = false;
               //Plot Position on Map
               LivePositionActivity.PlotNewPosition(newLongitude,newLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "Sensor", results[0],TotalDistance);
   
   
   
   
   
       }
   }
   

   public class CustomSensorService extends Service implements SensorEventListener{

   static SensorManager sensorManager;
   static Sensor mAccelerometer;
   private Sensor mMagnetometer;
   private Sensor mLinearAccelertion;
   
   static Context mContext;
   
   private static float[] AccelerometerValue;
   private static float[] MagnetometerValue;
   
   public static  Float currentAcceleration = 0.0F;
   public static  Float  currentDirection = 0.0F;
   public static Float CurrentSpeed = 0.0F;
   public static Float CurrentDistanceTravelled = 0.0F;
   /*---------------------------------------------*/
   float[] prevValues,speed;
   float[] currentValues;
   float prevTime, currentTime, changeTime,distanceY,distanceX,distanceZ;
   float[] currentVelocity;
   public static CalculatePosition CalcPosition;
   /*-----FILTER VARIABLES-------------------------*-/
    * 
    * 
    */
   
   public static Float prevAcceleration = 0.0F;
   public static Float prevSpeed = 0.0F;
   public static Float prevDistance = 0.0F;
   
   public static Float totalDistance;
   
   TextView tv;
   Boolean First,FirstSensor = true;
   
   @Override
   public void onCreate(){
   
       super.onCreate();
       mContext = getApplicationContext();
       CalcPosition =  new CalculatePosition();
       First = FirstSensor = true;
       currentValues = new float[3];
       prevValues = new float[3];
       currentVelocity = new float[3];
       speed = new float[3];
       totalDistance = 0.0F;
       Toast.makeText(getApplicationContext(),"Service Created",Toast.LENGTH_SHORT).show();
   
       sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
   
       mAccelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
       mMagnetometer = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
       //mGyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
       mLinearAccelertion = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
   
       sensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_NORMAL);
       sensorManager.registerListener(this, mMagnetometer, SensorManager.SENSOR_DELAY_NORMAL);
       //sensorManager.registerListener(this, mGyro, SensorManager.SENSOR_DELAY_NORMAL);
       sensorManager.registerListener(this, mLinearAccelertion, SensorManager.SENSOR_DELAY_NORMAL);
   
   }
   
   @Override
   public void onDestroy(){
       Toast.makeText(this, "Service Destroyed", Toast.LENGTH_SHORT).show();
       sensorManager.unregisterListener(this);
       //sensorManager = null;
       super.onDestroy();
   }
   @Override
   public void onAccuracyChanged(Sensor sensor, int accuracy) {
       // TODO Auto-generated method stub
   
   }
   
   @Override
   public void onSensorChanged(SensorEvent event) {
   
       float[] values = event.values;
       Sensor mSensor = event.sensor;
   
       if(mSensor.getType() == Sensor.TYPE_ACCELEROMETER){
           AccelerometerValue = values;
       }
   
       if(mSensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION){           
           if(First){
               prevValues = values;
               prevTime = event.timestamp / 1000000000;
               First = false;
               currentVelocity[0] = currentVelocity[1] = currentVelocity[2] = 0;
               distanceX = distanceY= distanceZ = 0;
           }
           else{
               currentTime = event.timestamp / 1000000000.0f;
   
               changeTime = currentTime - prevTime;
   
               prevTime = currentTime;
   
   
   
               calculateDistance(event.values, changeTime);
   
               currentAcceleration =  (float) Math.sqrt(event.values[0] * event.values[0] + event.values[1] * event.values[1] + event.values[2] * event.values[2]);
   
               CurrentSpeed = (float) Math.sqrt(speed[0] * speed[0] + speed[1] * speed[1] + speed[2] * speed[2]);
               CurrentDistanceTravelled = (float) Math.sqrt(distanceX *  distanceX + distanceY * distanceY +  distanceZ * distanceZ);
               CurrentDistanceTravelled = CurrentDistanceTravelled / 1000;
   
               if(FirstSensor){
                   prevAcceleration = currentAcceleration;
                   prevDistance = CurrentDistanceTravelled;
                   prevSpeed = CurrentSpeed;
                   FirstSensor = false;
               }
               prevValues = values;
   
           }
       }
   
       if(mSensor.getType() == Sensor.TYPE_MAGNETIC_FIELD){
           MagnetometerValue = values;
       }
   
       if(currentAcceleration != prevAcceleration || CurrentSpeed != prevSpeed || prevDistance != CurrentDistanceTravelled){
   
           if(!FirstSensor)
               totalDistance = totalDistance + CurrentDistanceTravelled * 1000;
           if (AccelerometerValue != null && MagnetometerValue != null && currentAcceleration != null) {
               //Direction
               float RT[] = new float[9];
               float I[] = new float[9];
               boolean success = SensorManager.getRotationMatrix(RT, I, AccelerometerValue,
                       MagnetometerValue);
               if (success) {
                   float orientation[] = new float[3];
                   SensorManager.getOrientation(RT, orientation);
                   float azimut = (float) Math.round(Math.toDegrees(orientation[0]));
                   currentDirection =(azimut+ 360) % 360;
                   if( CurrentSpeed > 0.2){
                       CalculatePosition.calculateNewPosition(getApplicationContext(),currentAcceleration,CurrentSpeed,CurrentDistanceTravelled,currentDirection,totalDistance);
                   }
               }
               prevAcceleration = currentAcceleration;
               prevSpeed = CurrentSpeed;
               prevDistance = CurrentDistanceTravelled;
           }
       }
   
   }
   
   
   @Override
   public IBinder onBind(Intent arg0) {
       // TODO Auto-generated method stub
       return null;
   }
   public void calculateDistance (float[] acceleration, float deltaTime) {
       float[] distance = new float[acceleration.length];
   
       for (int i = 0; i < acceleration.length; i++) {
           speed[i] = acceleration[i] * deltaTime;
           distance[i] = speed[i] * deltaTime + acceleration[i] * deltaTime * deltaTime / 2;
       }
       distanceX = distance[0];
       distanceY = distance[1];
       distanceZ = distance[2];
   }
   

   }

EDIT:

public static void PlotNewPosition(Double newLatitude, Double newLongitude, Float currentDistance, 
        Float currentAcceleration, Float currentSpeed, Float currentDirection, String dataType) {

    LatLng newPosition = new LatLng(newLongitude,newLatitude);

    if(dataType == "Sensor"){
        tvAcceleration.setText("Speed: " + currentSpeed + " Acceleration: " + currentAcceleration + " Distance: " + currentDistance +" Direction: " + currentDirection + " \n"); 
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("Position")
        .snippet("Sensor Position")
        .icon(BitmapDescriptorFactory
                .fromResource(R.drawable.line)));
    }else if(dataType == "GPSSensor"){
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("PositionCollaborated")
        .snippet("GPS Position"));
    }
    else{
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("Position")
        .snippet("New Position")
        .icon(BitmapDescriptorFactory
                .fromResource(R.drawable.linered)));
    }
    map.moveCamera(CameraUpdateFactory.newLatLngZoom(newPosition, 18));
}

Solution 4

As per our discussion, since your acceleration is continuously changing, the equations of motion that you have applied shall not give you an accurate answer.

You may have to keep updating your position and velocities as and when you get a new reading for acceleration.

Since this would be highly inefficient, my suggestion would be to call the update function every few seconds and use the average value of acceleration during that period to get the new velocity and position.

View more solutions

Comments

[Dawood Awan]

The Application is to use Sensors not Location Service.. A sensor Fusion Application :-(

[Dawood Awan]

Good Point. Looks like I will have to filter it at this point. an if condition maybe.

[Dawood Awan]

I have added some Sensor Filters at the Android End. Will post the answer when I find it. I did take your point in mind about the Initial Velocity

[Dawood Awan]

Great Explaination. Do you suggest I do these calculations on the Smartphone itself? And just to be clear dt = t2 - t1, where t1 is initial time and t2 = time of reading?

[Spektre]

yes and dt is time between 2 iterations = actual time - last actual time. does not matter if its measured before or after iteration (no branches present but i preffer to measure it in the beginning of iteration)

[Dawood Awan]

Thanks for the help. Just want to ask that why do I have to add the previous velocity to the new velocity? vx+=ax*dt;

[Spektre]

because you are measuring the acceleration only ... so the speed is integrated (via that addition) ... it is just rectangle rule fore integral computation

[Dawood Awan]

@AlexWien I tried to do it on the Smartphone, but as the SensorListener Thread runs on the main UI thread, the UI Hangs up. I am new to android, and the Questions I read on Stackoverflow suggest that I can't rune the Sensor Listener on a seperate Thread, Do you know any way to this position calculation on the Smartphone in a seperate Thread?

[Spektre]

If you can measure time at least in 1ms resolution then you do not need threads at all and can put all together in one loop. These computation does not require constant dt. Also some well placed Sleep + locks to shared variables can unfreeze a lot (sometimes the shedulers gone crazy when accessing visual stuff in thread in the wrong time even on windows and multicore CPU). On singlecore device you cannot assume 100% asynchro run of separate threads. Also sometimes the cause of freeze is OS api locks/waitforobject especially with combination with kernel mode code (then use own locks instead).

[AlexWien]

First read my answer, then you probaly can skip your project. But to answer: Even if the Sensor Listenrer runs on the main thread, simply store the sensor datat in an (circular) array. If your calculation is so expensive (which I doubt) then create a separate thread which does the calculation on that array; Or look at demo code. For iphone there are some demps, so I expect that for android there shouzld be demo code for simple caluclations on sensor data.

[Spektre]

yep you are right ... gravity should be taken out before integration without gyros it can be done but its horrible filtering and really slow

[Dawood Awan]

Interesting. Thanks for the Help

[Bresiu]

Hi, I'm currently working on similar app. Can You share whole code of Your application?

[Pandiri Deepak]

Unable to get same result. Can you add location listener code also?

[230490]

Im trying to do the samething on an embedded system which has 9 axis accelerometer. In this, i do get reading for accelerometer, magnetomer and gyroscope for all 3 axis. Im trying to reuse your calculation or algorithm. However im not understanding difference between sensor.TYPE_ACCELEROMETER and TYPE_LINEAR_ACCELERATION. What;s the difference between them? how do i really get linear_acceleration from a 3 axis accelerometer

[Dawood Awan]

@230490 the LINEAR_ACCELERATION is a calculated value, it's the same as TYPE_ACCELEROMETER minus the gravity. Reference: stackoverflow.com/a/27461970/1910735 also I think it is explained on this link (developer.android.com/reference/android/hardware/…)