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

//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

#1 楼

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

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

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

#2 楼

如果我对它的理解正确，则说明您有东西在移动，可以观察到速度，而这个速度很吵。通过测量，您可以观察到2种变化。
Kalman增益为零的原因是您隐式地假设物体的速度是恒定的，而您所需要做的就是估算这个真实速度。

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

您的模型是这样的，其中$ x_k $是时间$ k $和$ y_k $的速度是相应的度量。

$$ x_k = x_ {k-1} $$
$$ y_k = x_k + q_k $$

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

相反，您要说的是：

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

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

$$ x_ {k} = x_ {k-1} + \ underbrace {v_ {k-1}} _ {\ text {您添加不确定性}} $$
$$ y_k = x_k + q_k $$
其中$ q_k $和$ v_k $被假定为白噪声。

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

$$ \ hat y_ {k | k-1} = \ hat x_ {k | k-1} $$
$$ 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}）$$
$$ P_ {k | k} = P_ {k | k -1}-K_kP_ {k | k-1} $$
$$ P_ {k + 1 | k} = P_ {k | k} + Q ^ s $$

在您的案例的0.05值为观察噪声协方差$ Q ^ o $。要进行此更改，只需将$ Q ^ s $设置为将状态噪声covarinace（状态中的不可信度）设置为某个恒定值。

在您的代码中，需要做一些细微修改：

stateVariance = 0.5

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

通过在代码中不添加stateVariance或$ Q ^ s $，您可以假定它为零。

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

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

#3 楼

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

我建议

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

$$ a [k + 1] = a [k] + w $$
$$ cov（w）= Q $$
测量值：
$$ z = \ sum \ limits_ {i = 0} ^ n a_i \，x ^ i = y $$

因此，不要使用$ x $作为状态，而引入系数（$ a $）作为状态

#4 楼

我认为您可以使用经典控制理论中的一些想法，例如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增益可以进行微调。无论如何，请尝试使用参数并查看获得的结果。
虽然有复杂的调优方法，但我认为您不需要它。
祝您好运。

#5 楼

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