大家好！
我正在编写一款Android应用程序，利用设备的GPS来计算车辆行驶速度。这应该是精确到1-2公里/小时左右，我通过查看两个GPS位置之间的距离并将其除以这些位置分开的时间来实现这一点，非常简单，并对最后三个记录的坐标进行平衡。
我在后台服务中获取GPS数据，该服务具有自己的looper处理程序，因此每当我从LocationListener获取新位置时，我调用Kalmans update（）方法，并通过调用sendEmptyDelayedMessage将predict（）延迟到正常间隔中的handler自身中。
我已经阅读了Smooth GPS data，并尝试实现了villoren在回答中提供的github中的滤波器，但结果仍然波动不定。接着我使用了这个教程http://www.codeproject.com/Articles/326657/KalmanDemo中的演示代码进行了改编。我手工完成了所有数学运算，以更好地理解滤波器，但我不确定是否完全理解了他提供的源代码，但这就是我现在所使用的：
我注释掉的部分是：
/*// 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;
*/
我对提供的代码中的数学问题表示质疑，但考虑到（他声称）他已经在火箭制导系统中实现了卡尔曼滤波器，我倾向于相信他的数学是正确的 ;)
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;
}
}
我正在使用两个1D过滤器，一个用于纬度，一个用于经度，并在每次预测调用后构建一个新的位置对象。
我的初始化是 qx = gpsAccuracy，qv = gpsAccuracy，r = gpsAccuracy/10，pd = gpsAccuracy/10，ix = 初始位置。
我使用教程中提供的值，这是他在评论中推荐的。
使用这种方法，我得到了波动很大的速度，而且速度偏差很大，当我走路时，我得到的速度从50到几百公里/小时不等，偶尔还有5-7公里/小时，这更加准确，但我需要速度保持一致并且至少处于合理范围内。
m_x0和m_x1的数学公式中，x += Ky，其中在您的情况下，K应该是2行，而y是标量。您引用的代码使用了y1，这根本不应该存在。m_x1的更新应该是y0 * k1。 - Ben Jackson