智能手机GPS定位算法研究

Question

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

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

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

初始位置将取自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 所以计算速度有问题,但我不知道是什么

回答:

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

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

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

Answer 1

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

1. Newton - D'Lambert物理学的非相对论速度决定了这一点:

   // 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. 传感器可以旋转,因此必须应用方向:

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

3. 衡量时间至关重要

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

4. 指南针一直不正确

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

5. 为什么在服务器上做简单的计算？

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

[编辑1]附加说明

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

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

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

Answer 2

加速度传感器和陀螺仪不适合位置计算.
几秒钟后,错误变得令人难以置信.(我几乎不记得双重整合是问题).
看看这个关于传感器融合的Google技术谈话视频,他非常详细地解释了为什么这是不可能的.