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.
