滤波算法_标准卡尔曼滤波(SKF, Standard Kalman filter)_①基础铺垫

学习卡尔曼滤波不能一蹴而就，特别是对于基础薄弱者而言，需要一步一步来，在推导kalman滤波算法之前，需要学习一些基础知识作为铺垫。

1、递归算法

卡尔曼滤波，它本质上是一种最优的递归的数字处理算法。卡尔曼滤波器的应用非常广泛，这是因为在实际的系统当中，存在许多的不确定性，主要包含以下三个方面：
① 不存在完美的数学模型
② 系统的扰动不可控，也很难建模
③ 测量传感器存在误差

例子： 找不同的人测量一个硬币的直径（测量的人不同，尺子存在误差）

测量结果：$z_k$表示第k次的测量结果。

此时，如果我们要去估计真实数据 → 自然想到取平均值

数学表达如下：
$${\hat{x}}_k=\frac{1}{k}\left(z_1+z_2+\cdots +z_k\right)，{\hat{x}}_k表示第k次的估计值。$$

将取平均推导成递归表达式:
x ^ k = 1 k ( z 1 + z 2 + ⋯ + z k − 1 ) + 1 k z k = 1 k ⋅ k − 1 k − 1 ( z 1 + z 2 + ⋯ + z k − 1 ) + 1 k z k = k − 1 k ⋅ 1 k − 1 ( z 1 + z 2 + ⋯ + z k − 1 ) + 1 k z k = k − 1 k ⋅ x ^ k − 1 + 1 k z k = x ^ k − 1 − 1 k x ^ k − 1 + 1 k z k

x^k​​=k1​(z1​+z2​+⋯+zk−1​)+k1​zk​=k1​⋅k−1k−1​(z1​+z2​+⋯+zk−1​)+k1​zk​=kk−1​⋅k−11​(z1​+z2​+⋯+zk−1​)+k1​zk​=kk−1​⋅x^k−1​+k1​zk​=x^k−1​−k1​x^k−1​+k1​zk​​

整理可得：
$${\hat{x}}_k={\hat{x}}_{k-1}+\frac{1}{k}\left(z_k-{\hat{x}}_{k-1}\right)$$

当k↑，$\frac{1}{k}\to 0$，${\hat{x}}_k\to {\hat{x}}_{k-1}$ ，随着k增加，测量的结果$z_k$就不重要了。

理解: 当你有了大量的数据之后，测量了很多次
对估计的结果很有信心了，所以以后的测量值就变得不那么重要了

相反，当k小，$\frac{1}{k}$大，$z_k$的作用较大。

当k↑， ， ，随着k增加，测量的结果 就不重要了
理解: 当你有了大量的数据之后，测量了很多次
对估计的结果很有信心了，所以以后的测量值就变得不那么重要了
相反，当k小， 大， 的作用较大。

我们用$K_k$来代替$\frac{1}{k}$，就可以得到
$${\hat{x}}_k={\hat{x}}_{k-1}+K_k\left(z_k-{\hat{x}}_{k-1}\right)$$

当前的估计值 = 上一次的估计值 + 系数 × （当前的测量值 – 上一次的估计值）

在Kalman滤波器里面这个$K_k$叫作： Kalman Gain 卡尔曼增益/因数。

观察公式 ${\hat{x}}_k={\hat{x}}_{k-1}+K_k\left(z_k-{\hat{x}}_{k-1}\right)$，估计值${\hat{x}}_k$和上一次的估计值${\hat{x}}_{k-1}$有关，这就是递归的思想。

---

下面对$K_k$做一个简单的讨论，做一个初步的认识。
引入两个概念。
估计误差，用$e_{EST}$表示，（$e$是error误差的缩写， $EST$是estimate估计的缩写)
测量误差，用$e_{MEA}$表示，（$MEA$是measurement测量的缩写）
$$K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{ME{A}_k}}$$

讨论： 在k时刻
① $e_{ES{T}_{k-1}}$ >> $e_{ME{A}_k}$ , $K_k\to 1$ ,${\hat{x}}_k={\hat{x}}_{k-1}+1\cdot \left(z_k-{\hat{x}}_{k-1}\right)=z_k$ 。当k-1的估计误差 远大于 k次的测量误差的时候，第k的估计就很趋近于测量值。
② $e_{ES{T}_{k-1}}$ << $e_{ME{A}_k}$, $K_k\to 0$ , ${\hat{x}}_k={\hat{x}}_{k-1}+0\cdot \left(z_k-{\hat{x}}_{k-1}\right)={\hat{x}}_{k-1}$。当测量误差很大的时候，我们去选择更多的相信估计值。

---

下面用一个例子来简单体会一下上面学到的知识。
Step1 计算Kalman Gain $$K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{ME{A}_k}}$$
Step2 计算估计值$${\hat{x}}_k={\hat{x}}_{k-1}+K_k\left(z_k-{\hat{x}}_{k-1}\right)$$
Step3 更新估计误差 ，$e_{ES{T}_k}=\left(1-K_k\right)e_{ES{T}_{k-1}}$，此部分会在后面详细推导。

有了这三步以后，开始可以应用了。
比如一个物体他实际的长度是$x=50mm$，我们第一次的估计值随便估计一个${\hat{x}}_0=40mm$,第一次的估计误差随便给一个数字$e_{ES{T}_0}=5mm$，假设我们的测量误差都是$e_{MEA}=3mm$。
下面给出Excel的计算结果

根据上面的内容，有没有体会到递归的味道呢？

2、数据融合

假设我们用两个称去称一个东西，得到了两个结果分别是

利用上小节学到的内容，我们进行如下估计：
$$\hat{z}=z_1+k\left(z_2-z_1\right)$$
$k$为kalman gain，$k\in \left[0,1\right]$,当$k=0$时，$\hat{z}=z_1$,我们更相信称1的结果，当$k=1$时，$\hat{z}=z_2$,我们更相信称2的结果。

下面来求解最优的$k$，使得估计值$\hat{z}$的标准差$\delta \hat{z}$最小，标准差最小，即方差$\mathrm{var}\left(\hat{z}\right)$最小（方差是标准差的平方）。
δ z ^ 2 = v a r ( z ^ ) = v a r [ z 1 + k ( z 2 − z 1 ) ] = v a r [ ( 1 − k ) z 1 + k z 2 ] , 两个称是相互独立的，互不影响 = ( 1 − k ) 2 v a r ( z 1 ) + k 2 v a r ( z 2 ) = ( 1 − k ) 2 δ 1 2 + k 2 δ 2 2

δz^2​​=var(z^)=var[z1​+k(z2​−z1​)]=var[(1−k)z1​+kz2​],两个称是相互独立的，互不影响=(1−k)2var(z1​)+k2var(z2​)=(1−k)2δ12​+k2δ22​​
要寻找一个合适的$k$，使得${\delta }_{\hat{z}}^2$最小，那么就对${\delta }_{\hat{z}}^2$求$k$的导数，并令其$=0$。

d δ z ^ 2 d k = 2 ⋅ ( − 1 ) ⋅ ( 1 − k ) δ 1 2 + 2 k δ 2 2 = 0 ⇒ k ( δ 1 2 + δ 2 2 ) = δ 1 2 ⇒ k = δ 1 2 δ 1 2 + δ 2 2

dkdδz^2​​​=2⋅(−1)⋅(1−k)δ12​+2kδ22​=0⇒k(δ12​+δ22​)=δ12​⇒k=δ12​+δ22​δ12​​​

把实际的参数代入可得：
$$k=\frac{{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}=\frac{2^2}{2^2+4^2}=0.2,\hat{z}=z_1+k\left(z_2-z_1\right)=30+0.2\left(32-30\right)=30.4g$$

另外，估计值的标准差也可以代入计算。
$${\delta }_{\hat{z}}^2={\left(1-k\right)}^2{\delta }_1^2+k^2{\delta }_2^2={\left(1-0.2\right)}^2{2}^2+0.2^2{4}^2=3.2\Rightarrow \delta \hat{z}=\sqrt[]{{\delta }_{\hat{z}}^2}=1.79g$$

下面分析一下上述的推导结果
$\delta \hat{z}<\delta z_1$ 且 $\delta \hat{z}<\delta z_2$，说明什么？ 说明利用两个不太准确的称的测量值对真实值进行估计计算，能够得到一个比两个称都更准确的结果。这个过程，就叫做数据融合。

夹带点私货： 那么如果我有三个称呢？ 我先两个称融合，用结果再和第三称再融合。
在卡尔曼滤波的时候，是用模型估计 和 测量估计进行数据融合， 如果我的系统存在多个同类型传感器可以得到测量值呢，那是否可以先多个传感器先进行数据融合，然后再进行 模型估计和测量估计的数据融合呢？ 如果我的系统存在多个同类型传感器和多个不同类型的传感器可得到与状态向量相关的测量值呢？ 那这时候应该如何利用这些去得到一个更准确的状态估计呢？ 这是后续值得进一步学习的方向。

感谢您的阅读，欢迎留言讨论、收藏、点赞、分享。