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

Question

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

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

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

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

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

我的算法如下 - (但不是计算准确位置):请帮助我改进它。

注意: 我的算法代码是用 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;

这是我得到的结果 --> 手机没有动。如您所见，speed 是 27.3263111114502 所以在计算 Speed 时有问题，但我不知道是什么

回答:

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

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

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

Answer 1

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

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;
   

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 或非常接近它时，设备可能静止不动(除非它是飞行器)。可以通过实际重力方向修正方位/方向。