Question

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

加速度计 - ＆gt;计算线性加速度
磁强计+加速度计 - ＆gt;行动方向

初始位置将取自GPS（纬度+经度）。

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

我的算法如下 - （但不算精确位置）：请帮助我改进它。

注意： 我的算法代码在C＃中（我将传感器数据发送到服务器 - 数据存储在数据库中。我正在计算服务器上的位置）

使用TimeStamps计算所有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;

这是我得到的结果 - ＆gt;电话没动。你可以看到速度是27.3263111114502 所以计算速度有问题，但我不知道是什么

enter image description here

解答：

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

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

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

enter image description here

Answer 1

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

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

传感器可以旋转，因此必须应用方向：

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

衡量时间至关重要

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

应针对某些峰值过滤指南针值。有时它会读取坏的值，也可以通过电磁污染或金属环境来消除。在这种情况下，可以在移动期间通过GPS检查方向，并且可以进行一些校正。例如，每分钟检查一次GPS，并将GPS方向与指南针进行比较，如果它经常偏离某个角度，则将其添加或减去它。
为什么在服务器上进行简单的计算???

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

[编辑1]附加说明

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

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

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

Answer 2

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

Answer 3

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

注意：这只在三星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); } }

公共类CustomSensorService extends Service实现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并轻松准确地访问位置，方向，速度等。

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

智能手机GPS定位算法研究

6 个答案: