我的算法来计算智能手机的位置 - GPS 和传感器

My Algorithm to Calculate Position of Smartphone - GPS and Sensors(我的算法来计算智能手机的位置 - GPS 和传感器)

问题描述

我正在开发一个基于传感器数据计算位置的安卓应用程序

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

1. 加速度计 --> 计算线性加速度

1. Accelerometer --> Calculate Linear Acceleration

磁力计 + 加速度计 --> 运动方向

Magnetometer + Accelerometer --> Direction of movement

初始位置将从 GPS(纬度 + 经度)获取.

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.

注意: 我的算法代码在 C# 中(我将传感器数据发送到服务器 - 数据存储在数据库中.我正在计算服务器上的位置)

所有 DateTime 对象均使用时间戳计算 - 从 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;

这是我得到的结果 --> 手机没有移动.如您所见速度是 27.3263111114502 所以计算速度有问题，但我不知道是什么

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

回答:

我找到了一个基于传感器计算位置的解决方案:我在下面发布了一个答案.

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

这是与 GPS 相比的结果(注意: GPS 为红色)

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

推荐答案

正如你们中的一些人提到的，你的方程式错误，但这只是错误的一部分.

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

1. 牛顿 - 非相对论速度的达朗贝尔物理学规定了这一点:

// 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;

// 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;