卡尔曼滤波的 GPS 数据仍然波动很大

Question

大家好！

我正在编写一个使用设备 GPS 计算车辆行驶速度的 Android 应用。这应该精确到大约 1-2 公里/小时，我通过查看两个 GPS 位置之间的距离并将其除以这些位置分开的时间来做到这一点，非常简单，然后为最后三个记录的坐标，然后结束。

我在后台服务中获取 GPS 数据，该服务有一个处理程序来处理它自己的循环器，所以每当我从 LocationListener 获取新位置时，我都会调用 Kalmans update() 方法并在处理程序中调用 predict()通过在 predict() 之后调用 sendEmptyDelayedMessage 来定期

我已阅读 Smooth GPS data 并且实际上还尝试在 github 中实现过滤器，该过滤器由 villoren 提供，以回答该主题，这也产生了波动的结果。 然后我改编了本教程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;
}

}

我正在使用两个一维过滤器，一个用于纬度，一个用于经度，然后在每次预测调用后从中构造一个新的位置对象。

我的初始化是 qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy/10 , pd = gpsAccuracy/10, ix = 初始位置。

我使用从教程中获取代码后的值，这是他在 cmets 中推荐的。

使用这个，我得到的速度是 a) 波动很大，b) 速度很差，我在步行时得到的速度从 50 到几百公里/小时，偶尔也会有 5-7 个，哪个更准确，但我需要速度保持一致并且至少在合理范围内。

Answer 1

我发现了一些问题：

• 您的update() 包含预测和 更新，但您也有一个predict()，因此如果您实际调用predict()（您没有包括外循环）。
• 对于您的测量是位置还是位置和速度存在一些混淆。您可以看到 cmets 声称 H=[1,0] 和 H=[1,1]（他们可能指的是 H=[1,0;0,1]） 由于矩阵数学是手写的，因此有关单一测量的假设被纳入所有矩阵步骤，但代码仍然也尝试“测量”速度。
• 对于从位置估计速度的 KF，您不希望像这样注入合成速度（作为一阶差分）。让这个结果从 KF 中自然发生。对于H=[1,0]，您可以看到K=PH'/S 应该有2 行，并且都适用于y0。这将同时更新 x0 和 x1。

除了看看他们对H 做了什么之外，我并没有真正检查矩阵数学。你真的应该用一个很好的矩阵库来开发这种算法（例如，numpy，用于 Python，或 Eigen 用于 C++）。当您进行微不足道的更改时（例如，如果您想尝试使用 2D 过滤器），这将为您节省大量代码更改，并避免让您发疯的简单矩阵数学错误。如果您必须针对完全手写的矩阵运算进行优化，请最后进行，以便比较结果并验证您的手写编码。

最后，关于您的特定应用，其他帖子完全正确：GPS 已经在过滤数据，其中一个输出是速度。

Answer 2

试试这个简单的改变：

float speed = location.getSpeed() x 4;

Answer 3

由 GPS 接收器提供的 GPS 位置已经经过严重的卡尔曼滤波。如果位置仍在跳跃，您无法使用卡尔曼滤波器很好地解决该问题。 原因是低速移动不能很好地提供稳定的位置和速度（和方向） 只需删除所有低于 10km/h 的位置，就无需再进行任何过滤。