卡尔曼滤波简单笔记整理

摘要

本文笔记根据b站DR_CAN博主的卡尔曼滤波器视频和另一位博主scoutee的滤波笔记一：卡尔曼滤波（Kalman Filtering）详解整理。并在章节5中贴上了自己使用stm32对陀螺仪数据进行滤波的代码，可以根据代码进行拓展使用。

章节1：递归算法

卡尔曼滤波器（Kalman Filter）实际上是一种Optimal Recursive Data Processing Algorithm（最优递归数字处理算法）

章节1.1：例子

假设我用一把尺子测量一个硬币的直径 $Z_k$，如下图所示 。
当需要估计真实值的时候，我们很自然会想到取平均，那么有硬币直径的估计值${\hat{X}}_k$：
$$\begin{array}{rl}& {\hat{X}}_k=\frac{1}{k}(Z_1+Z_2+\cdots +Z_k)\\ & =\frac{1}{k}(Z_1+Z_2+\cdots +Z_{k-1})+\frac{1}{k}{Z}_k\\ & =\frac{k-1}{k}\frac{1}{k-1}(Z_1+Z_2+\cdots +Z_{k-1})+\frac{1}{k}{Z}_k\\ & =\frac{k-1}{k}{\hat{X}}_{k-1}+\frac{1}{k}{Z}_k\\ & ={\hat{X}}_{k-1}+\frac{1}{k}(Z_k-{\hat{X}}_{k-1})\end{array}\text{\hspace{1em}(1)}$$
我们可以看到随着k的增大，1/k趋向于0, 有 ${\hat{X}}_k\to {\hat{X}}_{k-1}$。也就是说：随着测量次数的增多，测量值$Z_k$的值变得越来越不重要，因为我们已经得到较为准确的估计值${\hat{X}}_k$。同理如果测量次数k比较小，那么每次测量得到的结果对估计值相当重要。例如，当k=1时，${\hat{X}}_k=Z_1$。
令 $K_k=\frac{1}{k}$，那么可以得到:
$${\hat{X}}_k={\hat{X}}_{k-1}+K_k(Z_k-{\hat{X}}_{k-1})\text{\hspace{1em}(2)}$$

该公式的理解为：当前的估计值 = 上一次的估计值 + 系数 *（当前的测量值-上一次的估计值）
也就是说当前的估计值与上次估计值相关，上次估计值又与上上次有关。所以这种算法是一种递归算法，其中$K_k$被称为卡尔曼系数/增益。但同时卡尔曼滤波器又不需要很多数据，仅需要上次的数据即可，这也是卡尔曼滤波器的优势之一。

章节1.2：误差与卡尔曼增益

1. 误差和卡尔曼增益的计算关系：

   • 估计误差（Estimate Error）: $e_{EST}$

   • 测量误差（Measurement Error）：$e_{MEA}$

   • 根据测量误差和估计误差，可以得到卡尔曼增益： $K_k=\frac{e_{EST}^{k-1}}{e_{EST}^{k-1}+e_{MEA}^k}$

   • 估计误差更新：$e_{EST}=（1-K_k）e_{EST}^{k-1}$
     我们分析一下：
     $卡尔曼增益=\frac{上一次估计误差}{上一次估计误差+这次测量误差}$

     • 假若$e_{EST}^{k-1}⋙e_{MEA}^k$，那么$K_k\to 1$，有${\hat{X}}_k={\hat{X}}_{k-1}+1\ast (Z_k-{\hat{X}}_{k-1})=Z_k$，也就是说当第k-1次估计误差远大于第k次测量误差时候，我们会更相信测量值。
     • 假若$e_{EST}^{k-1}⋘e_{MEA}^k$，那么$K_k\to 0$，有${\hat{X}}_k={\hat{X}}_{k-1}+0\ast (Z_k-{\hat{X}}_{k-1})={\hat{X}}_{k-1}$，我们此时更相信上一次估计值。

2. 举个例子：
   假设实际长度50cm，测量值为51cm，测量误差是3cm（测量仪器的固有参数，整个过程保持不变），即实际长度在47-53之间。假设初始估计值是48cm（在我的理解中，第一次的估计值按照测量值来算好了），初始估计误差为3cm。如表所示为递归结果。
   

章节2：数据融合算法

章节2.1：例子

假设有两个秤去测量同一个东西，两个秤测量结果服从正态分布。

1. $Z_1=30g$，标准差为${\sigma }_{Z_1}=2g$
2. $Z_2=32g$，标准差为${\sigma }_{Z_2}=3g$
   那么其分布结果如图所示：
   
   根据两个秤的结果估算实际重量$\hat{Z}$，那么直觉来看，结果肯定位于$Z_1$和$Z_2$这两个值之间。并且，由于第一个秤的标准差更小，结果应该会更靠近第一个秤。融合方式为：
   $$\hat{Z}=Z_1+K_k(Z_2-Z_1)，K_k\in [0,1]\text{\hspace{1em}(3)}$$
   其中，$K_k$为卡尔曼增益。要使$\hat{Z}$最准确，即要使$\hat{Z}$的方差最小，那么有：
   $$\begin{array}{rl}& {\sigma }_{\hat{z}}^2=Var(Z_1+K_k(Z_2-Z_1))\\ & =Var((1-K_k)Z_1+Z_2)\end{array}\text{\hspace{1em}(4)}$$
   由于两个秤的结果$Z_1$和$Z_2$是相互独立的，所以：
   $$\begin{array}{rl}& {\sigma }_{\hat{z}}^2=Var((1-K_k)Z_1+Z_2)\\ & =Var((1-K_k)Z_1)+Var(Z_2)\\ & =(1-K_k{)}^2{\sigma }_{Z_1}^2+{\sigma }_{Z_2}^2\end{array}\text{\hspace{1em}(5)}$$
   公式$5$对$K_k$求导，并令其为0：$\frac{d{\sigma }_{\hat{z}}^2}{d{K}_k}=0$，求解得到：$K_k=\frac{{\sigma }_{Z_1}^2}{{\sigma }_{Z_1}^2+{\sigma }_{Z_2}^2}=\frac{2^2}{2^2+3^2}=\frac{4}{13}$
   代回公式$3$中可得最优值：$\hat{Z}=30+\frac{4}{13}(32-30)=30.615$，代回公式$5$中可得标准差：${\sigma }_{\hat{z}}=1.583$。可以看到，经过数据融合之后的数据方差是要小于两个秤的方差的。我对此的理解是两个不确定的结果融合之后可以增加数据的可信度，就好像俗话说：三个臭皮匠顶个诸葛亮。也就是说任何事情在自己不确定的情况下，找个人讨论讨论就可以增加结果的成功性。嘿嘿\dog

章节3：协方差矩阵和状态空间方程

章节3.1：协方差矩阵

协方差矩阵可以同时将方差和协方差体现出来描述变量之间的联动性。

1. 举个例子：
   球队中各球员的信息如下表格所示：

球员	身高 x	体重 y	年龄 z
张三	182	80	29
李四	180	77	26
王五	193	86	29
平均值	185	81	28

• 那么可以计算:

  • 身高方差为${\sigma }_x^2=\frac{1}{3}[(182-185{)}^2+(180-185{)}^2+(193-185{)}^2]=32.67$

  • 体重方差为${\sigma }_y^2=\frac{1}{3}[(80-81{)}^2+(77-81{)}^2+(86-81{)}^2]=14$

  • 年龄方差为${\sigma }_z^2=\frac{1}{3}[(29-28{)}^2+(26-28{)}^2+(29-28{)}^2]=2$

  • 协方差为(协方差为正，代表两变量具有正相关性 )：

    • ${\sigma }_x{\sigma }_y=\frac{1}{3}[(182-185)(80-81)+(180-185)(77-81)+(193-185)(86-81)]=21={\sigma }_y{\sigma }_x$
    • ${\sigma }_x{\sigma }_z=\frac{1}{3}[(182-185)(29-28)+(180-185)(26-28)+(193-185)(29-28)]=5={\sigma }_z{\sigma }_x$
    • ${\sigma }_y{\sigma }_z=\frac{1}{3}[(80-81)(29-28)+(77-81)(26-28)+(86-81)(29-28)]=4={\sigma }_z{\sigma }_y$

  因此协方差矩阵为$P=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ {\sigma }_y{\sigma }_x& {\sigma }_y^2& {\sigma }_y{\sigma }_z\\ {\sigma }_z{\sigma }_x& {\sigma }_z{\sigma }_y& {\sigma }_z^2\end{array}\right]=\left[\begin{array}{ccc}32.67& 21& 5\\ 21& 14& 4\\ 5& 4& 2\end{array}\right]$

• 利用矩阵直接计算协方差矩阵：

  • 已知$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]=\left[\begin{array}{ccc}182& 80& 29\\ 180& 77& 26\\ 193& 86& 29\end{array}\right]$
  • 令a=$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]-\frac{1}{3}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$
  • 那么协方差矩阵为：$P=\frac{1}{3}{a}^{T}a$

章节3.2：状态空间方程

1. 举个例子：通过弹簧振动阻尼系统学习装填空间方程的表达形式
   
   已知弹簧胡克系数是k，阻尼系数是B，向右移动的距离是x，向右的力是F，物体质量为m。那么有动态状态方程：
   $$m\ddot{x}+B\dot{x}+kx=F\text{\hspace{1em}(7)}$$
   其中， $\ddot{x}=\frac{d^{2}x}{d{t}^2}=a$(加速度) ，$\dot{x}=\frac{dx}{dt}=v$（速度），F又可以视为系统输入u 。该系统中，系统状态变量为：
   $$x_1=x，x_2=\dot{x}\text{\hspace{1em}(8)}$$
   那么有：

   • 运动状态方程 ：
     $$\begin{array}{rl}& \dot{x_1}=x_2\\ & \dot{x_2}=\ddot{x}=(F-B\dot{x}-kx)/m=\frac{1}{m}u-\frac{B}{m}{x}_2-\frac{k}{m}{x}_1\end{array}\text{\hspace{1em}(9)}$$
   • 测量方程为：
     $$\begin{array}{rl}& z_1=x=x_1，(位置)\\ & z_2=\dot{x}=x_2，(加速度)\end{array}\text{\hspace{1em}(10)}$$

   因此状态空间的表达形式为：
   $$\left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{B}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u\text{\hspace{1em}(11)}$$
   $$\left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]\text{\hspace{1em}(12)}$$
   其中公式$11$代表运动状态方程，公式$12$代表测量方程。
   理想情况下

   • 状态空间时间连续的表达形式为：
     $$\begin{array}{rl}& \dot{X}(t)=Ax(t)+Bu(t)\\ & Z=Hx(t)\end{array}\text{\hspace{1em}(13)}$$
   • 状态空间时间离散的表达形式为：
     $$\begin{array}{rl}& x_k=A{x}_{k-1}+B{u}_{k-1}\\ & z_k=H{x}_k\end{array}\text{\hspace{1em}(14)}$$

   实际情况下，由于存在噪声影响，状态空间表达方式修正如下：

   • 状态空间时间连续的表达形式为：
     $$\begin{array}{rl}& \dot{X}(t)=Ax(t)+Bu(t)+w(t)\\ & Z=Hx(t)+v(t)\end{array}\text{\hspace{1em}(15)}$$
   • 状态空间时间离散的表达形式为：
     $$\begin{array}{rl}& x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ & z_k=H{x}_k+v_k\end{array}\text{\hspace{1em}(16)}$$

2. 如何通过状态空间方程得到相对准确的估计值
   由于现实条件下，存在噪声影响，导致模型（$x_k$）和测量（$z_k$）均不准确，那么如何在不准确的情况下估计相对准确的${\hat{x}}_k$？这就是卡尔曼滤波器要解决的问题。

   还记得我们在章节2提到的数据融合算法吗，实际上卡尔曼滤波器就是实现数据融合，即：
   

章节4：卡尔曼增益公式推导和误差协方差矩阵公式推导

章节4.1：卡尔曼增益公式推导

1. 如公式$16$所示，我们认为$x_k$是状态变量，$A$是状态转换矩阵，$B$是控制转换矩阵，$H$是量测矩阵，$u_{k-1}$是k-1刻的系统输入，$w_{k-1}$和$v_k$分别为过程噪声和测量噪声，并且我们认为噪声均服从高斯分布（$P(w)\sim (0,Q),P(v)\sim (0,R)$），但过程噪声是不可测的。因此，下一步为了方便建模，我们忽视掉噪声的影响。也是因为少了噪声这一项，所以下面的结果都是估计值，而不像公式15,16中是真实值。

2. 通过计算，已知 先验估计 ：
   $${\hat{\tilde{x}}}_k=A{\hat{x}}_{k-1}+B{u}_{k-1}\text{\hspace{1em}(17)}$$
   由于 测量值$z_k$可以由仪器直接测量得到，因此根据公式$14$，可以得到估计值：${\hat{x}}_k^{mea}=H^{-1}{z}_k$。那么根据计算得到的不准确先验估计值${\hat{\tilde{x}}}_k$和测量得到的不准确估计值${\hat{x}}_k^{mea}$融合之后得到最优估计值，即 后验估计值 ：
   $$\begin{array}{rl}& {\hat{x}}_k={\hat{\tilde{x}}}_k+G({\hat{x}}_k^{mea}-{\hat{\tilde{x}}}_k)\\ & ={\hat{\tilde{x}}}_k+G(H^{-1}{z}_k-{\hat{\tilde{x}}}_k)\end{array}\text{\hspace{1em}(18)}$$
   简单起见，令 $G=K_{k}H$，那么公式$18$可以化简得到最终的卡尔曼滤波公式：
   $${\hat{x}}_k={\hat{\tilde{x}}}_k+K_k(z_k-H{\hat{\tilde{x}}}_k)，K_k\in [0,H^{-1}]\text{\hspace{1em}(19)}$$
   为了使结果更可信，我们必须要寻找最优$K_k$，使得${\hat{x}}_k$更加趋向于真实值$x_k$。因此，接下来的过程就类似于章节2的推导过程。

3. 首先引入误差：$e_k=x_k-{\hat{x}}_k$，由于服从高斯分布的变量经过线性组合后仍服从高斯分布，因此$e\sim (0,p)$。$p$是协方差矩阵。
   实际上，当估计结果越准确，$e_k$的方差是越小的。由于协方差主对角线上元素相加值是该矩阵的迹，因此我们的优化目标就是让$p$的迹最小！！！

   • 计算$e_k$的协方差矩阵：$p_k=E[e_k{e}_k^T]=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$，将公式$16$和$19$代入可得：
     $$\begin{array}{rl}& x_k-{\hat{x}}_k=x_k-{\hat{\tilde{x}}}_k-K_k(z_k-H{\hat{\tilde{x}}}_k)\\ & =x_k-(I-K_{k}H){\hat{\tilde{x}}}_k-K_k{z}_k\\ & =x_k-(I-K_{k}H){\hat{\tilde{x}}}_k-K_k(H{x}_k+v_k)\\ & =(I-K_{k}H)(x_k-{\hat{\tilde{x}}}_k)-K_k{v}_k\end{array}\text{\hspace{1em}(20)}$$
     定义先验误差：${\tilde{e}}_k=x_k-{\hat{\tilde{x}}}_k$，有$\tilde{e}\sim (0,\tilde{p})$那么
     p k = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ]      = E { [ ( I − K k H ) e ~ k − K k v k ] [ ( I − K k H ) e ~ k − K k v k ] T }      = E { [ ( I − K k H ) e ~ k − K k v k ] [ e ~ k T ( I − K k H ) T − v k T K k T ] }      = E { ( I − K k H ) e ~ k e ~ k T ( I − K k H ) T − ( I − K k H ) e ~ k v k T K k T − K k v k e ~ k T ( I − K k H ) T + K k v k v k T K k T }      = E { ( I − K k H ) e ~ k e ~ k T ( I − K k H ) T } − E { ( I − K k H ) e ~ k v k T K k T } − E { K k v k e ~ k T ( I − K k H ) T } + E { K k v k v k T K k T } (21) \begin{aligned} &p_k=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \\ &\ \ \ \ = E\{[(I-K_kH )\tilde{e}_k-K_k{v_k}] [(I-K_kH )\tilde{e}_k-K_k{v_k}]^T\}\\ &\ \ \ \ = E\{[(I-K_kH )\tilde{e}_k-K_k{v_k}] [\tilde{e}_k^T(I-K_kH )^T-v_k^TK_k^T]\}\\ &\ \ \ \ = E\{(I-K_kH )\tilde{e}_k \tilde{e}_k^T(I-K_kH )^T-(I-K_kH )\tilde{e}_kv_k^TK_k^T-K_k{v_k}\tilde{e}_k^T(I-K_kH )^T+K_k{v_k}v_k^TK_k^T\}\\ &\ \ \ \ = E\{(I-K_kH )\tilde{e}_k \tilde{e}_k^T(I-K_kH )^T\}-E\{(I-K_kH )\tilde{e}_kv_k^TK_k^T\}-E\{K_k{v_k}\tilde{e}_k^T(I-K_kH )^T\}+E\{K_k{v_k}v_k^TK_k^T\}\\ \end{aligned} \tag{21} ​pk​=E[(xk​−x^k​)(xk​−x^k​)T]    =E{[(I−Kk​H)e~k​−Kk​vk​][(I−Kk​H)e~k​−Kk​vk​]T}    =E{[(I−Kk​H)e~k​−Kk​vk​][e~kT​(I−Kk​H)T−vkT​KkT​]}    =E{(I−Kk​H)e~k​e~kT​(I−Kk​H)T−(I−Kk​H)e~k​vkT​KkT​−Kk​vk​e~kT​(I−Kk​H)T+Kk​vk​vkT​KkT​}    =E{(I−Kk​H)e~k​e~kT​(I−Kk​H)T}−E{(I−Kk​H)e~k​vkT​KkT​}−E{Kk​vk​e~kT​(I−Kk​H)T}+E{Kk​vk​vkT​KkT​}​(21)
     方便起见，先但看第二项：$E\{(I-K_{k}H){\tilde{e}}_k{v}_k^T{K}_k^T\}$，由于$(I-K_{k}H)$，$K_k^T$是常数，因此：
     E { ( I − K k H ) e ~ k v k T K k T } = ( I − K k H ) E { e ~ k v k T } K k T E\{(I-K_kH )\tilde{e}_kv_k^TK_k^T\}=(I-K_kH )E\{\tilde{e}_kv_k^T\}K_k^T E{(I−Kk​H)e~k​vkT​KkT​}=(I−Kk​H)E{e~k​vkT​}KkT​。
     将公式17和先验误差代入，有：
     E { e ~ k v k T } = E { ( x k − x ^ k ) v k T } = E { ( A x k − 1 + B u k − 1 + w k − 1 − A x ^ k − 1 − B u k − 1 ) v k T } E\{\tilde{e}_kv_k^T\}=E\{(x_k -\hat{x}_k) v_k^T\}=E\{(Ax_{k-1}+Bu_{k-1}+\textcolor{red}{w_{k-1}} -A\hat{x}_{k-1}-Bu_{k-1}) v_k^T\} E{e~k​vkT​}=E{(xk​−x^k​)vkT​}=E{(Axk−1​+Buk−1​+wk−1​−Ax^k−1​−Buk−1​)vkT​}，这其中，${\hat{x}}_{k-1}$，$x_{k-1}$，$w_{k-1}$和$v_k^T$是非同一时刻的变量，因此，这两者相互独立互不影响，即： E { e ~ k v k T } = E [ A x k − 1 + w k − 1 − A x ^ k − 1 ] E [ v k T ] E\{\tilde{e}_kv_k^T\}=E[Ax_{k-1}+\textcolor{red}{w_{k-1}} -A\hat{x}_{k-1}]E[v_k^T] E{e~k​vkT​}=E[Axk−1​+wk−1​−Ax^k−1​]E[vkT​]，因为$E[v_k^T]=0$，故第二项值为0。同理可得第三项值为0。因此：
     p k = E { ( I − K k H ) e ~ k e ~ k T ( I − K k H ) T } + E { K k v k v k T K k T }      = ( I − K k H ) E { e ~ k e ~ k T } ( I − K k H ) T + K k E { v k v k T } K k T      = ( I − K k H ) p ~ k ( I − K k H ) T + K k R K k T      = ( p ~ k − K k H p ~ k ) ( I − H T K k T ) + K k R K k T      = p ~ k − p ~ k H T K k T − K k H p ~ k + K k H p ~ k H T K k T + K k R K k T (22) \begin{aligned} &p_k= E\{(I-K_kH )\tilde{e}_k \tilde{e}_k^T(I-K_kH )^T\}+E\{K_k{v_k}v_k^TK_k^T\}\\ &\ \ \ \ = (I-K_kH )E\{\tilde{e}_k \tilde{e}_k^T\}(I-K_kH )^T+K_kE\{{v_k}v_k^T\}K_k^T\\ &\ \ \ \ = (I-K_kH )\tilde{p}_k(I-K_kH )^T+K_kRK_k^T\\ &\ \ \ \ = (\tilde{p}_k-K_kH\tilde{p}_k )(I-H^T K_k^T)+K_kRK_k^T\\ &\ \ \ \ = \tilde{p}_k-\tilde{p}_kH^T K_k^T-K_kH\tilde{p}_k+K_kH\tilde{p}_kH^T K_k^T+K_kRK_k^T\\ \end{aligned} \tag{22} ​pk​=E{(I−Kk​H)e~k​e~kT​(I−Kk​H)T}+E{Kk​vk​vkT​KkT​}    =(I−Kk​H)E{e~k​e~kT​}(I−Kk​H)T+Kk​E{vk​vkT​}KkT​    =(I−Kk​H)p~​k​(I−Kk​H)T+Kk​RKkT​    =(p~​k​−Kk​Hp~​k​)(I−HTKkT​)+Kk​RKkT​    =p~​k​−p~​k​HTKkT​−Kk​Hp~​k​+Kk​Hp~​k​HTKkT​+Kk​RKkT​​(22)

   回到我们的原始目的，我们的优化目标就是让$p$的迹最小，那么：
   $$\begin{array}{rl}& tr(p_k)=tr({\tilde{p}}_k)-tr({\tilde{p}}_k{H}^T{K}_k^T)-tr(K_{k}H{\tilde{p}}_k)+tr(K_{k}H{\tilde{p}}_k{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)\\ & =tr({\tilde{p}}_k)-tr((K_{k}H{\tilde{p}}_k^T{)}^T)-tr(K_{k}H{\tilde{p}}_k)+tr(K_{k}H{\tilde{p}}_k{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)\\ & =tr({\tilde{p}}_k)-2tr(K_{k}H{\tilde{p}}_k)+tr(K_{k}H{\tilde{p}}_k{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)(转置不改变对角位置数据)\end{array}\text{\hspace{1em}(23)}$$
   对公式23进行求导：
   $$\begin{array}{rl}& \frac{dtr(p_k)}{d{K}_k}=\frac{dtr({\tilde{p}}_k)}{d{K}_k}-2\frac{dtr(K_{k}H{\tilde{p}}_k)}{d{K}_k}+\frac{dtr(K_{k}H{\tilde{p}}_k{H}^T{K}_k^T)}{d{K}_k}+\frac{dtr(K_{k}R{K}_k^T)}{d{K}_k}\\ & =0-2(H{\tilde{p}}_k{)}^T+2K_{k}H{\tilde{p}}_k{H}^T+2K_{k}R\end{array}\text{\hspace{1em}(24)}$$
   令公式24为0，得到 卡尔曼滤增益的最优取值(核心公式) ：
   $$K_k=\frac{{\tilde{p}}_k{H}^T}{H{\tilde{p}}_k{H}^T+R}\text{\hspace{1em}(25)}$$
   从公式25可以看出，当测量噪声R很大，卡尔曼增益为0，代入公式19，得：${\hat{x}}_k={\hat{\tilde{x}}}_k$，即此时结果全部相信先验估计值。当R很小的时候，可以忽略不计，则:${\hat{x}}_k=H^{-1}{z}_k$。该结果同样符合我们在章节2中得到的规律。
   根据公式22和25，可以得到 后验估计误差$e_k$协方差的更新公式：
   $$\begin{array}{rl}& p_k={\tilde{p}}_k-{\tilde{p}}_k{H}^T{K}_k^T-K_{k}H{\tilde{p}}_k+K_{k}H{\tilde{p}}_k{H}^T{K}_k^T+K_{k}R{K}_k^T\\ & ={\tilde{p}}_k(I-H^T{K}_k^T)-K_{k}H{\tilde{p}}_k+K_k(H{\tilde{p}}_k{H}^T+R)K_k^T\\ & ={\tilde{p}}_k(I-H^T{K}_k^T)-K_{k}H{\tilde{p}}_k+\frac{{\tilde{p}}_k{H}^T}{H{\tilde{p}}_k{H}^T+R}(H{\tilde{p}}_k{H}^T+R)K_k^T\\ & ={\tilde{p}}_k(I-H^T{K}_k^T)-K_{k}H{\tilde{p}}_k+{\tilde{p}}_k{H}^T{K}_k^T\\ & ={\tilde{p}}_k(I-H^T{K}_k^T)-K_{k}H{\tilde{p}}_k+{(K_{k}H{\tilde{p}}_k)}^T\\ & ={\tilde{p}}_k(I-H^T{K}_k^T)\end{array}\text{\hspace{1em}(26)}$$

章节4.2：误差的协方差矩阵公式推导

在上一节中，我们推导出来四个公式：

• 先验估计 ：
  $${\hat{\tilde{x}}}_k=A{\hat{x}}_{k-1}+B{u}_{k-1}\text{\hspace{1em}(27)}$$
• 后验估计 ：
  $${\hat{x}}_k={\hat{\tilde{x}}}_k+K_k(z_k-H{\hat{\tilde{x}}}_k)，K_k\in [0,H^{-1}]\text{\hspace{1em}(28)}$$
• 卡尔曼滤增益 ：
  $$K_k=\frac{{\tilde{p}}_k{H}^T}{H{\tilde{p}}_k{H}^T+R}\text{\hspace{1em}(29)}$$
• 后验估计误差$e_k$协方差：
  $$\begin{array}{rl}& p_k={\tilde{p}}_k(I-H^T{K}_k^T)\end{array}\text{\hspace{1em}(30)}$$
  四个公式中存在未知量先验估计误差协方差：${\tilde{p}}_k$，根据先验估计误差定义：${\tilde{e}}_k=x_k-{\hat{\tilde{x}}}_k$。将公式16（$x_k$定义）和27（${\hat{\tilde{x}}}_k$定义）代入，可以得到：
  $$\begin{array}{rl}& {\tilde{e}}_k=x_k-{\hat{\tilde{x}}}_k\\ & =A{x}_{k-1}+B{u}_{k-1}+w_{k-1}-A{\hat{x}}_{k-1}-B{u}_{k-1}\\ & =A(x_{k-1}-{\hat{x}}_{k-1})+w_{k-1}\\ & =A{e}_{k-1}+w_{k-1}\end{array}\text{\hspace{1em}(31)}$$

因此先验估计误差协方差为：
$$\begin{array}{rl}& {\tilde{p}}_k=E[{\tilde{e}}_k{\tilde{e}}_k^T]\\ & =E[(A{e}_{k-1}+w_{k-1})(A{e}_{k-1}+w_{k-1}{)}^T]\\ & =E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]\end{array}\text{\hspace{1em}(32)}$$
类似于我们在处理公式21的方式，式中第二项：$E[A{e}_{k-1}{w}_{k-1}^T]=AE\{(x_{k-1}-{\hat{x}}_{k-1})w_{k-1}^T\}=E\{(A{x}_{k-2}+B{u}_{k-2}+w_{k-2}-A{\hat{x}}_{k-2}-B{u}_{k-2})w_{k-1}^T\}$。
由于$w_{k-1}$和$w_{k-2}$，$x_{k-2}$，${\hat{x}}_{k-2}$处于非同一时刻，因此$e_{k-1}$和$w_{k-1}^T$相互独立。那么：$E[A{e}_{k-1}{w}_{k-1}^T]=E[A{e}_{k-1}]E[w_{k-1}^T]=0$，同理第三项也是0。因此可以得到:

• 先验估计误差协方差的计算公式（更新公式） ：
  $$\begin{array}{rl}& {\tilde{p}}_k=E[{\tilde{e}}_k{\tilde{e}}_k^T]\\ & =E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]\\ & =E[A{e}_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]\\ & =A{p}_{k-1}{A}^T+Q\end{array}\text{\hspace{1em}(33)}$$

章节5：卡尔曼滤波器对数据进行处理

根据第四章节，我们可以得到卡尔曼滤波器对数据进行处理的步骤（ 五个核心公式）为：

• 首先，在预测阶段：
  • 根据状态空间方程对数据进行 先验估计 ：${\hat{\tilde{x}}}_k=A{\hat{x}}_{k-1}+B{u}_{k-1}$
  • 更新 先验估计误差协方差 ：${\tilde{p}}_k=A{p}_{k-1}{A}^T+Q$
• 然后，根据测量值校正预测值：
  • 计算 卡尔曼增益：$K_k=\frac{{\tilde{p}}_k{H}^T}{H{\tilde{p}}_k{H}^T+R}$
  • 更新 后验估计误差协方差 ：$p_k={\tilde{p}}_k(I-H^T{K}_k^T)$
  • 根据数据融合算法，得到 后验估计：${\hat{x}}_k={\hat{\tilde{x}}}_k+K_k(z_k-H{\hat{\tilde{x}}}_k)$

个人理解，为了使用卡尔曼滤波器，需要初始化${\hat{x}}_0$和$p_0$。一般情况下我会将测量值的第一个值初始化为${\hat{x}}_0$，$p_0$也采用测量仪器测量误差协方差。

代码示例

使用方式示例：

• 先定义一个卡尔曼指针：Kalman_t p；
• 调用KalmanCreate()创建并初始化该卡尔曼滤波器；
• 读取传感器数据，然后调用KalmanFilter()滤波。

Kalman p;                  //  定义卡尔曼滤波器结构体                                            
float SersorData;           // 传感器数据                                
KalmanCreate(&p,1,10);      //  初始化卡尔曼滤波器                                      
while(1)                                                                
{                                                                            
	SersorData = sersor();                     // 获取传感器数据                                      
	SersorData = KalmanFilter(&p,SersorData);  // 传感器数据滤波                                                                       
}                                                                            

/**
  * @name   Kalman_t struct 
  * @brief  卡尔曼滤波器的结构体
  * @param  见内部的标注
  *         
  * @retval none
  * @attention 注意，由于此代码创建时只用于陀螺仪单轴数据滤波，因此所有的变量结构没有定义成矩阵格式。如果需要可以使用stm32FPU开启矩阵计算。
  */
typedef struct {
    float X_last; //上一时刻的后验估计值：\hat{X}_{k-1}
    float X_pre;  //当前时刻的先验估计值：\hat{\tilde{X}}_k
    float X_now;  //当下时刻的后验估计值: \hat{X}_k
    float P_mid;  //当前时刻先验估计误差协方差： \tilde{p}_k
    float P_now;  //当前时刻后验估计误差协方差： p_k
    float P_last; //上一时刻后验估计误差协方差：p_{k-1}
    float Kk;     //卡尔曼增益
    float A;      //状态转换矩阵
	float B;	  //控制转换矩阵
    float Q;		//系统噪声误差（协方差）
    float R;		//测量噪声误差（协方差）
    float H;		//量测矩阵
}Kalman_t;

/**
  * @name   kalmanCreate
  * @brief  创建卡尔曼滤波器
  * @param  p:  滤波器结构体
  *         T_Q:系统噪声误差（协方差）
  *         T_R:测量噪声误差（协方差）
  *         
  * @retval none
  */
void KalmanCreate(Kalman_t *p,float T_Q,float T_R)
{
    p->X_last = (float)0;
    p->P_last = 0;
    p->Q = T_Q;
    p->R = T_R;
    p->A = 1;
	p->B = 0;
    p->H = 1;
    p->X_mid = p->X_last;
}

/**
  * @name   KalmanFilter
  * @brief  卡尔曼滤波器
  * @param  p:  滤波器的结构体变量
  *         dat:	要滤波的数据
  * @retval 返回滤波后的数据
  * @attention 
  *            A=1 B=0 H=1 I=1  W(K)  V(k)是白噪声，叠加在测量值上了，可以忽略不计
  *            该函数对应我们本章总结的五个核心公式
  */

float KalmanFilter(Kalman_t* p,float dat)
{
    p->X_mid =p->A*p->X_last;                     //先验估计
    p->P_mid = p->A*p->P_last+p->Q;               //先验估计误差
    p->Kk = p->P_mid/(p->P_mid+p->R);             //卡尔曼增益
    p->X_now = p->X_mid+p->kg*(dat-p->X_mid);     //后验估计（数据融合）
    p->P_now = (1-p->Kk)*p->P_mid;                //后验估计误差
    
    p->P_last = p->P_now;                         //更新上一时刻后验估计误差
    p->X_last = p->X_now;							//更新上一时刻后验估计
    return p->X_now;							  //返回滤波后数据值
}

章节6：拓展卡尔曼滤波器-Extended Kalman Filter（EKF）

拓展卡尔曼滤波的作用就是对非线性测量系统进行滤波，但是由于我们前面讨论的都是针对线性系统的。而且原来服从高斯分布的变量经过非线性系统之后，不再服从高斯分布。所以，为了解决这问题，我们就要想办法把非线性系统通过近似的方式转化为线性系统。这个利器就是：泰勒展开。(等待后续增加)