title: My Algorithm to Calculate Position of Smartphone - GPS and Sensors
body: <p>I am developing an android application to calculate position based on Sensor's Data</p>
  
  <ol>
  <li><p>Accelerometer --> Calculate Linear Acceleration</p></li>
  <li><p>Magnetometer + Accelerometer --> Direction of movement</p></li>
  </ol>
  
  <p>The initial position will be taken from GPS (Latitude + Longitude).</p>
  
  <p>Now based on Sensor's Readings i need to calculate the new position of the Smartphone:</p>
  
  <p>My Algorithm is following - (But is not calculating Accurate Position): Please help me improve it.</p>
  
  <p><strong>Note:</strong> <strong>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)</strong></p>
  
  <p><strong>All DateTime Objects have been calculated using TimeStamps - From 01-01-1970</strong></p>
  
  <pre><code>    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 --&gt; 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;
  </code></pre>
  
  <p>This is the Result I get --> Phone was not moving. As you can see <em>speed is 27.3263111114502</em> <strong>So there is something wrong in calculating Speed but I don't know what</strong></p>
  
  <p><img src="https://i.stack.imgur.com/oi3NR.png" alt="enter image description here"></p>
  
  <p><strong>ANSWER:</strong></p>
  
  <p>I found a solution to calculate position based on Sensor: I have posted an Answer below.</p>
  
  <p>If you need any help, please leave a comment</p>
  
  <p>this is The results compared to GPS (<strong>Note:</strong> GPS is in Red)</p>
  
  <p><img src="https://i.stack.imgur.com/3QjcP.png" alt="enter image description here"></p>