java - Los datos de GPS filtrados por Kalman siguen fluctuando mucho

android kalman-filter

Question

¡Todos!

Estoy escribiendo una aplicación para Android que usa los dispositivos GPS para calcular la velocidad de conducción de un vehículo. Se supone que esto es preciso a aproximadamente 1-2 km / h, y lo estoy haciendo al observar la distancia entre dos ubicaciones de GPS y dividirla para cuando estas ubicaciones están separadas, bastante sencillo, y luego hacer esto para el las últimas tres coordenadas registradas y al final.

Obtengo los datos del GPS en un servicio en segundo plano, que tiene un manejador en su propio looper, así que cada vez que obtengo una nueva ubicación del LocationListener, llamo al método Kalmans update () y llamo a prediction () en un manejador en intervalos regulares llamando a sendEmptyDelayedMessage a sí mismo después de la predicción ()

He leído datos Smooth GPS y también he intentado implementar el filtro en el github proporcionado por villoren en una respuesta a ese tema, que también arrojó resultados fluctuantes. A continuación, he adaptado el código de demostración de este tutorial http://www.codeproject.com/Articles/326657/KalmanDemo , con el que estoy trabajando en este momento. Hice todas las matemáticas a mano para comprender mejor el filtro, y no estoy seguro si entendí su código fuente proporcionado por completo, pero esto es con lo que estoy trabajando ahora mismo:

La parte donde comenté

/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/

es donde no estaba de acuerdo con las matemáticas del código proporcionado, pero dado (afirma) que ha implementado filtros de Kalman en sistemas de guía de cohetes, me inclino a creer que sus cálculos son correctos;)

public class KalmanFilter {
/*
X = State
F = rolls X forward, typically be some time delta.
U = adds in values per unit time dt.
P = Covariance – how each thing varies compared to each other.
Y = Residual (delta of measured and last state).
M = Measurement
S = Residual of covariance.
R = Minimal innovative covariance, keeps filter from locking in to a solution.
K = Kalman gain
Q = minimal update covariance of P, keeps P from getting too small.
H = Rolls actual to predicted.
I = identity matrix.
*/
//State X[0] =position, X[1] = velocity.
private double m_x0, m_x1;
//P = a 2x2 matrix, uncertainty
private double m_p0, m_p1,m_p2, m_p3;
//Q = minimal covariance (2x2).
private double m_q0, m_q1, m_q2, m_q3;
//R = single value.
private double m_r;
//H = [1, 0], we measure only position so there is no update of state.
private final double m_h1 = 1, m_h2 = 0;
//F = 2x2 matrix: [1, dt], [0, 1].
public void update(double m, double dt){
// Predict to now, then update.
// Predict:
// X = F*X + H*U
// P = F*X*F^T + Q.
// Update:
// Y = M – H*X Called the innovation = measurement – state transformed by H.
// S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R
// K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.
// X = X + K*Y Update with gain the new measurement
// P = (I – K * H) * P Update covariance to this time.
// X = F*X + H*U
double oldX = m_x0;
m_x0 = m_x0 + (dt * m_x1);
// P = F*X*F^T + Q
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
m_p1 = m_p1 + dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;
// Y = M – H*X
//To get the change in velocity, we pretend to be measuring velocity as well and
//use H as [1,1]
double y0 = m - m_x0;
double y1 = ((m - oldX) / dt) - m_x1;
// S = H*P*H^T + R
//because H is [1,0], s is only a single value
double s = m_p0 + m_r;
/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/
// K = P * H^T *S^-1
double k0 = m_p0 / s;
double k1 = m_p2 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k0;
m_x1 += y1 * k1;
// P = (I – K * H) * P
m_p0 = m_p0 - k0* m_p0;
m_p1 = m_p1 - k0* m_p1;
m_p2 = m_p2 - k1* m_p2;
m_p3 = m_p3 - k1* m_p3;
}
public void predict(double dt){
//X = F * X + H * U Rolls state (X) forward to new time.
m_x0 = m_x0 + (dt * m_x1);
//P = F * P * F^T + Q Rolls the uncertainty forward in time.
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
/* m_p1 = m_p1+ dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;*/
}
/// <summary>
/// Reset the filter.
/// </summary>
/// <param name="qx">Measurement to position state minimal variance.</param>
/// <param name="qv">Measurement to velocity state minimal variance.</param>
/// <param name="r">Measurement covariance (sets minimal gain).</param>
/// <param name="pd">Initial variance.</param>
/// <param name="ix">Initial position.</param>
/**
*
* @param qx Measurement to position state minimal variance = accuracy of gps
* @param qv Measurement to velocity state minimal variance = accuracy of gps
* @param r Masurement covariance (sets minimal gain) = 0.accuracy
* @param pd Initial variance = accuracy of gps data 0.accuracy
* @param ix Initial position = position
*/
public void reset(double qx, double qv, double r, double pd, double ix){
m_q0 = qx; m_q1 = qv;
m_r = r;
m_p0 = m_p3 = pd;
m_p1 = m_p2 = 0;
m_x0 = ix;
m_x1 = 0;
}
public double getPosition(){
return m_x0;
}
public double getSpeed(){
return m_x1;
}
}

Estoy usando dos filtros 1D, uno para latitud y uno para longitud y luego construyo un nuevo objeto de ubicación después de cada llamada de predicción.

Mi inicialización es qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy / 10, pd = gpsAccuracy / 10, ix = initial position.

Utilizo los valores después del tutorial del que obtuve el código, esto es lo que recomendó en los comentarios.

Al usar esto, obtengo velocidades que a) fluctúan mucho, yb) velocidades que están MUY FUERA, obtengo velocidades de 50 - algunos cientos de km / h mientras camino, y luego también el ocasional 5-7, que es más preciso, pero necesito que las velocidades sean consistentes y al menos en un rango razonable.

Answer 1

La ubicación del GPS, entregada por el receptor GPS ya está fuertemente filtrada por Kalman. Si las ubicaciones todavía están saltando, no puedes resolver eso con un filtro de Kalman. La causa es que moverse con una velocidad baja no da una posición estable y velocidad (y dirección). Simplemente elimine toda la ubicación a menos de 10 km / hy no habrá necesidad de ningún tipo de filtrado.

Answer 2

Prueba este simple cambio:

float speed = location.getSpeed() x 4;

Answer 3

Veo algunos problemas:

• Su update() contiene predicción y actualización, pero también tiene una predict() , por lo que estaría integrando la velocidad si realmente llama a predict() (no incluyó el bucle externo).
• Existe cierta confusión en cuanto a si su medida es posición o posición y velocidad. Puedes ver comentarios que dicen H=[1,0] y H=[1,1] (por lo que probablemente significaron H=[1,0;0,1] ) Dado que la matemática de la matriz está escrita a mano, la suposición sobre la medición individual se hornea en todos los pasos de la matriz, pero el código también intenta "medir" la velocidad también.
• Para un KF que estima la velocidad desde posiciones, no desea inyectar una velocidad sintética como esa (como una diferencia de primer orden). Deje que el resultado ocurra naturalmente desde el KF. Para H=[1,0] , puede ver cómo K=PH''/S debe tener 2 filas, y ambas se aplican a y0 . Eso actualizará tanto x0 como x1 .

Realmente no verifiqué la matriz matemática más que para ver qué habían hecho con H Realmente debería desarrollar este tipo de algoritmo con una buena biblioteca de matriz (por ejemplo, numpy, para Python o Eigen para C ++). Eso le ahorrará muchos cambios de código cuando realice cambios triviales (por ejemplo, si desea experimentar con un filtro 2D) y evitará simples errores de matriz matemática que lo volverán loco. Si tiene que optimizar para operaciones de matriz totalmente escritas a mano, hágalo durar, de modo que pueda comparar sus resultados y verificar su codificación manual.

Y, por último, las otras publicaciones son completamente correctas sobre su aplicación específica: el GPS ya está filtrando los datos, y una de las salidas es la velocidad.