1

我正在使用以下平滑功能来平滑 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）时会发生这种情况。

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

4

1 回答 1

2

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

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

  1. 预测步骤将估计值提前一步传播（在您的情况下，根据方差为 的零均值高斯噪声x[k+1] = x[k] + w[k]在哪里）。这意味着状态方差增加了.w[k]qq
  2. 滤波步骤结合了测量信息（根据方差为 的零均值高斯噪声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，您可以假设高斯噪声： 那么您有 95% 的概率真实值位于 的置信区间中[x-2*sqrt(p), x+2*sqrt(p)]。
于 2012-12-12T18:03:50.753 回答