Question

我正在使用以下平滑功能来平滑GPS的速度读数。

void smoothing_init()
{
    k = 0;
    kalman_init(0.0625, 32, 1.3833094, 0);
}

void kalman_init(double _q, double _r, double _p, double intial_value)
{
    q = _q;
    r = _r;
    p = _p;
    x = intial_value;
}


double smoothing_add_sample(double measurement)
{
    p = p + q;
    k = p / (p + r);
    x = x + k * (measurement - x);
    p = (1 - k) * p;
    return x;
}

然而，有时这会给我平滑值700（正常范围是0-150）然后下降。我想当我用0初始化例程但立即接收高于0的读数（例如40,50）时会发生这种情况。

如何调整这些功能以自然地防止此类峰值，但仍能够平滑数据。

Answer 1

卡尔曼滤波器是一种估计器，可以最小化线性系统的系统状态（p）的方差（代码中的x）。方差p可以解释为x值的置信度度量。

对于每个时间步k，过滤器执行两个步骤：

预测步骤将估计值提前一步传播（在您的情况下根据x[k+1] = x[k] + w[k]，其中w[k]是零均值高斯噪声，方差为q）。这意味着状态方差增加了q。
过滤步骤包含测量信息（根据measurement[k] = x[k] + v[k]，其中v[k]是零均值高斯噪声，方差为r）。

对于经典估计问题p初始化的值非常大（对初始值x几乎没有信心）。随着时间的推移，p会降低到q附近的某个值。请注意p独立于measurement，因此仅取决于q,r和p[0]。

所以对于调整：

使用较大的p进行初始化，即p/r>>0; x的初始化值并不重要。
商r/q调整平滑的强度（r/q=0，然后根本没有平滑）。
要选择r,q的幅度，您可以假设高斯噪声：那么您的概率值为[x-2*sqrt(p), x+2*sqrt(p)]的置信区间的概率为95％。

修复卡尔曼平滑功能

1 个答案: