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

我开发一个Android应用程序来计算基于传感器的数据我的算法来计算智能手机的位置 - GPS和传感器

加速度位置 - >计算直线加速
磁力仪+加速度计 - - >运动方向

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

enter image description here

答：

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

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

这是比GPS的结果（注： GPS是红色）

enter image description here

来源

2013-11-01 Dawood Awan

你确定运动和加速度的方向的行进的过程中不会改变？您应用的公式假定在行驶过程中加速度保持不变。 –

加速度变化。 –

在^ 2（从A点到B点）时，如果s = ut +（1/2），则只能在A和B之间的整个时间内加速度保持恒定为'a'时才能应用。 –

正如你们中的一些人所说的，你得到的方程是错误的，但这只是错误的一部分。

牛顿 - D'兰伯特物理非相对论速度决定的：

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

图10是全局重力矢量（~9.81 m/s^2地球上）
在代码我的全球Y轴指向上，因此gy=-9.81，其余为0.0

测量定时是临界

加速度必须是尽可能经常检查（第二个是很长时间）。我建议不要使用大于10毫秒的定时器周期来保持精度，同时您应该用GPS值覆盖计算的位置。罗盘方向可以不经常但适当的过滤
指南针是不正确的所有的时间

罗盘值应该被过滤一些峰值进行检查。有时它读取不好的值，也可能被电磁污染或金属环境所污染。在这种情况下，可以在移动过程中通过GPS检查方向，并且可以进行一些校正。例如，每分钟扫描一次GPS，并将GPS方向与指南针进行比较，如果它始终是某个角度，则将其添加或减去。
为什么在服务器上进行简单的计算？

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

[编辑1]附加注释

编辑上方一点的代码。方向必须尽可能精确，以尽量减少累积误差。

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

实时添加校准。当过滤acceleration = 9.81或非常接近它，那么该设备可能会静止（除非它是一个飞行器）。方向/方向可以通过实际的重力方向进行校正。

来源

2013-11-04 09:32:10 Spektre

Great Explaination。您是否建议我在智能手机上进行这些计算？只是要清楚dt = t2 - t1，其中t1是初始时间，t2是读数的时间？ –

yes和dt是2次迭代之间的时间=实际时间 - 上次实际时间如果在迭代之前或之后测量，则无关紧要没有分支存在，但我主张在迭代开始时测量它） – Spektre

感谢帮帮我。只是想问，为什么我必须将上一个速度添加到新速度？ VX + =斧* dt的; –

-1

好像你正在使努力靠自己。您应该能够简单地使用Google Play Service Location API并且准确地访问位置，方向，速度等。

我会研究使用它，而不是做它的工作服务器端。

来源

2013-11-01 13:06:25 David

该应用程序是使用传感器不定位服务..一个传感器融合应用程序:-( –

我不太清楚，但我最好的猜测是解决此部分：

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

如果在prevLocation让说的速度为：27.326 ...而t == 0和currentAcceleration == 0（因为你说你是空闲）的finalvelocity会回落到

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

如果finalvelocity成为currentlocation的速度，使previouslocation = currentlocation。这意味着你的最终速度可能不会下降。但是再一次，这里有很多假设。

来源

2013-11-01 13:37:22 AssaultingCuccos

好点，看起来我必须在这一点过滤它，如果条件可能 –

@DawoodAwan你是否得到它的工作？ – AssaultingCuccos

我已经在Android End添加了一些传感器滤波器，当我找到它时会发布答案，我确实记住了初始速度 –

根据我们的讨论，由于您的加速度不断变化，因此您应用的运动方程式不会给出准确的答案。

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

由于这样做效率非常低，我建议每隔几秒调用一次更新函数，并使用该时段内加速度的平均值来获得新的速度和位置。

来源

2013-11-04 09:03:05

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

来源

2013-11-06 18:04:52 AlexWien

解决我使用传感器来计算，我想在这里发布我的代码的情况下，任何人的位置后，需要在今后的：

注：这是只检查了三星Galaxy S2手机，只有当人与走手机，它并没有被汽车或骑自行车

This is the result I got when compared when compared with GPS, (Red Line GPS, Blue is Position calculated with Sensor)

移动时，测试这是当与GPS相比时相比，我得到的结果，（红线GPS，蓝色是计算与森的位置）

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

我有两个单独的类：

CalculatePosition

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





    } 
}

公共类CustomSensorS ervice扩展服务实现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)); 
}

来源

2014-05-19 21:52:19

嗨，我目前正在研究类似的应用程序。你可以分享你的应用程序的整个代码？ – Bresiu

无法获得相同的结果。你还可以添加位置监听器代码吗？ –

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

回答

相关问题