Мой алгоритм расчета положения смартфона - GPS и датчики
Я разрабатываю приложение для Android для расчета положения на основе данных датчика
Акселерометр -> Рассчитать линейное ускорение
Магнитометр + Акселерометр -> Направление движения
Начальная позиция будет взята из 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 красный)
6 ответов
Как некоторые из вас упоминали, вы ошиблись в уравнениях, но это только часть ошибки.
Физика Ньютона-Д'Ламбера для нерелятивистских скоростей диктует это:
// 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 о слиянии сенсоров, он очень подробно объясняет, почему это невозможно.
После определения позиции, которую я рассчитал с помощью датчиков, я хотел бы опубликовать свой код здесь на случай, если кому-то понадобится в будущем:
Примечание: это было проверено только на телефоне Samsung 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 расширяет Сервис реализует 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));
}
Согласно нашему обсуждению, поскольку ваше ускорение постоянно меняется, применяемые вами уравнения движения не дают вам точного ответа.
Возможно, вам придется постоянно обновлять вашу позицию и скорости, как и когда вы получаете новое показание для ускорения.
Поскольку это было бы крайне неэффективно, я бы предложил вызывать функцию обновления каждые несколько секунд и использовать среднее значение ускорения в течение этого периода, чтобы получить новую скорость и положение.
Я не совсем уверен, но мое лучшее предположение было бы вокруг этой части:
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
Если конечная скорость становится скоростью текущего местоположения, то есть предыдущее местоположение = текущее местоположение. Это будет означать, что ваша конечная скорость может не снизиться. Но опять же, здесь довольно много предположений.
Похоже, вы делаете это тяжело для себя. Вы должны иметь возможность просто использовать API определения местоположения службы Google Play и легко получать точный доступ к местоположению, направлению, скорости и т. Д.
Я хотел бы использовать это вместо того, чтобы работать на стороне сервера для этого.