卡尔曼滤波器-实现，参数和调整

10

首先，这是我第一次尝试制作Kalman滤波器。

我之前曾在StackOverflow上发布了以下问题，即从速度值中滤除噪声和变化，该问题描述了本文的背景。这是我尝试过滤的典型值示例。他们不必一定要减少这种情况。但是变化率通常是这样的

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233.76
60 --- 232.97
66 --- 233.31
72 --- 233.99

我已经按照以下教程实现了我的Kalman过滤器：Kalman Filter for Dummies。

我的实现看起来像这样（伪代码）。

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

我开始时先估计为0。这似乎很好。但是我注意到的是，每次运行此更新时，kalmanGain都会减少，这意味着我相信我的新值越少，运行过滤器的时间就越长（？）。我不要

我从仅使用移动平均值（简单和指数加权）转向使用此方法。现在，我什至无法获得那么好的结果。

我的问题是这是否是正确的实现方式，并且根据我发布的样本值，我先前的误差方差和标准偏差是否看起来不错？我的参数实际上是随机选择的，以查看是否可以获得一些良好的结果。我尝试了几种不同的范围，但效果不佳。如果您有任何建议可以改变，我将不胜感激。很抱歉，如果缺少一些明显的东西。也是第一次在这里发布。

filters algorithms kalman-filters

— 奥莱
source

5

当您的输入信号包含一些线性动力系统状态的噪声观测值时，卡尔曼滤波器很有用。给定一系列对系统状态的观察，卡尔曼滤波器的目的是递归地提供对基础系统状态的越来越好的估计。为了成功应用它，您需要为要评估其状态的系统动态模型。如Wikipedia上的详细描述，该模型描述了状态基础系统在给定其先前状态，系统的任何输入以及称为过程噪声的高斯分布随机成分的情况下，如何在一个时间步长内变化。

话虽如此，目前尚不清楚您是否有这样的基础模型。链接的帖子表明您正在使用传感器的速度值。这些可以建模为对系统状态的直接观察（状态是其速度），或对系统状态的间接观察（例如，状态是其位置）。但是，为了使用Kalman框架，您需要选择一个模型，以期随着时间的流逝该状态将如何演变。使用此附加信息来生成最佳估计。卡尔曼滤波器不是一个魔术“黑匣子”，它只会“清理”应用于它的信号。

话虽如此，您所提到的现象在实践中确实会发生，在该现象中，卡尔曼滤波器对自己的输出将越来越有信心，直到输入观测值逐渐被忽略。这可以通过手动增加过程噪声协方差矩阵中的值来缓解。然后，定性地，系统状态转换的模型包含较大的随机成分，因此，在给定当前状态的情况下，估计器准确预测下一个状态的能力将降低。这将减少其对系统状态当前估计的依赖，并增加对后续观察的依赖，从而防止“忽略输入”效应。

— 杰森·R
source

+1：尤其是最后一段。可以将KF设计中的噪声协方差视为旋转的“旋钮”。

— Peter K.

4

如果我对它的理解正确，则说明您有东西在移动，可以观察到速度，而这个速度很吵。通过测量，您可以观察到2种变化。\

噪音引起的变化
变化，因为对象确实在改变速度（例如转弯）

卡尔曼增益变为零的原因是，您隐含地假设对象的速度是恒定的，而您要做的只是估算此真实速度。

“ 嘿，我有一个物体以恒定的速度运动，我想估计这个恒定的速度 ”

$x_k$ $k$ $y_k$

x_{k} = x_{k - 1}

$x_k = x_{k-1}$

y_{k} = x_{k} + q_{k}

$y_k = x_k + q_k$

但是您的对象并没有那样移动。速度在变化，您不知道它将如何以及何时发生变化。

您必须说的是：

“ 嘿，我有一个物体在以一定速度运动，但是我不确定它是如何改变其速度的 ”

您可以通过多种方法执行此操作：最简单的方法是增加状态的不确定性。

x_{k} = x_{k - 1} + \underset{you add uncertainty}{\underset{⏟}{v_{k - 1}}}

$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$

y_{k} = x_{k} + q_{k}

$y_k = x_k + q_k$

q_{k}

$q_k$

v_{k}

$v_k$

您的卡尔曼滤波器方程将如下所示：

{\hat{y}}_{k | k - 1} = {\hat{x}}_{k | k - 1}

$\hat y_{k|k-1} = \hat x_{k|k-1}$

K_{k} = \frac{P_{k | k - 1}}{P_{k | k - 1} + Q^{o}}

$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$

{\hat{x}}_{k | k} = {\hat{x}}_{k | k - 1} + K_{k} (y_{k} - {\hat{y}}_{k | k - 1})

$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$

P_{k | k} = P_{k | k - 1} - K_{k} P_{k | k - 1}

$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$

P_{k + 1 | k} = P_{k | k} + Q^{s}

$P_{k+1|k} =P_{k|k} + Q^s$

0.05 $Q^o$ $Q^s$

在您的代码中，稍作修改将是：

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance $Q^s$

该stateVariance值可以是您想要的任何值。这取决于您对速度实际变化的信心。如果您认为速度将保持恒定，请将其设置为较小的值。

这样，您的卡尔曼增益将不会为零。

— ssk08
source

3

您需要一个动态系统才能使用卡尔曼滤波器。

我会建议

y = \sum_{i = 0}^{n} a_{i} x^{i}

$y = \sum\limits_{i =0}^n a_i\, x^i$

a [k + 1] = a [k] + w

$a[k+1] = a[k] + w$

c o v (w) = Q

$cov(w) = Q$

z = \sum_{i = 0}^{n} a_{i} x^{i} = y

$z = \sum\limits_{i =0}^n a_i\, x^i = y$

$x$ $a$

— iao
source

1

我认为您可以使用经典控制理论中的一些思想，例如PID控制器。

您的信号Y可以是控制器u（t）的设定值。加工厂只有1，并且y（t）将被过滤输出。您要做的就是设置参数（调整）P，I和D，以获得所需的内容。

输出y（t）将尝试“跟随”输入u（t），但参数控制此跟踪的方式。

差分增益D将使您的响应对快速误差变化敏感。就您而言，我认为D应该很小。如果u（t）突然改变，您不希望y（t）改变。

积分增益“ I”将使您的响应对累积误差敏感。您应该在那里放高价值。如果u（t）更改级别并将其保持在该水平，则错误将累积，然后您希望y（t）进行相同的操作。

P增益可以微调。无论如何，请尝试使用这些参数，看看会得到什么。

虽然有复杂的调优方法，但是我认为您不需要它。

祝好运。

— 丹尼尔·R·皮帕
source

实际上，有更好的方法。看到这篇文章。

— Daniel R. Pipa

看到这篇文章

— Daniel R. Pipa 2012年

1

这是卡尔曼过滤器的简单干净的实现，遵循Wikipedia页面中的标记。https://github.com/zziz/kalman-filter

— fvzaur
source