我的算法來計算智能手機的位置 - 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和傳感器

回答

相關問題