智能手机GPS定位算法研究
Posted
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了智能手机GPS定位算法研究相关的知识,希望对你有一定的参考价值。
我正在开发一个android应用程序来根据传感器的数据计算位置
- 加速度计 - >计算线性加速度
- 磁力计+加速度计 - >运动方向
初始位置将取自GPS(纬度+经度)。
现在根据传感器的读数,我需要计算智能手机的新位置:
我的算法遵循 - (但不计算准确位置):请帮我改进它。
注意:我的算法代码在C#中(我将传感器数据发送到服务器 - 数据存储在数据库中。我正在计算服务器上的位置)
所有DateTime对象都是使用TimeStamps计算的 - 从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所以计算速度有问题,但我不知道是什么
回答:
我找到了一个基于Sensor计算位置的解决方案:我在下面发布了一个答案。
如果您需要任何帮助,请发表评论
这是与GPS相比的结果(注意:GPS是红色的)
正如你们有些人提到的那样,方程式错误,但这只是错误的一部分。
- 用于非相对论速度的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
或非常接近它时,设备可能静止不动(除非它是飞行机器)。可以通过实际重力方向来校正方向/方向。
加速度传感器和陀螺仪不适合位置计算。 几秒钟后,错误变得令人难以置信。 (我几乎不记得双重整合是问题)。 看看这个关于传感器融合的Google tech talk video,他非常详细地解释了为什么这是不可能的。
解决了我使用传感器计算的位置后,我想在此处发布我的代码,以防将来有人需要:
注意:这只在三星Galaxy S2手机上进行过检查,只有当人在手机上行走时,才会在车内或自行车上进行测试
这是我与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); mLine
以上是关于智能手机GPS定位算法研究的主要内容,如果未能解决你的问题,请参考以下文章