Kalman-filtered GPS data is still fluctuating a lot

Question

He everyone!

I'm writing an Android app that uses the devices GPS to calculate a vehicles driving speed. This is supposed to be accurate to about 1-2 km/h, and I'm doing it by looking at the distance between two GPS locations and dividing it by the time these locations are apart, pretty straightforward, and then doing this for the last three recorded coordinates and evening it out.

I get the GPS data in a background service, that has a handler to it's own looper, so whenever I get a new location from the LocationListener, I call the Kalmans update() method and call the predict() in a handler in regular intervals by calling sendEmptyDelayedMessage to itself after the predict()

I have read Smooth GPS data and have actually also tried implementing the filter in the github that was provided by villoren in an answer to that topic, which also yielded fluctuating results. I have then adapted the demo code from this tutorial http://www.codeproject.com/Articles/326657/KalmanDemo, which I am working with right now. I did all the maths by hand to better understand the filter, and I'm not sure if I understood his provided source code completely, but this is what I am working with right now:

The part where I commented out

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

is where I didn't agree with the maths of the code provided, but given (he states) he has implemented Kalman filters in rocket guidance systems I am inclined to believe his maths is right ;)

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

}

I'm using two 1D Filters, one for latitude and one for longitude and then construct a new location object out of that after each predict call.

My initialization is qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy/10 , pd = gpsAccuracy/10, ix = initial position.

I use the values after the tutorial I got the code from, this is what he recommended in the comments.

Using this, I get speeds that are a) fluctuating a lot, and b) speeds that are WAY off, I get speeds from 50 - a few hundred km/h while walking, and then also the occasional 5-7, which is more accurate, but I need the speeds to be consistent and at least in a sensible range.

Answer 1

I see a few issues:

• Your update() contains predict and update, but you also have a predict(), so you would be double-integrating velocity if you actually call predict() (you didn't include the outer loop).
• There's some confusion as to whether your measurement is position or position and velocity. You can see comments claiming H=[1,0] and H=[1,1] (by which they probably meant H=[1,0;0,1]) Since the matrix math is hand-written, the assumption about the single measurement is baked into all of the matrix steps, but the code still tries to "measure" velocity as well.
• For a KF which estimates velocity from positions, you do not want to inject synthetic velocity like that (as a first-order difference). Let that result occur naturally from the KF. For H=[1,0], you can see how K=PH'/S should have 2 rows, and both apply to y0. That will update both x0 and x1.

I didn't really check the matrix math other than to see what they had done with H. You should really develop this kind of algorithm with a nice matrix library (e.g. numpy, for Python, or Eigen for C++). That will save you a lot of code changes when you make trivial changes (e.g. if you want to experiment with a 2D filter) and avoid simple matrix math errors that will drive you mad. If you have to optimize to fully hand-written matrix operations, do it last so you can compare your results and verify your hand coding.

And finally, the other posts are entirely correct about your specific application: The GPS is already filtering the data, and one of the outputs is velocity.

Answer 2

Try this simple change:

float speed = location.getSpeed() x 4;

Answer 3

GPS location, delivered by the GPS receiver are already heavily Kalman filtered. If the locations are still jumping you cannot well solve that with a Kalman filter. The cause is that moving with a low speed does not well give stable positions and speed (and direction) Just remove all location under 10km/h and there will be no further need to any filtering.