我计算智能手机位置的算法 - 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;

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

回答：

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

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

这是与 GPS 相比的结果（注意： GPS 为红色）

Answer 1

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

1. 牛顿 - 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;
   

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 ms 的计时器周期来保持准确性，有时您应该使用 GPS 值覆盖计算出的位置。指南针方向可以不经常检查，但需要适当的过滤

4. 指南针始终不正确

   罗盘值应针对某些峰值进行过滤。有时它会读取错误的值，也可能会因电磁污染或金属环境而关闭。在这种情况下，可以通过 GPS 在移动过程中检查方向并进行一些修正。例如，每分钟检查一次 GPS 并将 GPS 方向与指南针进行比较，如果它一直在某个角度，则添加或减去它。

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

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

[编辑 1] 附加说明

稍微修改了上面的代码。方向必须尽可能精确，以尽量减少累积误差。

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

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

Answer 2

加速度传感器和陀螺仪不适用于位置计算。
几秒钟后，错误变得难以置信的高。 （我几乎不记得双重积分是问题所在）。
看看这个关于传感器融合的Google tech talk video， 他非常详细地解释了为什么这是不可能的。

Answer 3

在解决了我使用传感器计算的位置后，我想在这里发布我的代码，以防将来有人需要：

注意：此功能仅在三星 Galaxy S2 手机上进行过检测，仅在人走路时进行，在汽车或自行车上移动时未进行过检测

这是我与 GPS 比较时得到的结果，（红线 GPS，蓝色是通过传感器计算的位置）

代码效率不是很高，但我希望我分享这段代码能帮助某人并为他们指明正确的方向。

我有两个单独的班级：

1. 计算位置

2. CustomSensorService

   公共类CalculatePosition {

           static Double earthRadius = 6378D;
           static Double oldLatitude,oldLongitude;
           static Boolean IsFirst = true;
   
           static Double sensorLatitude, sensorLongitude;
   
           static Date CollaborationWithGPSTime;
           public static float[] results;
   
   
   
           public static void calculateNewPosition(Context applicationContext,
                   Float currentAcceleration, Float currentSpeed,
                   Float currentDistanceTravelled, Float currentDirection, Float TotalDistance) {
   
   
               results = new float[3];
               if(IsFirst){
                   CollaborationWithGPSTime = new Date();
                   Toast.makeText(applicationContext, "First", Toast.LENGTH_LONG).show();
                   oldLatitude = CustomLocationListener.mLatitude;
                   oldLongitude = CustomLocationListener.mLongitude;
                   sensorLatitude = oldLatitude;
                   sensorLongitude = oldLongitude;
                   LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor",0.0F,TotalDistance);
                   IsFirst  = false;
                   return;
               } 
   
               Date CurrentDateTime = new Date();
   
               if(CurrentDateTime.getTime() - CollaborationWithGPSTime.getTime() > 900000){
                   //This IF Statement is to Collaborate with GPS position --> For accuracy --> 900,000 == 15 minutes
                   oldLatitude = CustomLocationListener.mLatitude;
                   oldLongitude = CustomLocationListener.mLongitude;
                   LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor", 0.0F, 0.0F);
                   return;
               }
   
               //Convert Variables to Radian for the Formula
               oldLatitude = Math.PI * oldLatitude / 180;
               oldLongitude = Math.PI * oldLongitude / 180;
               currentDirection = (float) (Math.PI * currentDirection / 180.0);
   
               //Formulae to Calculate the NewLAtitude and NewLongtiude
               Double newLatitude = Math.asin(Math.sin(oldLatitude) * Math.cos(currentDistanceTravelled / earthRadius) + 
                       Math.cos(oldLatitude) * Math.sin(currentDistanceTravelled / earthRadius) * Math.cos(currentDirection));
               Double newLongitude = oldLongitude + Math.atan2(Math.sin(currentDirection) * Math.sin(currentDistanceTravelled / earthRadius)
                       * Math.cos(oldLatitude), Math.cos(currentDistanceTravelled / earthRadius)
                       - Math.sin(oldLatitude) * Math.sin(newLatitude));
   
               //Convert Back from radians
               newLatitude = 180 * newLatitude / Math.PI;
               newLongitude = 180 * newLongitude / Math.PI;
               currentDirection = (float) (180 * currentDirection / Math.PI);
   
               //Update old Latitude and Longitude
               oldLatitude = newLatitude;
               oldLongitude = newLongitude;
   
               sensorLatitude = oldLatitude;
               sensorLongitude = oldLongitude;
   
               IsFirst = false;
               //Plot Position on Map
               LivePositionActivity.PlotNewPosition(newLongitude,newLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "Sensor", results[0],TotalDistance);
   
   
   
   
   
       }
   }
   

   公共类 CustomSensorService 扩展服务实现 SensorEventListener{

   static SensorManager sensorManager;
   static Sensor mAccelerometer;
   private Sensor mMagnetometer;
   private Sensor mLinearAccelertion;
   
   static Context mContext;
   
   private static float[] AccelerometerValue;
   private static float[] MagnetometerValue;
   
   public static  Float currentAcceleration = 0.0F;
   public static  Float  currentDirection = 0.0F;
   public static Float CurrentSpeed = 0.0F;
   public static Float CurrentDistanceTravelled = 0.0F;
   /*---------------------------------------------*/
   float[] prevValues,speed;
   float[] currentValues;
   float prevTime, currentTime, changeTime,distanceY,distanceX,distanceZ;
   float[] currentVelocity;
   public static CalculatePosition CalcPosition;
   /*-----FILTER VARIABLES-------------------------*-/
    * 
    * 
    */
   
   public static Float prevAcceleration = 0.0F;
   public static Float prevSpeed = 0.0F;
   public static Float prevDistance = 0.0F;
   
   public static Float totalDistance;
   
   TextView tv;
   Boolean First,FirstSensor = true;
   
   @Override
   public void onCreate(){
   
       super.onCreate();
       mContext = getApplicationContext();
       CalcPosition =  new CalculatePosition();
       First = FirstSensor = true;
       currentValues = new float[3];
       prevValues = new float[3];
       currentVelocity = new float[3];
       speed = new float[3];
       totalDistance = 0.0F;
       Toast.makeText(getApplicationContext(),"Service Created",Toast.LENGTH_SHORT).show();
   
       sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
   
       mAccelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
       mMagnetometer = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
       //mGyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
       mLinearAccelertion = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
   
       sensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_NORMAL);
       sensorManager.registerListener(this, mMagnetometer, SensorManager.SENSOR_DELAY_NORMAL);
       //sensorManager.registerListener(this, mGyro, SensorManager.SENSOR_DELAY_NORMAL);
       sensorManager.registerListener(this, mLinearAccelertion, SensorManager.SENSOR_DELAY_NORMAL);
   
   }
   
   @Override
   public void onDestroy(){
       Toast.makeText(this, "Service Destroyed", Toast.LENGTH_SHORT).show();
       sensorManager.unregisterListener(this);
       //sensorManager = null;
       super.onDestroy();
   }
   @Override
   public void onAccuracyChanged(Sensor sensor, int accuracy) {
       // TODO Auto-generated method stub
   
   }
   
   @Override
   public void onSensorChanged(SensorEvent event) {
   
       float[] values = event.values;
       Sensor mSensor = event.sensor;
   
       if(mSensor.getType() == Sensor.TYPE_ACCELEROMETER){
           AccelerometerValue = values;
       }
   
       if(mSensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION){           
           if(First){
               prevValues = values;
               prevTime = event.timestamp / 1000000000;
               First = false;
               currentVelocity[0] = currentVelocity[1] = currentVelocity[2] = 0;
               distanceX = distanceY= distanceZ = 0;
           }
           else{
               currentTime = event.timestamp / 1000000000.0f;
   
               changeTime = currentTime - prevTime;
   
               prevTime = currentTime;
   
   
   
               calculateDistance(event.values, changeTime);
   
               currentAcceleration =  (float) Math.sqrt(event.values[0] * event.values[0] + event.values[1] * event.values[1] + event.values[2] * event.values[2]);
   
               CurrentSpeed = (float) Math.sqrt(speed[0] * speed[0] + speed[1] * speed[1] + speed[2] * speed[2]);
               CurrentDistanceTravelled = (float) Math.sqrt(distanceX *  distanceX + distanceY * distanceY +  distanceZ * distanceZ);
               CurrentDistanceTravelled = CurrentDistanceTravelled / 1000;
   
               if(FirstSensor){
                   prevAcceleration = currentAcceleration;
                   prevDistance = CurrentDistanceTravelled;
                   prevSpeed = CurrentSpeed;
                   FirstSensor = false;
               }
               prevValues = values;
   
           }
       }
   
       if(mSensor.getType() == Sensor.TYPE_MAGNETIC_FIELD){
           MagnetometerValue = values;
       }
   
       if(currentAcceleration != prevAcceleration || CurrentSpeed != prevSpeed || prevDistance != CurrentDistanceTravelled){
   
           if(!FirstSensor)
               totalDistance = totalDistance + CurrentDistanceTravelled * 1000;
           if (AccelerometerValue != null && MagnetometerValue != null && currentAcceleration != null) {
               //Direction
               float RT[] = new float[9];
               float I[] = new float[9];
               boolean success = SensorManager.getRotationMatrix(RT, I, AccelerometerValue,
                       MagnetometerValue);
               if (success) {
                   float orientation[] = new float[3];
                   SensorManager.getOrientation(RT, orientation);
                   float azimut = (float) Math.round(Math.toDegrees(orientation[0]));
                   currentDirection =(azimut+ 360) % 360;
                   if( CurrentSpeed > 0.2){
                       CalculatePosition.calculateNewPosition(getApplicationContext(),currentAcceleration,CurrentSpeed,CurrentDistanceTravelled,currentDirection,totalDistance);
                   }
               }
               prevAcceleration = currentAcceleration;
               prevSpeed = CurrentSpeed;
               prevDistance = CurrentDistanceTravelled;
           }
       }
   
   }
   
   
   @Override
   public IBinder onBind(Intent arg0) {
       // TODO Auto-generated method stub
       return null;
   }
   public void calculateDistance (float[] acceleration, float deltaTime) {
       float[] distance = new float[acceleration.length];
   
       for (int i = 0; i < acceleration.length; i++) {
           speed[i] = acceleration[i] * deltaTime;
           distance[i] = speed[i] * deltaTime + acceleration[i] * deltaTime * deltaTime / 2;
       }
       distanceX = distance[0];
       distanceY = distance[1];
       distanceZ = distance[2];
   }
   

   }

编辑：

public static void PlotNewPosition(Double newLatitude, Double newLongitude, Float currentDistance, 
        Float currentAcceleration, Float currentSpeed, Float currentDirection, String dataType) {

    LatLng newPosition = new LatLng(newLongitude,newLatitude);

    if(dataType == "Sensor"){
        tvAcceleration.setText("Speed: " + currentSpeed + " Acceleration: " + currentAcceleration + " Distance: " + currentDistance +" Direction: " + currentDirection + " \n"); 
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("Position")
        .snippet("Sensor Position")
        .icon(BitmapDescriptorFactory
                .fromResource(R.drawable.line)));
    }else if(dataType == "GPSSensor"){
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("PositionCollaborated")
        .snippet("GPS Position"));
    }
    else{
        map.addMarker(new MarkerOptions()
        .position(newPosition)
        .title("Position")
        .snippet("New Position")
        .icon(BitmapDescriptorFactory
                .fromResource(R.drawable.linered)));
    }
    map.moveCamera(CameraUpdateFactory.newLatLngZoom(newPosition, 18));
}

Answer 4

根据我们的讨论，由于你的加速度是不断变化的，你应用的运动方程不会给你一个准确的答案。

当您获得新的加速度读数时，您可能需要不断更新您的位置和速度。

由于这将非常低效，我的建议是每隔几秒调用一次更新函数，并使用该期间的加速度平均值来获取新的速度和位置。

Answer 5

我不太确定，但我最好的猜测应该是这部分：

Double initialVelocity = prevLocation.Speed;
var t = secondsTravelling.TotalSeconds;
var finalvelocity = initialVelocity + Double.Parse(currentAcceleration) * t;

如果让我们在 prevLocation 说速度是：27.326... 并且 t==0 和 currentAcceleration ==0（正如你所说的你是空闲的）最终速度将下降到

var finalvelocity = 27.326 + 0*0;
var finalvelocity == 27.326

如果finalvelocity变成currentlocation的速度，那么previouslocation = currentlocation。这意味着您的最终速度可能不会下降。但话又说回来，这里有很多假设。

Answer 6

看来你让自己很难受。您应该可以简单地使用Google Play Service Location API 并轻松准确地访问位置、方向、速度等。

我会考虑使用它而不是为它做服务器端的工作。