java - 经过卡尔曼滤波的 GPS 数据仍然波动很大

Question

大家好!

我正在编写一个 Android 应用程序，它使用设备 GPS 来计算车辆的行驶速度。这应该精确到大约 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 = initial position.

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

使用它，我得到的速度是 a) 波动很大，和 b) 速度很慢，我得到的速度是 50 - 走路时几百公里/小时，然后偶尔是 5-7，哪个更准确，但我需要速度保持一致，至少在一个合理的范围内。

Answer 1

我看到了一些问题:

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

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

最后，关于您的具体应用，其他帖子完全正确:GPS 已经在过滤数据，输出之一是速度。