卡尔曼滤波（二）（最优化递归数字处理算法）

一、数据融合

先举一个例子：有两个秤，A秤测得某个物体重30g（记为Z1），它的标准差是2g（记为σ1）；B

秤测得该物体重32g（记为Z2），它的标准差是4g（记为σ2）。 那如果根据这两个测量值去估计

该物体的重量，则可以用最优化的思想。

定义：Z_Hat（估计值） = Z1 + K * （Z2 - Z1）

K = 0时，Z_Hat = Z1，而K = 1时，Z_Hat = Z2；K在0~1范围内

可以寻求Z_Hat的方差最小的时候，得到最优的估计值。Z_Hat的方差 = Z1 + K *（Z2 - Z1）这个

整体的方差，这个整体可化成（1 - K）* Z1 + K * Z2，而Z1跟Z2是由两个独立的秤测出来的，那

么这个整体的方差有等于各自的方差之和，也就是（1 - K）* Z1的方差 + K * Z2的方差。由于知道

了Z1和Z2的标准差，那个这个整体的方差，其实就是关于K的一个二次函数（注意，求K * Z2的方

差，应该等于K^2 * Z2的方差），寻求K值使得这个整体的方差最小，就能够得到最优的估计值，

最后求的测量值是30.4g，它的标准差是1.79g

这个过程就是数据融合 

二、协方差矩阵

将方差和协方差通过矩阵表现出来

（1）方差的回顾

方差是表示数据离散程度的量，也就是说方差等于平方的期望减去期望的平方。
根据方差与期望之间的关系：
D(X)=E{【X-E(x)】^2 }
=E{ X^2 - 2X * E（X）+【E(x)】^2 }
=E(x^2) - 2E(x) * E(x) +【E(x)】^2
=E(x^2) - E(x)^2
把E(x)看成是一个常数，因为E(X)是已经求得的数学期望 ，而常数的期望就是本身。

（2）协方差是什么

 协方差的通俗讲解：如何通俗地解释协方差_哔哩哔哩_bilibili

其中协方差是体现变量之间的联动关系或者说是相关性，比如有两个变量X，Y，如果他们有相同

的变化趋势，则称为正相关，此时协方差＞0；如果他们有相反的变化趋势，则成为负相关，此时

协方差＜0；如果他们不相关，此时协方差 = 0。

协方差的表示，可以理解为在二维的平面内，先求出所有样本的加权平均数，把这个点的坐标定为

原点处，其他样本与这个原点构成的长方体的面积再乘上各自出现的概率，这样会得到的一个值，

把所有得到的这个值累加，如果是大于0的，说明两个变量的关系是正相关；如果小于0，说明两个

变量的关系是负相关（可以这么理解：如果所有样本都出现在第三象限，那么这些样本到达原点的

方法，都是横坐标和纵坐标增加，所以两个变量的关系自然是正相关。其他象限也可以这么去理解

推导）

 改写成期望的形式如下

（3）协方差矩阵

详解协方差与协方差矩阵_ybdesire的博客-CSDN博客_协方差矩阵

[线性代数] 如何求协方差矩阵_kuang_liu的博客-CSDN博客_协方差矩阵求解

 三、状态空间

（1）连续型的状态空间方程表达

d X（t）= A1 * X（t）+ B1 * U（t）

（2）离散型的状态空间方程表达

X（k）= A2 * X（k-1）+ B2 * U（k）

（3）测量型（也是离散型）的状态空间方程表达

Z（k）= H * X（k）

（4）引入不确定性的状态空间方程表达

计算得到的状态变量：X（k）= A2 * X（k-1）+ B2 * U（k）+ W（k-1），引入过程噪音，这部分

是由于模型不够准确所导致的

测量得到的状态变量：Z（k）= H * X（k）+ V（k），引入测量噪音，这部分是由于自然界的干扰

和传感器的精度误差所导致的

我们更希望，测量得到的状态变量，能够尽可能少地包含测量噪音带来的那部分影响；计算得到的

状态变量，能够补充上模型不够准确而缺失的那部分影响

（5）通过卡尔曼滤波器，并将计算得到的和测量得到的状态变量进行数据融合，得到最优的状态

变量的估计值