Question

大家好！

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

我在后台服务中获取GPS数据，该服务有自己的looper处理程序，因此每当我从LocationListener获取新位置时，我调用Kalmans update（）方法并调用预测（）通过在predict（）

之后调用sendEmptyDelayedMessage来定期处理一个处理程序

我已阅读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 =初始位置。

我在获得代码的教程之后使用这些值，这是他在评论中推荐的内容。

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

Answer 1

尝试这个简单的改变：

float speed = location.getSpeed() x 4;

Answer 2

我看到一些问题：

您的update()包含预测和更新，但您还有一个predict()，因此如果您实际调用predict()，则会对速度进行双重积分（你没有包括外环）。
关于您的测量是位置还是位置和速度，存在一些混淆。您可以看到声明H=[1,0]和H=[1,1]的评论（它们可能意味着H=[1,0;0,1]）因为矩阵数学是手写的，所以关于单次测量的假设被烘焙到所有矩阵中步骤，但代码仍然试图＆＃34;衡量＆＃34;速度也是如此。
对于从位置估算速度的KF，您不希望像这样注入合成速度（作为一阶差分）。让结果自然地从KF发生。对于H=[1,0]，您可以看到K=PH'/S应该有2行，并且都适用于y0。这将更新x0和x1。

我没有真正检查矩阵数学，除了看看他们用H做了什么。你应该用一个漂亮的矩阵库来开发这种算法（比如numpy，Python，或Eigen for C ++）。当您进行微小的更改时（例如，如果您想要试验2D过滤器）并避免简单的矩阵数学错误会让您发疯，这将为您节省大量代码更改。如果你必须优化到完全手写的矩阵运算，那就去做吧，这样你就可以比较结果并验证你的手工编码。

最后，其他帖子对您的具体应用完全正确：GPS已经过滤了数据，其中一个输出是速度。

Answer 3

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

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

3 个答案: