卡尔曼滤波（Kalman Filtering）——（3）数据融合 状态空间方程

一、数据融合

假设举例

  假设测量一物体的质量，现在有两个测量设备但是都存在误差且误差服从正态分布，分别用两个设备进行测量第一设备测量的测量值为$z_1$ ，标准差为${\sigma }_1$；第二设备测量的测量值为$z_2$ ，标准差为${\sigma }_2$ 。现在有两个测量值那么应该更愿意去相信那个设备测出的值呢？或者把两次数据进行处理导出一个更接近真实值的值呢？
  如果已知：$z_1=30g；{\sigma }_1=2g$
       $z_2=32g；{\sigma }_2=4g$
  现在让估算真实值，该如何计算呢？
分析：显而易见最终所估算的真实值也是服从正态分布的，且其结果在$z_1$$z_2$之间，由于标准差${\sigma }_1比{\sigma }_2$小则更靠近$z_1$。

公式推导过程

  通过上一博客可知：当前的估计值 = 上一次的估计值 + 系数 (当前测量值 - 上一次的估计值)Kalman Filtering通过调整观测值和估计值的权重，使修正后的值更趋向于相信观测值还是相信估计值的一个过程。当我们使用同一设备进行测量时估计值通常是进行数学建模计算得出，由于有的误差无法建模所以得出的值只能是估计值，如第一篇博客所举的例子中当前估计值与上一次估计值分别是第$K$次取平均值和第$K-1$次取平均值，而当前测量值为第$K$次测量结果。从这我们也可以看出数据融合的思想，数学建模值与测量值之间融合得到最接近真实值的结果。但是本博客举得例子是两个设备测量结果没有进行建模，应该如何使用Kalman Filtering？其实同理只需要将一个值作为数学建模的结果即估计值，另一个作为测量结果即可。所以Kalman Filtering的数据融合是两个值之间的融合，不论值是建模得来还是传感器测得都行。
  现在要得出一个最优估计值$\hat{z}$，由以上面推导的公式可知：（将$z_1$当作估计值；$z_2$当作测量值）
$$\hat{z}=z_1+K\left(z_2-z_1\right)\text{\hspace{1em}(1)}$$
式中 $:K⟶$ 卡尔曼增益/因数, 且 $K\in [0,1]$ 。
   $K=0$ $\Rightarrow$ $\hat{z}=z_1$
   $K=1$ $\Rightarrow$$\hat{z}=z_2$
现在目标是求 $K$ 使得 ${\sigma }_{\hat{z}}$ 最小$\Rightarrow$ 求 $K$ 使得方差 $\mathrm{Var}(\hat{z})$ 最小$\Rightarrow$ 求$K$使误差最小。
则：
$$\begin{array}{rl}{\sigma }_{\hat{z}}^2& =\mathbf{V}\mathrm{ar}\left(z_1+K\left(z_2-z_1\right)\right)\\ & =\mathbf{V}\mathrm{ar}\left(z_1-K{z}_1+K{z}_2\right)\\ & =\mathbf{V}\mathrm{ar}\left((1-K)z_1+K{z}_2\right)⟶两个设备测量结果与误差，相互独立，互不影响\\ & =\mathrm{Var}\left((1-K)z_1\right)+\mathrm{Var}\left(K{z}_2\right)⟶K为常数，从方差中提出得带平方\\ & =(1-K{)}^2\mathrm{Var}\left(z_1\right)+K^2\mathrm{Var}\left(z_2\right)\\ & =(1-K{)}^2{\sigma }_1^2+K^2{\sigma }_2^2\end{array}$$

求方差最小值时，则方差 ${\sigma }_{\hat{ı}}^2$ 对 $K$ 的导数为零，即：
$$\begin{array}{rl}\frac{\partial {\sigma }_{\hat{z}}^2}{\partial K}=0& \Rightarrow -2(1-K){\sigma }_1^2+2K{\sigma }_2^2=0\\ & \Rightarrow -{\sigma }_1^2+K{\sigma }_1^2+K{\sigma }_2^2=0\\ & \Rightarrow K\left({\sigma }_1^2+{\sigma }_2^2\right)={\sigma }_1^2\\ & \Rightarrow K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}\end{array}$$
可以看出K的取值与进行数据融合的两个值的误差大小有关：
   当${\sigma }_2>>{\sigma }_1$时 $K=0$ $\Rightarrow$ $\hat{z}=z_1$
   当${\sigma }_2<<{\sigma }_1$时 $K=1$ $\Rightarrow$$\hat{z}=z_2$
${\sigma }_1=2g;{\sigma }_2=4g$带入数据得：$K=0.2$带入（1）式中得$\hat{z}=30.4g$
      ${\sigma }_{\hat{z}}^2=3.2\Rightarrow {\sigma }_{\hat{z}}=1.79<min[{\sigma }_1^2,{\sigma }_2^2]$
上面所进行的过程就是数据融合的过程，由此可见将两个设备所测得的数据进行融合所得到的误差更小更加接近真实值。卡尔曼滤波利用信息融合减少误差从而使估计值更加接近真实值也就是寻找最优估计得过程。

再次理解

  现在可以看出卡尔曼滤波信息融合的关键就是要求卡尔曼增益$K$，这个就是一个权值使当前估计值趋近于误差小的估计值。上面通过严谨的数学公式推导得出了估计值$\hat{z}=30.4g$，下面用小学二年级的知识在来理解理解：
  已知：$z_1=30g；{\sigma }_1=2g$
     $z_2=32g；{\sigma }_2=4g$
解：
    加权平均值
$$\begin{array}{rl}\hat{z}& =z_1\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}+z_2\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}\\ & =30\times \frac{4^2}{2^2+4^2}+32\times \frac{2^2}{2^2+4^2}\\ & =30.4\end{array}$$
  两个设备测量的结果不相关，相互独立，则两个的加权平均值就为$\hat{z}$。其中误差小的测量结果乘以权重大的；反之误差大的测量结果乘以权重小的；很好理解因为我们是比较相信误差小的结果的。
  由于两个测量误差不相关则：（可以理解为并联电阻）
$$\begin{array}{rl}\frac{1}{{\sigma }_{\hat{z}}^2}& =\frac{1}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}\\ & =\frac{1}{2^2}+\frac{1}{4^2}\\ & =0.3125\\ \Rightarrow {\sigma }_{\hat{z}}^2& =3.2\Rightarrow {\sigma }_{\hat{z}}=1.79\end{array}$$
如果误差相关误差相关则为：${\sigma }_{\hat{z}}^2={\sigma }_1^2+{\sigma }_2^2$(串联电阻)
  一般情况下：串联误差为误差相关；
           并联误差为误差不相关。

二、状态空间方程

  系统会有一系列的状态，这个状态你可以任意的去选择，只要你觉得能够充分表达你所需要的信息。系统状态会发生变化，那么这个变化应当和什么有关呢？

1. 系统当前的状态；
2. 系统的输入。

阻尼滑块模型

  一个弹簧滑块系统，滑块的加速度（受到的外力）和系统当前的位置有关（弹簧弹力），也和外力输入有关（人手或电机给与滑块一个外力）。将输入记为u。系统状态的变化，对于连续系统而言，是系统的导数，对于离散系统而言，是系统状态的变化量，一般取为系统下一时刻的状态(${\mathbf{x}}_{k+1}$=$\mathbf{A}{\mathbf{x}}_k+B{u}_k$)，二者包含的信息一致。

系统的输出和什么有关呢？当然是和系统当前状态和输入有关。

1、连续表达式

该系统动态方程表达式为：
$$m\ddot{x}+c\dot{x}+kx=F$$
令 $x_1=x,x_2=\dot{x},u=F$, 有
$$\begin{array}{c}{\dot{x}}_1=x_2\\ {\dot{x}}_2=\ddot{x}=\frac{1}{m}u-\frac{c}{m}\dot{x}-\frac{k}{m}x\\ =\frac{1}{m}u-\frac{c}{m}{x}_2-\frac{k}{m}{x}_1\end{array}$$
假设位置测量量为 $z_1=x=x_1$, 速度测量量为 $z_2=\dot{x}=x_2$,将上式用矩阵形式表达则有：
$$\begin{array}{r}\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{c}{m}\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u\end{array}$$
$$\begin{array}{r}\left[\begin{array}{l}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{ll}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\end{array}\right]\end{array}$$
上述表达形式即状态空间表达形式，上述表达式可分别归纳为随时间变化表达式：
$$\begin{array}{c}\dot{\mathbf{x}}(t)=A\mathbf{x}(t)+Bu(t)\\ \mathbf{z}(t)=H\mathbf{x}(t)\end{array}$$

2、离散表达形式

$$\begin{array}{c}{\mathbf{x}}_k=\mathbf{A}{\mathbf{x}}_{k-1}+B{u}_{k-1}\\ {\mathbf{z}}_k=H{\mathbf{x}}_k\end{array}$$
其中，下标 $k、k-1、k+1$ 表示采样时间。即系统当前状态与系统上一时刻状态和上一时刻输入有关。

  由于所有的系统数学模型往往是理想模型，存在许多假设，与实际模型存在差距。比如上述阻尼滑块模型表达式中就忽略了阻力的存在即系统存在噪声且噪声的表达式未知，所以应当加入未知噪声时表达式为：
$$\begin{array}{c}{\mathbf{x}}_k=\mathbf{A}{\mathbf{x}}_{k-1}+B{u}_{k-1}+{\mathbf{w}}_{k-1}\\ {\mathbf{z}}_k=H{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}$$
其中：$w⟶$ 过程噪音
   $v⟶$ 测量噪音
卡尔曼滤波器的目的即从存在噪声的数据中求出误差最小的结果。

3、符号含义

符号	意义	备注
$x_k$	系统状态矩阵(真实值)	计算得到
$z_k$	状态阵的观测量	实际测量
$A$	状态转移矩阵	–
$B$	控制输入矩阵	–
$H$	状态观测矩阵	–
$w_{k-1}$	过程噪声	服从正态分布
$v_k$	测量噪声	服从正态分布

参考文献

  DR_CAN
  【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)