高翔【自动驾驶与机器人中的SLAM技术】学习笔记（八）卡尔曼滤波器四：一文理清卡尔曼滤波，从传感器数据融合开始谈起【转载】

卡尔曼滤波器三：熟悉

引用来源：一文理清卡尔曼滤波，从传感器数据融合开始谈起

1 从传感器的测量谈起

在正式讨论卡尔曼滤波前，我们先讨论对物理量的测量。我们会发现是和卡尔曼滤波紧密相关的。 我们知道，如果需要对自然界的某个物理量，比如温度，气压，速度等进行测量，我们需要用各种传感器进行测量。但是，因为器件的工艺不可能达到完美，或者其他不能被人为预测到或者控制到的因素和噪声等存在，

传感器对物理量的预测不可能是完全准确的。因此，我们与其把传感器的测量结果当成是一个确定值，不如把它看成是一个随机变量 v ，

其均值和方差分别为$\mu ,{\sigma }^2$，既是$v\sim P(\mu ,{\sigma }^2)$，，这两个统计参数描述了测量的输出值（也就是我们直接观察到的值）和对这个测量的可信程度。同时，我们要注意到，这里的$\mu ,{\sigma }^2$不一定是时间平稳的，也就是说可能随着时间的变化而变化。 （暂且假设传感器的测量均值是和真实值无偏的。）

如下图所示，如果直接观察传感器数据，那么其可能是会存在很大的抖动，而不是平滑的，原因可能是观察噪声的影响。

Fig 1. 传感器的数据直接观测结果。

我们这个时候就想到，如果一次观察是抖动的，有着${\sigma }^2$​的不确定的，那么如果用同一个传感器对这个物理量观察N次，然后对N次数据进行求和，以减少不确定性的影响，岂不妙哉？ 这样的确是可以的，这个就是信号处理当中的滑动窗口均值滤波(mean filter)。但这个简单操作有几个缺点：

1. 我们前面谈到了不确定度${\sigma }^2$是可能时变的，简单相加不能最好地消除不确定性。
2. 时间上滑动窗口进行多次测量的求和，会导致延迟。

对此，我们进行一个小改进，就是用多个相同的传感器去同时测量一个物理量，然后求和或者根据可靠程度去求加权平均和，我们假设多个传感器的采样值$x_i$，满足分布，其中i表示传感器序号：
$$x_i\sim P(\mu ,{\sigma }^2)\text{\hspace{1em}(1)}$$
我们发现，其因为假设是无偏测量传感器因此均值相同，但是每个传感器的不确定性不一定相同。

这个时候简单的求和就容易造成结果的偏移，我们不妨根据方差的大小，进行加权平均求和，在此之前，我们需要几个假设：

1. 不同传感器的测量都是一个随机变量，其均值$\mu$相同。

2. 不同传感器的测量之间是无关的，也就是说知道了$x_i$不能对知道其他策略$x_j$提供任何信息，但是也不会影响到观测$x_i$的均值，即是$E(x_j\mid x_i)=E(x_j)$

接下来，我们用这两个假设，进行简单的传感器间的数据融合以提高测量效果。Let’s move on！

2 简单版本，多传感器数据融合

为了简单起见，假设我们用两个相同的传感器进行测量，那么最后数据融合结果应该是：
$$\begin{gathered} \hat{x}=\alpha x_1+\beta x_2 \\ \alpha +\beta =1 \\ \Rightarrow \hat{x}=\alpha x_1+(1-\alpha )x_2\text{\hspace{1em}(2)} \end{gathered}$$
那么，融合后的估计$\hat{x}$​的不确定度可以通过$x_1,x_2$的方差进行衡量，公式如:
$${\hat{\sigma }}^2(\alpha )=(1-\alpha {)}^2{\sigma }_1^2+\alpha {\sigma }_2^2\text{\hspace{1em}(3)}$$
为了最小化 ${\hat{\sigma }}^2(\alpha )$，，我们用求导并且置为0的方法[3]，不难推导出当$\alpha =\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$时，式子(3)有最小值，此时，式子(2)可化为:
$$\hat{x}(x_1,x_2)=\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}{x}_1+\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{x}_2\text{\hspace{1em}(4)}$$
这里讨论的只是两个传感器的情况，可以简单地推导到多个传感器的情况和当观测值是一个向量时候的情况，以及为了计算有效性，采用迭代计算的方法，具体可以参考文献[3]。其中，为了以后讨论的方便，这里给出当观测值是一个向量，并且只有两个传感器时的公式(5)：
$$\begin{array}{rl}& {\mathbf{x}}_1\sim p_1({\mu }_1,{\Sigma }_1),{\mathbf{x}}_2\sim p_2({\mu }_2,{\Sigma }_2)\\ & K\\ & \\ & \end{array}$$