[java] Les données GPS filtrées par Kalman fluctuent encore beaucoup


0 Answers

Essayez ce simple changement:

float speed = location.getSpeed() x 4;
Question

Il a tout le monde!

J'écris une application Android qui utilise les appareils GPS pour calculer la vitesse de conduite d'un véhicule. Ceci est censé être précis à environ 1-2 km / h, et je le fais en regardant la distance entre deux emplacements GPS et en le divisant par le temps que ces emplacements sont séparés, assez simple, puis le faire pour le trois dernières coordonnées enregistrées et le soir.

Je reçois les données GPS dans un service d'arrière-plan, qui a un gestionnaire à son propre looper, donc quand je reçois un nouvel emplacement de LocationListener, j'appelle la méthode Kalmans update () et appelle le predict () dans un gestionnaire à intervalles réguliers en appelant sendEmptyDelayedMessage à lui-même après le predict ()

J'ai lu des données GPS lisses et j'ai essayé d'implémenter le filtre dans le github fourni par villoren dans une réponse à ce sujet, qui a aussi donné des résultats fluctuants. J'ai ensuite adapté le code de démo de ce tutoriel http://www.codeproject.com/Articles/326657/KalmanDemo , avec lequel je travaille actuellement. J'ai fait tous les calculs à la main pour mieux comprendre le filtre, et je ne suis pas sûr d'avoir complètement compris son code source, mais c'est ce sur quoi je travaille en ce moment:

La partie où j'ai commenté

/*// 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;
*/

est où je n'étais pas d'accord avec les maths du code fourni, mais donné (il déclare) il a mis en application des filtres de Kalman dans des systèmes de guidage de fusée je suis enclin à croire que ses maths ont raison;)

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

}

J'utilise deux filtres 1D, l'un pour la latitude et l'autre pour la longitude, puis construit un nouvel objet d'emplacement après chaque appel de prédiction.

Mon initialisation est qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy / 10, pd = gpsAccuracy / 10, ix = position initiale.

J'utilise les valeurs après le tutoriel dont j'ai obtenu le code, c'est ce qu'il a recommandé dans les commentaires.

En utilisant ceci, j'obtiens des vitesses qui sont a) fluctuant beaucoup, et b) des vitesses qui sont ÉTEINTES, j'obtiens des vitesses de 50 - quelques centaines de km / h en marchant, et puis aussi occasionnellement 5-7, qui est plus précis, mais j'ai besoin des vitesses pour être cohérent et au moins dans une gamme raisonnable.




Related