卡尔曼滤波器的实用方法及其实现方法

引用地址：http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
另外一篇可以参考：详解卡尔曼滤波原理【https://blog.csdn.net/u010720661/article/details/63253509】

长期以来，我一直对kalman文件管理器及其工作方式感到困惑，我还在我的Balancing机器人（网址：http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/）上使用了kalman过滤器，但我从未解释过它是如何实现的。实际上，我从来没有花时间坐在笔和纸上坐下来自己尝试做数学，所以我实际上不知道它是如何实现的。
事实证明这是一件好事，因为我实际上在原始代码中发现了一个错误，但稍后我会再讲到。

我实际上是在2011年12月在高中时就将卡尔曼滤波器作为我的主要作业写的。但是，我仅使用卡尔曼滤波器来计算由已知的Gaussian white noise调制的直流信号的真实电压。我的作业可以在以下zip文件中找到：http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip。它是丹麦语，但您可以正确地使用Google翻译来翻译其中的一些内容。如果您对作业有任何具体问题，请在下面的评论中提问。

好的，但是回到主题。令我难过的是，我从未花时间坐下来对基于加速度计和陀螺仪的卡尔曼滤波器进行数学运算。这并不像我预期的那么难，但是我必须承认，我仍然没有研究背后的更深层的理论，即它为何真正起作用。但是对于我和大多数人来说，与实施更深层的理论以及方程为何起作用相比，我对实施过滤器更感兴趣。

在开始之前，您必须具有一些有关矩阵的基本知识，例如矩阵的乘法和矩阵的转置。如果没有，请访问以下网站：

http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product_.28two_matrices.29
http://www.mathwarehouse.com/algebra/matrix/multiply-matrix.php
http://en.wikipedia.org/wiki/Transpose
http://en.wikipedia.org/wiki/Covariance_matrix
对于不知道卡尔曼滤波器是什么的人来说，它是一种使用一段时间内观察到的一系列测量值的算法，在这里，是加速度计和陀螺仪的数据。这些测量将包含会导致测量误差的噪声。然后，卡尔曼滤波器将尝试根据当前和以前的状态来估计系统状态，该状态比单独的测量更为精确。

在这种情况下，问题在于，由于机器人来回运动，当加速度计用于测量重力加速度时，加速度计通常非常嘈杂。陀螺仪的问题在于它会随时间漂移-就像旋转的陀螺仪在失去速度时会开始掉落一样。
简而言之，可以说您只能短期信任陀螺仪，但能长期信任加速度计。

实际上，有一个非常简单的方法可以通过使用一个免费的滤波器来解决这个问题，该滤波器基本上只由加速度计上的数字低通滤波器和陀螺仪读数上的数字高通滤波器组成。但是它不如卡尔曼滤波器那么精确，但是其他人已经使用微调的免费滤波器成功构建了平衡机器人。

有关陀螺仪，加速度计和complimentary滤波器的更多信息，请参见此pdf（http://web.mit.edu/scolton/www/filter.pdf）。在下面的博客文章中可以找到互补滤波器和卡尔曼滤波器之间的比较。

卡尔曼滤波器通过基于测量结果生成系统状态的统计最优估计来进行操作。为此，需要知道滤波器输入的噪声(称为测量噪声)，还需要知道系统本身的噪声(称为过程噪声)。为此，噪声必须是高斯分布并且均值为零，幸运的是，对于我们来说，大多数随机噪声都具有此特性。
有关过滤器原理的更多信息，请查看以下页面：

http://en.wikipedia.org/wiki/kalman_filter
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

1. 系统状态 ${\mathbf{x}}_k$

对于某些人来说，本文的下一部分可能看起来很混乱，但是我向您保证，如果您精通数学，那么只要拿起笔和一张纸并尝试推导它，就不那么困难了。

如果像我一样，如果您没有可用于矩阵的计算器或计算机程序，那么我建议您使用免费的在线计算器Wolfram Alpha：http://www.wolframalpha.com/。我将其用于本文的所有计算。

我将使用与Wikipedia：http://en.wikipedia.org/wiki/Kalman_filter文章相同的表示法，但我想指出的是，当矩阵是常数且不依赖于当前时间时，您不必在其后写k。因此，例如$F_k$可以简化为F。

我也想对这些符号的其他方面做一个小的解释。
首先，我将记录一下所谓的先前状态：
$${\hat{\mathbf{x}}}_{k-1|k-1}$$

这是基于先前状态及其之前状态的估计的先前估计状态。
接下来是先验状态：
$${\hat{\mathbf{x}}}_{k|k-1}$$

先验表示基于系统的先前状态及其之前状态的估计，在当前时间k对状态矩阵的估计。
最后一个称为后验状态：
$${\hat{\mathbf{x}}}_{k|k}$$

是在给定k并包括k的情况下在k时刻估计的状态。
问题在于系统状态本身是隐藏的，只能通过观察来观察$z_k$。这也称为隐马尔可夫模型。

这意味着该状态将基于时间k处的状态以及所有先前状态。这也意味着在kalman滤波器稳定之前，您不能相信状态的估计–请看我作业首页的图表。

$\hat{x}$是对状态的估计。与x意味着真实状态的单一状态不同-我们试图估计的状态。
因此，在时间k的状态表示为：
$${\mathbf{x}}_k$$
如果由下式给出，则系统在时间k的状态：

$${\mathbf{x}}_k=\mathbf{F}{x}_{k-1}+\mathbf{B}{u}_k+w_k$$
$x_k$状态矩阵表示为：

$${\mathbf{x}}_k={\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_k$$
如您所见，滤波器的输出将是角度$\theta$，${\dot{\theta }}_b$是基于加速度计和陀螺仪的测量结果的偏差。偏差是陀螺仪漂移的量。这意味着可以通过从陀螺仪测量值中减去偏差来获得真实速率。

下一个是F矩阵，它是应用于prevouis状态的状态转换模型$x_{k-1}$。

在这种情况下F定义为：
$$\mathbf{F}=\left[\begin{array}{cc}1& -\Delta t\\ 0& 1\end{array}\right]$$
我知道这种方法$-\Delta t$似乎令人困惑，但是稍后会有意义(请看我的评论)。

接下来是控制输入$u_k$，在这里，它是在时间k处以每秒度数(°/ s)为单位的陀螺仪测量值，也称为速率$\dot{\theta }$。我们实际上将状态方程重写为：
$${\mathbf{x}}_k=\mathbf{F}{x}_{k-1}+\mathbf{B}{\dot{\theta }}_k+w_k$$
接下来是B矩阵。称为控制输入模型，定义为：
$$\mathbf{B}=\left[\begin{array}{c}\Delta t\\ 0\end{array}\right]$$
这是完全合理的，因为当您将速率$\dot{\theta }$与增量时间$\Delta t$相乘时，您将获得角度$\theta$，并且由于我们无法直接基于该速率计算偏差，因此我们将设置 矩阵为0。

$w_k$是过程噪声，它是高斯分布，均值为零，并且与Q时间k呈协方差：

$${\mathbf{w}}_k\sim N\left(0，{\mathbf{Q}}_k\right)$$
$Q_k$是过程噪声协方差矩阵，在这种情况下是加速度计和偏差状态估计的协方差矩阵。在这种情况下，我们将认为偏置和加速度计的估计值是独立的，因此它实际上等于加速度计和偏置的估计值的方差。
最终矩阵的定义如下：
$${\mathbf{Q}}_k=\left[\begin{array}{cc}{Q}_{\theta }& 0\\ 0& Q_{{\dot{\theta }}_b}\end{array}\right]\Delta t$$
如您所见，$Q_k$协方差矩阵取决于当前时间k，因此加速度计方差$Q_{\theta }$和偏差方差
$Q_{{\dot{\theta }}_b}$ 乘以增量时间$\Delta t$。
这是有道理的，因为自从上次更新状态以来，过程噪声会随着时间的延长而变大。例如，陀螺仪可能已经漂移了。
您必须知道这些常数才能使kalman滤波器起作用。

请注意，如果设置较大的值，则状态估计中的噪声会更多。 因此，例如，如果估计角度开始漂移，则必须增加$Q_{{\dot{\theta }}_b}$的值。 否则，如果估计趋于缓慢，则说明您对角度的估计过于信任，应尝试减小$Q_{\theta }$的值以使其更具响应性。

2. 测量${\mathbf{z}}_k$

现在我们来看一下真实状态$x_k$的观测值或测量值$z_k$。观察结果$z_k$如下：

$${\mathbf{z}}_k=\mathbf{H}{x}_k+v_k$$
如您所见，测量$z_k$是通过当前状态$x_k$乘以H矩阵加上测量噪声得出的$v_k$。

H称为观察模型，用于将真实状态空间映射到观察空间。无法观察到真实状态。由于测量只是来自加速度计的测量，因此H可表示为：
$$\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$$
测量的噪声必须是高斯分布，并且均值为零，并且R协方差为：
$${\mathbf{v}}_k\sim N\left(0，\mathbf{R}\right)$$
但是由于R不是矩阵，所以测量噪声仅等于测量的方差，因为同一变量的协方差等于方差。 有关更多信息，请参见此页面。
现在我们可以这样定义R：

$$\mathbf{R}=E\left[\begin{array}{cc}{v}_k& {v_k}^T\end{array}\right]=var(v_k)$$
有关协方差的更多信息，请参见Wikipedia和我的作业。

我们将假设测量噪声是相同的，并且不依赖于时间k：

$$var(v_k)=var(v)$$
请注意，如果将测量噪声方差$var(v)$设置得太高，则滤波器对新测量的信任度会降低，因此响应会非常缓慢，但是如果该值太小，则该值可能会过冲并且会产生噪声，因为我们对加速度计的测量结果过于信任。

因此，要四舍五入，您必须找到过程噪声方差$Q_{\theta }和Q_{{\dot{\theta }}_b}$以及测量噪声$var(v)$的测量方差。 有多种找到它们的方法，但这超出了本文的范围。

3. 卡尔曼滤波方程

现在，我们将使用这些方程来估计系统在时间k的真实状态${\hat{x}}_k$。一些聪明的家伙想出了下面的等式来估计系统的状态。
这些方程式可以写得更紧凑，但我更喜欢将其扩展，因此更易于实现和理解不同的step 。

3.1 预测

在前两个方程中，我们将尝试预测时间k的当前状态和误差协方差矩阵。首先，过滤器将尝试基于所有先前状态和陀螺仪测量来估计当前状态：
$${\hat{\mathbf{x}}}_{k|k-1}=\mathbf{F}{\hat{x}}_{k-1|k-1}+\mathbf{B}{\dot{\theta }}_k$$

这就是为什么将其称为控制输入的原因，因为我们将其用作估计当前时间k的状态的额外输入，称为先验状态${\hat{x}}_{k|k-1}$，如本文开头所述。
接下来的事情是，我们将尝试估计先验误差协方差矩阵$P_{k|k-1}$基于先前的误差协方差矩阵$P_{k-1|k-1}$，其定义为：
$${\mathbf{P}}_{k|k-1}=\mathbf{F}{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}^T+{\mathbf{Q}}_k$$

该矩阵用于估计我们对估计状态的当前值的信任程度。 越小，我们越信任当前的估计状态。 上面方程的原理实际上很容易理解，因为很明显，自从我们上次更新状态估计值以来，误差协方差会增加，因此我们将误差协方差矩阵乘以状态转换模型F和转置$F^T$的乘积，并在时间k加上当前过程噪声$Q_k$。

P在我们的情况下，误差协方差矩阵是2×2矩阵：
$$\mathbf{P}=\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]$$

3.2. 更新

我们要做的第一件事是计算测量值$z_k$与先验状态之间的差$x_{k|k-1}$，这也称为创新：
$${\tilde{\mathbf{y}}}_k={\mathbf{z}}_k-\mathbf{H}{\hat{x}}_{k|k-1}$$
观测模型H用于将先验状态映射$x_{k|k-1}$到观测空间，该状态是来自加速度计的测量值，因此，该创新不是矩阵
$${\tilde{\mathbf{y}}}_k={\left[\begin{array}{c}\tilde{\mathbf{y}}\end{array}\right]}_k$$
我们要做的下一步是计算所谓的创新协方差：
$${\mathbf{S}}_k=\mathbf{H}{\mathbf{P}}_{k|k-1}{\mathbf{H}}^T+\mathbf{R}$$

它的作用是尝试根据先验误差协方差矩阵$P_{k|k-1}$和测量协方差矩阵预测我们应该信任测量的程度R。观测模型H用于将先验误差协方差矩阵映射$P_{k|k-1}$到观测空间。
测量噪声的值越大，S的值就越大小号，这意味着我们不太相信传入的测量。
在这种情况下S，不是矩阵，而是写为：
$${\mathbf{S}}_k={\left[\begin{array}{c}\mathbf{S}\end{array}\right]}_k$$
下一步是计算卡尔曼增益。卡尔曼增益用于表示我们对创新的信任程度，其定义为：
$${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}^T{\mathbf{S}}_k^{-1}$$
您会看到，如果我们不信任创新，那么创新协方差小号会很高；如果我们信任状态的估计值，那么误差协方差矩阵P将很小，因此，如果我们信任创新，卡尔曼增益将很小且相反但不信任当前状态的估计。

如果您更深入地看，您会发现观察模型的转置H用于将误差协方差矩阵的状态映射P到观察空间。然后，我们乘以创新协方差的倒数，比较误差协方差矩阵小号。

这很有意义，因为我们将使用观察模型H从状态误差协方差中提取数据，并将其与创新协方差的当前估计值进行比较。

请注意，如果您不知道启动时的状态，则可以这样设置误差协方差矩阵：
$$\mathbf{P}=\left[\begin{array}{cc}L& 0\\ 0& L\end{array}\right]$$

其中L代表大量。
对于我的平衡机器人，我知道启动角度，并且可以通过校准找到启动时陀螺仪的偏置，因此我假设启动时状态是已知的，因此我按如下方式初始化误差协方差矩阵：
$$\mathbf{P}=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]$$
请查看我的校准例程以获取更多信息。

在这种情况下，卡尔曼增益为2×1矩阵：
$$\mathbf{K}=\left[\begin{array}{c}{K}_0\\ K_1\end{array}\right]$$
现在我们可以更新当前状态的后验估计：
$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k{\tilde{\mathbf{y}}}_k$$

这是通过将先验状态${\hat{x}}_{k|k-1}$与kalman增益乘以创新得到的${\tilde{y}}_k$。
请记住，创新${\tilde{y}}_k$是度量$z_k$与先验状态之间的差异${\hat{x}}_{k|k-1}$，因此创新可以是正面的，也可以是负面的。
当我们简单地校正先验状态的估计值时，就可以理解该方程式，该估计值${\hat{x}}_{k|k-1}$是使用先前的状态和陀螺仪测量以及测量(在此情况下为加速度计)计算得出的。

我们要做的最后一件事是更新后验误差协方差矩阵：
$${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{k}}_k\mathbf{H}){\mathbf{P}}_{k|k-1}$$
其中I称为身份矩阵，定义为：
$$\mathbf{I}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$
过滤器正在做的是，它基本上是根据我们对估计值的校正量对误差协方差矩阵进行自我校正。这是有道理的，因为我们根据先验误差协方差矩阵校正了状态$P_{k|k-1}$，而且根据创新协方差校正了状态$S_k$。

3.4 实现过滤器

在本节中，我将使用上面的公式将过滤器实现为简单的c ++代码，该代码可用于平衡机器人，四轴飞行器和其他需要计算角度，偏差或比率的应用。

如果您想要旁边的代码，可以在github上找到它：https : //github.com/TkJElectronics/kalmanFilter。

我将简单地在每个step 的顶部编写方程式，然后对其进行简化，然后在C中编写如何完成该方程式，最后，如我所使用的，我将在每个step 的底部链接至Wolfram Alpha中完成的计算。他们来做计算。

3.4.1 第1步：

$${\hat{\mathbf{x}}}_{k|k-1}=\mathbf{F}{\hat{x}}_{k-1|k-1}+\mathbf{B}{\dot{\theta }}_k$$
$${\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k|k-1}=\left[\begin{array}{cc}1& -\Delta t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k-1|k-1}+\left[\begin{array}{c}\Delta t\\ 0\end{array}\right]{\dot{\theta }}_k$$
$$={\left[\begin{array}{c}\theta -{\dot{\theta }}_b\Delta t\\ {\dot{\theta }}_b\end{array}\right]}_{k-1|k-1}+\left[\begin{array}{c}\Delta t\\ 0\end{array}\right]{\dot{\theta }}_k$$
$$=\left[\begin{array}{c}\theta -{\dot{\theta }}_b\Delta t+\dot{\theta }\Delta t\\ {\dot{\theta }}_b\end{array}\right]$$
$$=\left[\begin{array}{c}\theta +\Delta t(\dot{\theta }-{\dot{\theta }}_b)\\ {\dot{\theta }}_b\end{array}\right]$$
如您所见，角度的先验估计${\hat{\theta }}_{k|k-1}$等于先前状态的估计${\hat{\theta }}_{k-1|k-1}$加上无偏率乘以增量时间$\Delta t$。
由于我们无法直接测量偏差，因此先验偏差的估算值恰好等于前一个偏差。

可以这样用C编写：

rate= newRate -bias;
angle + = dt * rate;

请注意，我计算的是无偏速率，因此用户也可以使用它。

Wolfram Alpha链接：

Eq. 1.1

3.4.2 第2步：

$${\mathbf{P}}_{k|k-1}=\mathbf{F}{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}^T+{\mathbf{Q}}_k$$
$${\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}=\left[\begin{array}{cc}1& -\Delta t\\ 0& 1\end{array}\right]{\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k-1|k-1}\left[\begin{array}{cc}1& 0\\ -\Delta t& 1\end{array}\right]+\left[\begin{array}{cc}{Q}_{\theta }& 0\\ 0& Q_{{\dot{\theta }}_b}\end{array}\right]\Delta t$$
$$={\left[\begin{array}{cc}{P}_{00}-\Delta t{P}_{10}& P_{01}-\Delta t{P}_{11}\\ P_{10}& P_{11}\end{array}\right]}_{k-1|k-1}\left[\begin{array}{cc}1& 0\\ -\Delta t& 1\end{array}\right]+\left[\begin{array}{cc}{Q}_{\theta }& 0\\ 0& Q_{{\dot{\theta }}_b}\end{array}\right]\Delta t$$
$$={\left[\begin{array}{cc}{P}_{00}-\Delta t{P}_{10}-\Delta t(P_{01}-\Delta t{P}_{11})& P_{01}-\Delta t{P}_{11}\\ P_{10}-\Delta t{P}_{11}& P_{11}\end{array}\right]}_{k-1|k-1}+\left[\begin{array}{cc}{Q}_{\theta }& 0\\ 0& Q_{{\dot{\theta }}_b}\end{array}\right]\Delta t$$
$$=\left[\begin{array}{cc}{P}_{00}-\Delta t{P}_{10}-\Delta t(P_{01}-\Delta t{P}_{11})+Q_{\theta }\Delta t& P_{01}-\Delta t{P}_{11}\\ P_{10}-\Delta t{P}_{11}& P_{11}+Q_{{\dot{\theta }}_b}\Delta t\end{array}\right]$$
$$=\left[\begin{array}{cc}{P}_{00}+\Delta t(\Delta t{P}_{11}-P_{01}-P_{10}+Q_{\theta })& P_{01}-\Delta t{P}_{11}\\ P_{10}-\Delta t{P}_{11}& P_{11}+Q_{{\dot{\theta }}_b}\Delta t\end{array}\right]$$
上面的等式可以用C编写，如下所示：

P[0][0] += DT * ( DT * P[1][1] - P[0][1] - P[1][0] + Q_angle );
P[0][1] -= dt * P[1][1] ;
P[1][0] -= dt * P[1][1] ;
P[1][1] += Q_gyroBias * dt ;

请注意，这是我使用的原始代码中存在错误的代码部分。

Wolfram Alpha链接：

Eq. 2.1
Eq. 2.2
Eq. 2.3
Eq. 2.4

3.4.3 第三步：

$${\tilde{\mathbf{y}}}_k={\mathbf{z}}_k-\mathbf{H}{\hat{x}}_{k|k-1}={\mathbf{z}}_k-\left[\begin{array}{cc}1& 0\end{array}\right]{\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k|k-1}={\mathbf{z}}_k-{\theta }_{k|k-1}$$
可以像这样用C计算创新：

y = newAngle - angle ;

Wolfram Alpha链接：

Eq. 3.1

3.4.4 第四步：

$${\mathbf{S}}_k=\mathbf{H}{\mathbf{P}}_{k|k-1}{\mathbf{H}}^T+\mathbf{R}$$
$$=\left[\begin{array}{cc}1& 0\end{array}\right]{\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}\left[\begin{array}{c}1\\ 0\end{array}\right]+\mathbf{R}$$
$$={P_{00}}_{k|k-1}+\mathbf{R}$$
$$={P_{00}}_{k|k-1}+var(v)$$
同样，C代码非常简单：

S = P [ 0 ] [ 0 ] + R_{measure} ;

Wolfram Alpha链接：

Eq. 4.1

3.4.5 step 5：

$${\mathbf{k}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}^T{\mathbf{S}}_k^{-1}$$
$${\left[\begin{array}{c}{k}_0\\ k_1\end{array}\right]}_k={\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}\left[\begin{array}{c}1\\ 0\end{array}\right]{\mathbf{S}}_k^{-1}$$
$$={\left[\begin{array}{c}{P}_{00}\\ P_{10}\end{array}\right]}_{k|k-1}{\mathbf{S}}_k^{-1}$$
$$=\frac{{\left[\begin{array}{c}{P}_{00}\\ P_{10}\end{array}\right]}_{k|k-1}}{{\mathbf{S}}_k}$$
请注意，在其他情况下，它S可以是矩阵，而不能仅P由除以S。相反，您必须计算矩阵的逆。有关如何操作的更多信息，请参见下一页。

C实现看起来像这样：

k [ 0 ] = P [ 0 ] [ 0 ] / S ;
k [ 1 ] = P [ 1 ] [ 0 ] / S ;

Wolfram Alpha链接：

Eq. 5.1

3.4.6 step 6：

$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k{\tilde{\mathbf{y}}}_k$$
$${\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k|k}={\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k|k-1}+{\left[\begin{array}{c}{K}_0\\ K_1\end{array}\right]}_k{\tilde{\mathbf{y}}}_k$$
$$={\left[\begin{array}{c}\theta \\ {\dot{\theta }}_b\end{array}\right]}_{k|k-1}+{\left[\begin{array}{c}{K}_0\tilde{\mathbf{y}}\\ K_1\tilde{\mathbf{y}}\end{array}\right]}_k$$
再一次，方程式的结尾很短，可以这样用C编写：

angle + = k [ 0 ] * y ;
bias + = k [ 1 ] * y ;

3.4.7 step 7：

$${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{k}}_k\mathbf{H}){\mathbf{P}}_{k|k-1}$$
$${\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k}=\left(\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]-{\left[\begin{array}{c}{k}_0\\ k_1\end{array}\right]}_k\left[\begin{array}{cc}1& 0\end{array}\right]\right){\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}$$
$$=\left(\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]-{\left[\begin{array}{cc}{k}_0& 0\\ k_1& 0\end{array}\right]}_k\right){\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}$$
$$={\left[\begin{array}{cc}{P}_{00}& P_{01}\\ P_{10}& P_{11}\end{array}\right]}_{k|k-1}-\left[\begin{array}{cc}{k}_0{P}_{00}& k_0{P}_{01}\\ k_1{P}_{00}& k_1{P}_{01}\end{array}\right]$$
请记住，由于状态估计的误差已减小，因此我们再次减小了误差协方差矩阵。
C代码如下所示：

float P00_temp = P [ 0 ] [ 0 ] ;
float  P01_temp = P [ 0 ] [ 1 ] ;

P [ 0 ] [ 0 ] -= k [ 0 ] * P00_temp ;
P [ 0 ] [ 1 ] -= k [ 0 ] * P01_temp ;
P [ 1 ] [0 ] -= k [ 1 ] * P00_temp ;
P [ 1 ] [ 1 ] -= k [ 1 ] * P01_temp ;

Wolfram Alpha链接：

Eq. 7.1
Eq. 7.2
Eq. 7.3
请注意，我发现以下变化对大多数IMU都适用：

float Q_angle = 0.001 ;
float Q_gyroBias = 0.003 ;
float R_measure = 0.03 ;
请记住，如果需要在启动时使用输出，则在启动时设置目标角度非常重要。有关更多信息，请参见平衡机器人的校准例程。

如果您错过了它，这里是我编写的库，任何支持float 数学的微控制器都可以使用该库。可以在github上找到源代码：https : //github.com/TkJElectronics/kalmanFilter。

如果您喜欢有关卡尔曼滤波器的视频说明，建议您观看以下视频系列：http : //www.youtube.com/watch?v=FkCT_LV9Syk。

请注意，如果您需要以完整的3D方向表示物体，则不能使用该库，因为欧拉角会受到所谓的“ 云台锁定”的困扰，您将需要使用四元数来做到这一点，但这是一个完整的故事。现在，请看下面的页面。

这是所有已知的信息，希望对您有帮助，或者您有任何疑问可以在下面发表评论，如果您需要编写方程式，它也支持LaTex语法。
如果您发现任何错误，也请通知我。

另外，作为一名控制工程师，我想补充一点细节：万向节锁定的发生是由于系统的机械特性，与数学表示无关。它反映为欧拉角表示中的自由度损失和雅可比矩阵中的奇点。但是四元数也会发生同样的问题。尽管四元数不会受到奇异性的影响，因此可以跟踪角度，但是仍然会发生机械万向节锁定。考虑一下机械臂，您会发现有时一只手腕必须非常(几乎无限地)快速运动。发生这种情况是由于用于构建机器人的欧拉角的奇异性(可以在公式中使用欧拉角来反映)，但是即使我们使用四元数进行跟踪(也可以正确跟踪，但速度是无限的)，也会发生这种情况。不幸的是，我们无法基于四元数构建机器人手臂。问题在于逆动力学。

最后，从过滤的角度来看，只能通过欧拉角观察到万向节锁定，但是从控制的角度来看，由于机器人的机械原理，可能发生这种情况。

原文最后的评论很精彩