小编典典

My Algorithm to Calculate Position of Smartphone - GPS and Sensors

c#

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

enter image description here

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)

enter image description here


阅读 184

收藏
2020-05-19

共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+=axdt; // update speed via integration of acceleration
    vy+=ay
    dt;
    vz+=azdt;
    x+=vx
    dt; // update position via integration of velocity
    y+=vydt;
    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+=aydt;
    vz+=az
    dt;
    x+=vxdt; // 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
    • 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

  1. 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.

  1. 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.

2020-05-19