Mi algoritmo para calcular la posición del teléfono inteligente – GPS y sensores

Estoy desarrollando una aplicación de Android para calcular la posición en base a los datos del sensor

  1. Acelerómetro -> Calcular la aceleración lineal

  2. Magnetómetro + Acelerómetro -> Dirección de movimiento

La posición inicial se tomará del GPS (Latitud + Longitud).

Ahora, según las lecturas del sensor, necesito calcular la nueva posición del teléfono inteligente:

Mi algoritmo sigue: (pero no está calculando la posición exacta): Por favor, ayúdame a mejorarlo.

Nota: el código de mi algoritmo está en C # (estoy enviando datos del sensor al servidor, donde los datos se almacenan en la base de datos. Estoy calculando la posición en el servidor)

Todos los objetos de DateTime se han calculado utilizando TimeStamps – Desde 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; 

Este es el resultado que obtengo -> El teléfono no se movía. Como se puede ver, la velocidad es 27.3263111114502 Por lo que hay un error en el cálculo de la velocidad, pero no sé qué

introduzca la descripción de la imagen aquí

RESPONDER:

Encontré una solución para calcular la posición basada en el Sensor: He publicado una Respuesta a continuación.

Si necesitas ayuda, por favor deja un comentario.

esto es Los resultados en comparación con GPS ( Nota: GPS está en rojo)

introduzca la descripción de la imagen aquí

Como algunos de ustedes mencionaron, las ecuaciones son incorrectas, pero eso es solo una parte del error.

  1. La física de Newton – D’Lambert para velocidades no relativistas dicta esto:

     // 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. el sensor puede girar por lo que debe aplicarse la dirección:

     // 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 es el vector de gravedad global ( ~9.81 m/s^2 en la Tierra)
    • en el código, mi eje Y global apunta hacia arriba, por lo que gy=-9.81 y el rest son 0.0
  3. medir los tiempos son críticos

    El acelerómetro se debe revisar con la mayor frecuencia posible (el segundo es un tiempo muy largo). Recomiendo no usar un período de temporizador superior a 10 ms para preservar la precisión también el tiempo que debe anular la posición calculada con el valor de GPS. La dirección de la brújula se puede verificar con menos frecuencia pero con una filtración adecuada

  4. la brújula no es correcta todo el tiempo

    Los valores de la brújula deben filtrarse para algunos valores máximos. A veces, lee valores incorrectos y también puede desactivarse por contaminación electromagnética o por medio de metales. En ese caso, la dirección se puede verificar por GPS durante el movimiento y se pueden hacer algunas correcciones. Por ejemplo, gire el GPS cada minuto y compare la dirección del GPS con la brújula y, si está constantemente en algún ángulo, añádalo o reste.

  5. ¿Por qué hacer cálculos simples en el servidor?

    Odiar el desperdicio de tráfico en línea. Sí, puede registrar datos en el servidor (pero aún creo que el archivo en el dispositivo será mejor), pero ¿por qué ver la funcionalidad de la posición límite mediante la conexión a Internet? Sin mencionar los retrasos ...

[Editar 1] notas adicionales

Editado el código anterior un poco. La orientación debe ser tan precisa como sea posible para minimizar los errores acumulativos.

Gyros sería mejor que la brújula (o incluso mejor usarlos a ambos). La aceleración debe ser filtrada. Algunos filtros de paso bajo deberían estar bien. Después de la extracción por gravedad, limitaría ax, ay, az a valores utilizables y descartaría valores demasiado pequeños. Si está cerca de una velocidad baja, también haga una parada completa (si no es un tren o un movimiento en el vacío). Eso debería reducir la deriva pero boost otros errores, por lo que se debe encontrar un compromiso entre ellos.

Añadir calibración sobre la marcha. Cuando la acceleration = 9.81 filtrada acceleration = 9.81 o muy cerca de él, entonces el dispositivo probablemente está parado (a menos que sea una máquina voladora). La orientación / dirección se puede corregir por la dirección de gravedad real.

Los sensores de aceleración y los giroscopios no son adecuados para el cálculo de la posición.
Después de unos segundos los errores se vuelven increíblemente altos. (Apenas recuerdo que la doble integración sea el problema).
Mire este video de Google tech talk acerca de la fusión de sensores, explica en detalle por qué esto no es posible.

Después de resolver la posición que calculé utilizando Sensors, me gustaría publicar mi código aquí en caso de que alguien lo necesite en el futuro:

Nota: Esto solo se verificó en el teléfono Samsung Galaxy S2 y solo cuando la persona caminaba con el teléfono, no se ha probado cuando se mueve en automóvil o en bicicleta

Este es el resultado que obtuve al compararlo con el GPS, (Red Line GPS, Blue es la Posición calculada con el Sensor)

Este es el resultado que obtuve al compararlo con el GPS, (Red Line GPS, Blue es la Posición calculada con el Sensor)

El código no es muy eficiente, pero espero que compartir este código ayude a alguien y lo señale en la dirección correcta.

Tuve dos clases separadas:

  1. Calcular posición
  2. CustomSensorService

    clase pública 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); } } 

    La clase pública CustomSensorService extiende los implementos de servicio 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]; } 

    }

EDITAR:

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

De acuerdo con nuestra discusión, dado que su aceleración cambia continuamente, las ecuaciones de movimiento que ha aplicado no le darán una respuesta precisa.

Es posible que tenga que seguir actualizando su posición y velocidades a medida que obtenga una nueva lectura de aceleración.

Como esto sería altamente ineficiente, mi sugerencia sería llamar a la función de actualización cada pocos segundos y usar el valor promedio de aceleración durante ese período para obtener la nueva velocidad y posición.

No estoy muy seguro, pero mi mejor conjetura sería alrededor de esta parte:

 Double initialVelocity = prevLocation.Speed; var t = secondsTravelling.TotalSeconds; var finalvelocity = initialVelocity + Double.Parse(currentAcceleration) * t; 

si digamos en la ubicación previa, la velocidad era: 27.326 … y t == 0 y currentAcceleration == 0 (como dijiste que estabas inactivo) la finalvelocity se reduciría a

 var finalvelocity = 27.326 + 0*0; var finalvelocity == 27.326 

Si el destino final se convierte en la velocidad de la ubicación actual, de modo que previouslocation = currentlocation. Esto significaría que su finalvelocity podría no disminuir. Pero, de nuevo, hay bastantes suposiciones aquí.

Parece que te lo estás poniendo difícil. Debería poder utilizar simplemente la API de ubicación del servicio Google Play y acceder fácilmente a la ubicación, dirección, velocidad, etc. con precisión.

Me gustaría usar eso en lugar de hacer el trabajo del servidor.