Usar el acelerómetro en caso de pérdida de señal GPS

[ManuLG]

Actualmente cuando detecta que el GPS le envía varias veces la misma coordenada ignora el GPS y usa solamente el acelerómetro, pero los resultado obtenidos no son buenos:

[0skiC]

Hay que verificar que los ejes del acelerómetro estén correctos. La forma de la gráfica en el momento en el que perdemos la señal GPS es correcta (el acelerómetro "pinta" las rectas y las curvas), pero claramente el eje en el que lo "pinta" no es el correcto. Los siguientes picos bruscos del acelerómetro se deben a la reconexión del GPS.

[ManuLG]

Solución parcial del problema

Tras una sesión de pruebas con @0skiC esta tarde hemos conseguido solucionar parcialmente el problema, estos son los resultados obtenidos al representar la trazada SÓLO con el acelerómetro (representamos los valores del vector de estados antes de la etapa de correción de Kalman):

• Detalle:

Color rojo: Zona de pérdida de GPS + recuperación Color verde: Zona de recuperación

La solución, parcial hasta el momento, consiste en modificar el eje del acelerómetro para que coincida con el del GPS:

    // Para que en caso de que no haya movimiento la orientación siga siendo la misma
    if (!((flat_ant == flat) && (flon_ant == flon))) {
      orientacion = gps.course_to(flat_ant, flon_ant, flat, flon);
    }

    // Correción de ejes
    z[2] *= cos(grado_to_radian * (orientacion - 90.0 - orientacion_acelerometro(x[2], x[3]))); // ax
    z[3] *= sin(grado_to_radian * (orientacion - 90.0 - orientacion_acelerometro(x[2], x[3]))); // ay

    // Siendo "orientacion" la última orientación valida proporcionada por el GPS

[ManuLG]

Salida de Kalman de la solución parcial

• Detalles:

Color rojo: Pérdida de GPS

[ManuLG]

Nuevo planteamiento, sabiendo que el ángulo de la orientación del vehículo coincide con el ángulo entre el eje Norte y eje X del acelerómetro, el planteamiento es el siguiente:

Código:

    acelx = (float)myIMU.accelCount[0] * myIMU.aRes * g_to_ms2;
    acely = (float)myIMU.accelCount[1] * myIMU.aRes * g_to_ms2;

    z[2] = acelx * cos(orientacion) + acely * cos(90 + orientacion);
    z[3] = acelx * sin(orientacion) + acely * sin(90 + orientacion);

Conclusión: Descomponer tanto la componente x como la y en ambos ejes del GPS (el Norte y el Este)

[ManuLG]

Corrección sobre la idea anterior

Basándome en este artículo, la conversión es un poco distinta a como la había pensado:

    acelx = (float)myIMU.accelCount[0] * myIMU.aRes * gravedad; // - accelBias[0]; Aceleración X
    acely = (float)myIMU.accelCount[1] * myIMU.aRes * gravedad; // - accelBias[1]; Aceleración Y

    // Comprobamos que nos hemos movido del origen
    if (dist_actual != 0) {
      z[0] = distancia * sin(grado_to_radian * angulo); // Para invertir la imagen
      z[1] = distancia * cos(grado_to_radian * angulo);
      z[2] = acelx * cos((90 - orientacion) * grado_to_radian) + acely * sin((90 - orientacion) * grado_to_radian);
      z[3] = acely * cos((90 - orientacion) * grado_to_radian) - acelx * sin((90 - orientacion) * grado_to_radian);
      z[4] = velocidad * sin(grado_to_radian * orientacion);
      z[5] = velocidad * cos(grado_to_radian * orientacion);
    } else {
      z[2] = 0.0;
      z[3] = 0.0;
      z[4] = 0.0;
      z[5] = 0.0;
    }

[ManuLG]

Solución final

A mayores de lo dicho en el comentario anterior hace falta actualizar la orientación, eso lo hacemos mediante este código:

float orientacion_acelerometro(float lat, float lon, float lat_ant, float lon_ant) {
  return 90.0 * grado_to_radian - (2 * M_PI + atan2(lon - lon_ant, lat - lat_ant));
}

Resultado