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

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

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

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

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

现在基于传感器的读数,我需要计算智能手机的新位置:

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

注意: 我的algorithm代码是C#(我正在发送传感器数据到服务器 – 数据存储在数据库中,我正在计算服务器上的位置)

所有date时间对象已经使用时间戳计算 – 从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 所以计算速度有点问题,但我不知道是什么

在这里输入图像说明

回答:

我find了一个解决scheme来计算基于传感器的位置:我已经发布了下面的答案。

如果您需要任何帮助,请留下评论

这是比较GPS的结果( 注意: GPS是红色的)

在这里输入图像说明

Solutions Collecting From Web of "我的algorithm来计算智能手机的位置 – GPS和传感器"

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

  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是全球重力vector(地球上~9.81 m/s^2
    • 在代码中,我的全局Y轴指向所以gy=-9.81 ,其余的为0.0
  3. 衡量时机至关重要

    加速度计必须尽可能经常检查(其次是很长的时间)。 我build议不要使用大于10毫秒的定时器周期来保持精度,同时也应该使用GPS值覆盖计算的位置。 指南针的方向可以检查较less,但适当的过滤

  4. 指南针是不正确的

    应该过滤一些峰值的指南针值。 有时读取不好的值,也可能被电磁污染或金属环境污染。 在这种情况下,可以在移动过程中通过GPS检查方向,并且可以进行一些校正。 例如,每分钟切一个GPS,并将GPS方向与指南针进行比较,如果某个angular度不变,则将其添加或减去。

  5. 为什么简单的计算在服务器上?

    讨厌在线浪费stream量。 是的,你可以在服务器上logging数据(但仍然认为设备上的文件会更好),但为什么通过互联网连接限制位置function? 更不用说拖延了…

[编辑1]额外的笔记

稍微编辑一下代码。 方向必须尽可能精确,以尽量减less累积误差。

陀螺仪会比指南针更好(或者更好地使用它们)。 应该加速过滤。 一些低通滤波应该是可以的。 重力去除后,我会限制ax,ay,az到可用值,并丢掉太小的值。 如果靠近低速也会停下来(如果不是真空中的火车或运动)。 这应该降低漂移,但增加其他的错误,所以他们之间必须妥协。

即时添加校准。 当过滤acceleration = 9.81或非常接近它,那么设备可能静止不动(除非它是一个飞行器)。 方向/方向可以通过实际重力方向进行校正。

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

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

注意:只有在三星Galaxy S2手机上才能查看,只有当用户使用手机行走时,在车内或自行车中才能testing

这是我比较GPS时得到的结果(红线GPS,蓝色是用传感器计算的位置)

这是我比较GPS时得到的结果(红线GPS,蓝色是用传感器计算的位置)

代码效率不高,但是我希望我的代码能够帮助别人,让他们指向正确的方向。

我有两个单独的课程:

  1. CalculatePosition
  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扩展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)); } 

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

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

由于这样效率很低,所以我build议每隔几秒调用一次更新函数,并使用这段时间内加速度的平均值来得到新的速度和位置。

我不太清楚,但我最好的猜测是围绕这个部分:

 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成为当前位置的速度,那么previouslocation = currentlocation。 这意味着你的最终速度可能不会下降。 但是,再一次,这里有相当多的假设。

好像你正在让自己很难。 您应该能够简单地使用Google Play服务位置API,并且准确地轻松访问位置,方向,速度等。

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