10

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

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

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

初始位置将从 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 所以计算速度有问题，但我不知道是什么

在此处输入图像描述

回答：

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

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

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

在此处输入图像描述

4

6 回答 6

9

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

  1. 牛顿 - 非相对论速度的 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或非常靠近它时，设备可能会静止不动（除非它是飞行机器）。方向/方向可以通过实际重力方向进行校正。

于 2013-11-04T09:32:10.853 回答
5

加速度传感器和陀螺仪不适合位置计算。
几秒钟后，错误变得难以置信的高。（我几乎不记得双重积分是问题所在）。
看看这个关于传感器融合的谷歌技术谈话视频，他非常详细地解释了为什么这是不可能的。

于 2013-11-06T18:04:52.653 回答
3

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

注意：这仅在三星 Galaxy S2 手机上进行了检查，并且仅在人带着手机走路时进行了检查，在汽车或自行车上移动时未进行过测试

这是我与 GPS 比较时得到的结果，（红线 GPS，蓝色是使用传感器计算的位置）

这是我与 GPS 比较时得到的结果，（红线 GPS，蓝色是使用传感器计算的位置）

该代码效率不高，但我希望我分享此代码将帮助某人并为他们指明正确的方向。

我有两个单独的课程：

  1. 计算位置

  2. 自定义传感器服务

    公共类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 扩展服务实现 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-19T21:52:19.703 回答
1

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

当您获得新的加速度读数时，您可能必须不断更新您的位置和速度。

由于这将非常低效，我的建议是每隔几秒调用一次更新函数，并使用该期间的加速度平均值来获得新的速度和位置。

于 2013-11-04T09:03:05.900 回答
0

我不太确定，但我最好的猜测是围绕这部分：

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。这意味着您的最终速度可能不会下降。但是话又说回来，这里有很多假设。

于 2013-11-01T13:37:22.960 回答
-2

好像你在给自己做事。您应该能够简单地使用Google Play Service Location API并轻松准确地访问位置、方向、速度等。

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

于 2013-11-01T13:06:25.250 回答