智能手机GPS定位算法研究

Posted

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了智能手机GPS定位算法研究相关的知识,希望对你有一定的参考价值。

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

  1. 加速度计 - >计算线性加速度
  2. 磁力计+加速度计 - >运动方向

初始位置将取自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是红色的)

答案

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

  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;
  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是全球引力矢量(地球上的~9.81 m/s^2) 在代码我的全球Y轴指向所以gy=-9.81和其余是0.0
  3. 衡量时间至关重要 必须尽可能经常检查加速度计(第二次是很长时间)。我建议不要使用大于10毫秒的计时器周期来保持准确性,也应该不时地用GPS值覆盖计算的位置。指南针方向可以较少检查,但适当的过滤
  4. 指南针一直不正确 应针对某些峰值过滤指南针值。有时它会读取坏的值,也可以通过电磁污染或金属环境来消除。在这种情况下,可以在移动期间通过GPS检查方向,并且可以进行一些校正。例如,每分钟擦拭GPS并将GPS方向与指南针进行比较,如果它常常是某个角度,则将其添加或减去它。
  5. 为什么在服务器上做简单的计算? 讨厌在线浪费流量。是的你可以在服务器上记录数据(但我认为设备上的文件会更好),但为什么要通过互联网连接限制位置功能?更不用说延误......

[编辑1]附加说明

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

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

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

另一答案

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

另一答案

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

注意:这只在三星Galaxy S2手机上进行过检查,只有当人在手机上行走时,才会在车内或自行车上进行测试

这是我与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 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定位算法研究的主要内容,如果未能解决你的问题,请参考以下文章

    Android如何开启GPS定位服务?

    如何将手机GPS定位的数据通过手机上网功能传送给远处的电脑(电脑连接互联网),使得电脑对手机进行准确定位?

    为啥不同的手机定位有误差。

    关于手机定位轨迹的算法逻辑

    GPS定位基本原理浅析

    共享单车的定位系统是位于单车的哪里