Мой алгоритм для расчета положения смартфона - GPS и датчиков

Я разрабатываю приложение для Android для расчета позиции на основе данных датчиков

Акселерометр → Рассчитать линейное ускорение
Магнитометр + Акселерометр → Направление движения

Исходное положение будет взято из GPS (широта + долгота).

Теперь, основываясь на показаниях датчиков, мне нужно вычислить новое положение смартфона:

Мой алгоритм следующий - (Но не вычисляет Точную позицию): Пожалуйста, помогите мне улучшить его.

Примечание: Мой алгоритм Код находится в С# (я отправляю данные датчиков на сервер - где данные хранятся в базе данных. Я вычисляю позицию на сервере)

Все объекты 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 Итак, что-то не так в вычислении скорости, но я не знаю, что

ОТВЕТ:

Я нашел решение для вычисления позиции на основе датчика: я опубликовал ответ ниже.

Если вам нужна помощь, оставьте комментарий

Это результаты по сравнению с GPS ( Примечание: 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;

датчик может вращаться, поэтому направление должно быть применено:

// 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 или очень близко к нему, то устройство, вероятно, стоит на месте (если это не летающий аппарат). Ориентация/направление могут быть скорректированы с учетом фактического направления силы тяжести.

Ответ 2

Датчики ускорения и гироскопы не подходят для расчета положения.
Через несколько секунд ошибки становятся невероятно высокими. (Я почти не помню, что двойная интеграция является проблемой).
Посмотрите на это Google Talk Talk о слиянии датчиков, он очень подробно объясняет, почему это невозможно.

Ответ 3

После решения позиции, которую я вычислил с помощью датчиков, я хотел бы опубликовать свой код здесь, если кому-то понадобится в будущем:

Примечание. Это было проверено только на телефоне Samsung Galaxy S2, и только когда человек шел по телефону, он не тестировался при движении в автомобиле или на велосипеде

Это результат, который я получил при сравнении по сравнению с GPS, (Red Line GPS, Blue - позиция, рассчитанная с помощью датчика)

Код не очень эффективен, но я надеюсь, что мой общий доступ к этому коду поможет кому-то и укажет их в правильном направлении.

У меня было два отдельных класса:

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 Сервис реализует 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];
}

}

EDIT:

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));
}

Ответ 4

В соответствии с нашим обсуждением, поскольку ваше ускорение постоянно меняется, принятые вами уравнения движения не дают точного ответа.

Возможно, вам придется постоянно обновлять свои позиции и скорости, когда и когда вы получите новое чтение для ускорения.

Поскольку это было бы очень неэффективно, мое предложение состояло бы в том, чтобы вызвать функцию обновления каждые несколько секунд и использовать среднее значение ускорения в течение этого периода, чтобы получить новую скорость и положение.

Ответ 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

Если конечная скорость становится скоростью текущего местоположения, так что previouslocation = currentlocation. Это означало бы, что ваша окончательная цена может не снизиться. Но опять же, здесь есть несколько предположений.

Ответ 6

Похоже, вы делаете это тяжело для себя. Вы можете просто использовать Google Location Location API и легко получить доступ к местоположению, направлению, скорости и т.д. Точно.

Я бы изучил использование этого вместо того, чтобы делать на нем рабочую серверную часть.