Kalman滤波一

本笔记将记录大名鼎鼎的 Kalman Filter.
卡尔曼滤波器设计的目的：我们希望尽量的从具有不确定性的信息中获取尽可能多的有用信息。

首先需要是明确的是，对于滤波器这个我们如何理解？？----真的就是简单的求平均值的吗？
卡尔曼滤波器：是一个最优化的递归数据处理算法，而并不是简单的滤波器，不要给滤波器这个词误导了啊！！

直观理解

• 卡尔曼滤波出现的原因：现实生活中有太多不确定的因素了，而不确定性主要体现在
  1. 我们无法建立一个完美的数学模型
  2. 传感器的测量结果存在偏差
  3. 现实中存在着一些无法抗拒和测量的扰动

举个例子：假设我们用直尺测量一个物体，每次测量的结果为$Z_i$，第$k$次测量后，计估计值我们记为${\hat{X}}_k$
则我们由平均值可以得到：
$$\begin{array}{rl}{\hat{X}}_k& =\frac{1}{k}(Z_1+Z_2+...+Z_k)\\ & =\frac{1}{k}\frac{k-1}{k-1}(Z_1+Z_2+...+Z_{k-1})+\frac{1}{k}{Z}_k\end{array}$$
接着因为我们知道，${\hat{X}}_{k-1}=\frac{1}{k-1}(Z_1+Z_2+...+Z_{k-1})$
所以，我们得到：
$${\hat{X}}_k={\hat{X}}_{k-1}+\frac{1}{k}(Z_k-{\hat{X}}_{k-1})$$

可以看出这个式子同时包含了$X_k$和$X_{k-1}$,这也就很好说明了卡尔曼滤波器的递归性质；
在卡尔曼滤波器中，我们将上式的系数$\frac{1}{k}$记为：$K_k$，即卡尔曼增益
通过这个系数我们可以看出：

• 若系数$K_k\to 0$，则我们的估计值将于我们所测量的结果无关，我们越来越相信我们的计算结果(这可能也就是我们测量了很多次的原因吧！)
• 若系数$K_k\to 1$, 则我们的估计值就是我们所测量的值！

在这里我们首先提一下这个卡尔曼系数的计算公式(详细的推导见后面部分)
$$K_k=\frac{e_{es{t}_{k-1}}}{e_{es{t}_{k-1}}+e_{me{a}_k}}$$
其中：$e_{es{t}_{k-1}}$为估计值与真实值的误差；$e_{me{a}_k}$为测量值与真实值的误差
(疑问：可是我们的真实值不知道啊啊啊！！)

我们再讲解一下，在解决实际问题的时候应用卡尔曼滤波器的步骤：

• Step1:计算卡尔曼增益$K_k$：$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}+\frac{1}{k}(Z_k-{\hat{X}}_{k-1})$
• Step3:更新估计误差$e_{es{t}_k}=(1-K_k)e_{es{t}_{k-1}}$(这个公式我们后面也会详细的推导的)

需要注意一下，我们在这里首先要确定是卡尔曼增益，但是在计算的过程中我们很容易发现，我们还缺一个参数(测量误差哪里来的)，特别是第一个的测量误差(因为在后面的可以根据我们所更新的公式来进行推导的)，是我们随意估计的吗？(的确是的，因为我们的估计误差后面会不断的跟新，所以第一次可以随意给的，但是好像也不能太随意吧，啊啊我也不太清楚，接着往后学着看吧)

下面这张图是某一次假象的应用卡尔曼滤波算法的测量过程

数学基础

在这一部分我将大致梳理一下，卡尔曼滤波器所涉及到的数学概念

• 数据融合
  利用多个传感器测量同一个被测物体，然后融合这些测量值和对应的信息，实现比单个的传感器更准确的判断！

  (数据融合是一个大词，让人看了不明觉厉。其实就是利用统计信息，让数据之间相互校准。)

举个例子：假设我们用两台称去称量同一个物体，假设称A测量的结果为$Z_1=30g$，方差${\sigma }_1=2g$;称B的测量结果为$Z_2=32g$，方差${\sigma }_2=4g$，并且假设都满足正态分布，那么问题是我们如何根据这两个数据得出更加准确的结果？？
我们假设直接引用我们的卡尔曼滤波的思想？假设我们得到的估计值为$\hat{Z}=Z_1+K(Z_2-Z_1)$，其中$K$就是卡尔曼增益，接下来的问题就是这个参数取何值的时候，我们能够得到更加准确的估计值也(也就是说，如何将该估计值的方差最小)
由方差的性质可以得到：
$$\begin{array}{rl}{\hat{\sigma }}^2& =var(Z_1+K(Z_2-Z_1))\\ & =var((1-K)Z_1)+var(K{Z}_2)\\ & =(1-K{)}^2{\sigma }_1^2+K^2{\sigma }_2^2\end{array}$$
所以我们呢可以对参数$K$进行求导，求极小值，进而得到该问题的解。最终我们得到结果肯定是介于两者之间的，而方差比两者都小(也可以从正态分布的图像可以看出来的)

• 协方差矩阵
  下面我们将回顾一下概率中的几个概念：
  • 方差：用来描述数据偏离整体数据均值的程度的，$E\{(X-EX{)}^2\}$
  • 协方差：这个时候就不是一组数据了，而是需要描述两组数据之间的关系了，$E\{(X-EX)(Y-EY)\}$
    - 协方差矩阵：将上述的方差和协方差组成一个矩阵
    $$P=\left(\begin{array}{cc}{\sigma }_x^2& {\sigma }_{xy}^2\\ {\sigma }_{xy}^2& {\sigma }_y^2\end{array}\right)$$
• 状态方程
  • 概念：由状态变量所张成的空间，就叫做状态空间，而状态方程是由这些状态变量的一阶微分方程组来描述这个系统。
• 观测器
  • 这里就涉及到了状态观测的概念了，这些都是现代控制理论中所学的啊啊！！
  • 在实际的控制过程中，我们所采用的状态变量有时候并不能直接测得的，或者说直接测得的误差将会很大！因此我们不得不将状态进行重构，来间接的获得我们的状态变量！

接着我们来描述一下这些式子：
$$\begin{array}{rl}\dot{X\text{X}X}& =AX\text{X}X+Bu\text{u}u\\ Y\text{Y}Y& =CX\text{X}X+Du\text{u}u\end{array}$$
其中，$X=[x_1,x_2,x_3,...,x_n{]}^T$为状态矢量
将其离散化后：
$$\begin{array}{rl}{X\text{X}X}_k& =A{X\text{X}X}_{k-1}+B{u\text{u}u}_{k-1}\\ {Y\text{Y}Y}_k& =C{X\text{X}X}_k+D{u\text{u}u}_k\end{array}$$
当然了，离散化后，上面两个式子中的$A$就不是同一个矩阵了！！(而且没有$\dot{X}$)了！
然而正如我们之前所说，现实生活中充满了不确定的因素，因此我们将得到
$$\begin{array}{rl}{X\text{X}X}_k& =A{X\text{X}X}_{k-1}+B{u\text{u}u}_{k-1}+{\omega \text{ω}\omega }_{k-1}\\ {Y\text{Y}Y}_k& =C{X\text{X}X}_k+D{u\text{u}u}_k+{\mu \text{μ}\mu }_k\end{array}$$
其中，$\omega \text{ω}\omega$为过程噪声和$\mu \text{μ}\mu$为测量噪声，假设服从无偏的(期望为0)高斯分布(正态分布)
到现在，假如我们直接测量这个状态变量：
$$\begin{array}{rl}Z\text{Z}Z& =HX\text{X}X\\ & =\left[\begin{array}{cccc}1& 0& \cdots & 0\\ 0& 1& \ddots & 0\\ ⋮& \ddots & \ddots & 0\\ 0& \cdots & 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]\end{array}$$
类比上面的离散化的过程，我们得到了测量值 ${Z\text{Z}Z}_k=H_k{X\text{X}X}_k+{\mu \text{μ}\mu }_k$

综合上述的式子，我们可以得到以下两个式子：
$$\begin{array}{rl}{X\text{X}X}_k& =A{X\text{X}X}_{k-1}+B{u\text{u}u}_{k-1}+{\omega \text{ω}\omega }_{k-1}\\ {Z\text{Z}Z}_k& =H{X\text{X}X}_k+{\mu \text{μ}\mu }_k\end{array}$$

其中${X\text{X}X}_k$为我们的计算值，而${Z\text{Z}Z}_k$为测量值
现在我们类比上面那个称物体重量的例子，在对比一下这两个式子，我们是不是发现了什么？？
如果我们可以用卡尔曼滤波融合这个两个式子我们是不是就可以得到更加准确的估计值$\widehat{X\text{X}X}$

参考资料

卡尔曼滤波算法

补充资料

卡尔曼滤波算法与最小二乘