c# – 我计算智能手机位置的算法 – GPS和传感器

我正在开发一个Android应用程序来根据传感器的数据计算位置

>加速度计 – >计算线性加速度
>磁力计加速度计 – >运动方向

初始位置将取自GPS(纬度经度).

现在根据传感器的读数,我需要计算智能手机的新位置：

我的算法遵循 – (但不计算准确位置)：请帮我改进它.

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

所有DateTime对象都是使用TimeStamps计算的 – 从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所以计算速度有问题,但我不知道是什么

回答：

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

如果您需要任何帮助,请发表评论

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

解决方法:

正如你们有些人提到的那样,方程式错误,但这只是错误的一部分.

> Newton – D’Alembert物理学的非相对论速度决定了这一点：

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

> gx,gy,gz是全球重力矢量(地球上~~ 9.81 m / s ^ 2)
>在代码中,我的全局Y轴指向,因此gy = -9.81,其余为0.0

>衡量时间至关重要

必须尽可能经常检查加速度计(第二次是很长时间).我建议不要使用大于10毫秒的计时器周期来保持准确性,也应该不时地用GPS值覆盖计算的位置.指南针方向可以较少检查,但适当的过滤
>指南针一直不正确

应针对某些峰值过滤指南针值.有时它会读取坏的值,也可以通过电磁污染或金属环境来消除.在这种情况下,可以在移动期间通过GPS检查方向,并且可以进行一些校正.例如,每分钟擦拭GPS并将GPS方向与指南针进行比较,如果它常常是某个角度,则将其添加或减去它.
>为什么在服务器上进行简单的计算？

讨厌在线浪费流量.是的你可以在服务器上记录数据(但我认为设备上的文件会更好),但为什么要通过互联网连接限制位置功能？更不用说延误……

[编辑1]附加说明

编辑了上面的代码.方向必须尽可能精确,以尽量减少累积误差.

陀螺仪会比罗盘更好(甚至更好地使用它们).应该过滤加速度.一些低通滤波应该没问题.重力去除后,我会将ax,ay,az限制为可用值并丢弃太小的值.如果接近低速也完全停止(如果它不是火车或真空运动).这应该降低漂移但增加其他错误,因此必须在它们之间找到妥协.

即时添加校准.当过滤后的加速度= 9.81或非常接近它时,设备可能静止不动(除非是飞行器).可以通过实际重力方向来校正方向/方向.

标签：android-sensors,android,algorithm,gps,c-2
来源： https://codeday.me/bug/20190915/1805833.html