多源融合之滤波对比
最近在看CSDN滤波方面的知识，但是很多都是激光雷达、视觉、信号处理等方面的大佬们总结的，我也做过卡尔曼滤波和粒子滤波。博主大地测量学与测量工程专业，主要做定位方面的内容，现在从我的学习过程来进行总结，也梳理一下相关思路。
融合的必要性
目前，定位技术众多，室外有GNSS，室内有UWB、WiFi、蓝牙等，桥接的还有惯性传感器，但是单一定位源无法满足高精度、高可靠的需要，更不要说在某些情况下面临着建筑物遮蔽、电磁信号异常等拒止环境导致GNSS、UWB等信号不可用。比如说室内环境中，PDR存在漂移，WiFi接收信号频率较低，地磁定位误匹配等等。按照测绘工程专业角度来说，只要是测量就存在误差，因此，需要利用多源传感器来保障定位精度及其可靠性，目前的融合手段包括图优化、滤波等手段，卡尔曼滤波自从20世纪60年代提出来一致沿用到现在，并且也进行了很多的改进。
总的来说，滤波融合的流程是根据前一历元的状态（ x ^ k − 1 \hat x_{k-1} x^k−1）及其协方差矩阵（ P ^ k − 1 \hat P_{k-1} P^k−1）预测当前时刻的状态（ x k x_{k} xk）及其协方差矩阵（ P k P_{k} Pk），利用其他传感器观测值来校正估计当前时刻的状态（ x ^ k \hat x_{k} x^k）及其协方差矩阵（ P ^ k \hat P_{k} P^k）。
规定说明
先验 状态更新得到的状态及其协方差矩阵
后验 量测更新得到的状态及其协方差矩阵
卡尔曼滤波
卡尔曼滤波过程假设动力学方程均为线性方程，满足“最小方差和最小”（和最小二乘一样）。本文将在过程噪声和量测噪声均为高斯白噪声的情况下推导卡尔曼滤波方程。
在利用滤波描述载体运动过程中，假设该系统为线性，分为状态方程和量测方程，分别为：
x k = A k x ^ k − 1 + w k x_k=A_k\hat x_{k-1}+w_k xk=Akx^k−1+wk
z k = H k x k + v k z_k=H_kx_k+v_k zk=Hkxk+vk
其中，
A
k
A_k
Ak和
H
k
H_k
Hk分别为k时刻状态转移矩阵和系数矩阵，
w
k
w_k
wk和
v
k
v_k
vk为过程噪声和量测噪声。
卡尔曼滤波状态更新过程（预测过程）：
x k = A k x ^ k − 1 x_k=A_k\hat x_{k-1} xk=Akx^k−1;
P k = A k P ^ k − 1 A k T + Q k P_k=A_k\hat P_{k-1}A_k^T+Q_k Pk=AkP^k−1AkT+Qk
其中，
Q
k
Q_k
Qk是过程噪声的协方差矩阵。
卡尔曼滤波量测更新过程（融合过程）：
K k = P k H k T ( H k P k H k T + R k ) − 1 K_k=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} Kk=PkHkT(HkPkHkT+Rk)−1
x ^ k = x k + K k ( Z k − H k x k ) \hat x_k=x_k+K_k(Z_k-H_kx_k) x^k=xk+Kk(Zk−Hkxk)
P ^ k = ( I − K k H k ) P k \hat P_k=(I-K_kH_k)P_k P^k=(I−KkHk)Pk
其中， R k R_k Rk为观测噪声协方差矩阵， K k K_k Kk又叫做滤波增益矩阵，通过量测更新过程第2和3公式可以发现，在预测 x k x_k xk和 P k P_k Pk之后，利用观测值对状态更新结果进行了修正，简而言之，就是将状态更新后的状态量向观测方程中的最优状态靠拢。
以GNSS/INS举例来阐述卡尔曼滤波过程。状态更新过程多为INS编排过程，矩阵 Q k Q_k Qk需要根据惯导器件参数确定，状态更新过程方程建立可以参考严恭敏老师的书《捷联惯导算法与组合导航原理》或是牛小骥老师和陈起金老师b站的研究生课程。观测方程即为GNSS定位原理，具体方程可以参考王坚老师《卫星定位原理与应用》，矩阵 R k R_k Rk就是卫星定位过程中的随机矩阵。
相比于卡尔曼滤波，最小二乘缺少了状态更新过程，进而在量测更新过程中也被简化。也就是说，如果在卡尔曼滤波过程中更信任量测，进而导致状态更新权重为或接近0，那么卡尔曼滤波也就接近最小二乘（此处最小二乘并不是指图优化）。
前述中，卡尔曼滤波基于白噪声、线性方程进行优化，但是工程应用中主要为非线性情况，GNSS定位中也是用到了级数展开求系数矩阵 H k H_k Hk。为了应对非线性情况，学者们提出了扩展卡尔曼滤波、无迹卡尔曼滤波等等，有色噪声也有学者进行研究，博主对这方面涉猎较少，不再扩展。
粒子滤波
粒子滤波是基于蒙特卡洛方法的贝叶斯滤波。蒙特卡洛方法是统计模拟方法（大量数据，利用频率逼近概率），贝叶斯滤波在贝叶斯法则（概率）基础上融合状态预测过程和量测更新过程（概率的融合），即最大后验概率。因此，粒子滤波非常适用于非线性较强情况下的状态估计。
假设有一个非线性系统，服从一阶马尔可夫模型，一阶马尔可夫模型指当前状态只与前一时刻状态有关，并假设当前观测值仅与当前时刻的装填有关，则该非线性状态方程和量测方程为：
x k = f k ( x k − 1 , w k ) x_k=f_k(x_{k-1},w_k) xk=fk(xk−1,wk)
z k = h k ( x k , v k ) z_k=h_k(x_k,v_k) zk=hk(xk,vk)
此时，不同于卡尔曼滤波上一时刻状态为
p
(
x
k
−
1
∣
z
1
:
k
−
1
)
p(x_{k-1}|z_{1:k-1})
p(xk−1∣z1:k−1)
状态更新方程
x k = f k ( x k − 1 ) x_k=f_k(x_{k-1}) xk=fk(xk−1)
p ( x k ∣ z 1 : k − 1 ) = p ( x k , x 1 : k − 1 ∣ z 1 : k − 1 ) = p ( x k ∣ x 1 : k − 1 , z 1 : k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) = p ( x k ∣ x k − 1 , z 1 : k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) \begin{aligned} p(x_k|z_{1:k-1}) &=p(x_k,x_{1:k-1}|z_{1:k-1}) \\ &=p(x_k|x_{1:k-1},z_{1:k-1})p(x_{k-1}|z_{1:k-1}) \\ &=p(x_k|x_{k-1},z_{1:k-1})p(x_{k-1}|z_{1:k-1}) \\ \end{aligned} p(xk∣z1:k−1)=p(xk,x1:k−1∣z1:k−1)=p(xk∣x1:k−1,z1:k−1)p(xk−1∣z1:k−1)=p(xk∣xk−1,z1:k−1)p(xk−1∣z1:k−1)
可以看出，协方差矩阵变成了求概率，在量测更新过程中，协方差矩阵层面的融合也将变成概率层面的融合。
量测更新方程
p ( x k ∣ z 1 : k ) = p ( z k ∣ x k , z 1 : k − 1 ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) \begin{aligned} p(x_{k}|z_{1:k})&=\cfrac {p(z_k|x_k,z_{1:k-1})p(x_k|z_{1:k-1})} {p(z_k|z_{1:k-1})}\\ &=\cfrac {p(z_k|x_k)p(x_k|z_{1:k-1})} {p(z_k|z_{1:k-1})}\\ \end{aligned} p(xk∣z1:k)=p(zk∣z1:k−1)p(zk∣xk,z1:k−1)p(xk∣z1:k−1)=p(zk∣z1:k−1)p(zk∣xk)p(xk∣z1:k−1)
其中， p ( z k ∣ z 1 : k − 1 ) p(z_k|z_{1:k-1}) p(zk∣z1:k−1)可以通过全概率公式得到
p ( z k ∣ z 1 : k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) p(z_k|z_{1:k-1})=p(z_k|x_k)p(x_k|y_{1:k-1}) p(zk∣z1:k−1)=p(zk∣xk)p(xk∣y1:k−1)
p
(
z
k
∣
z
1
:
k
−
1
)
p(z_k|z_{1:k-1})
p(zk∣z1:k−1)也叫做归一化常数。
对比于卡尔曼滤波中的量测更新，粒子滤波量测更新比较简洁，只根据当前时刻观测值与状态对先验状态更新权重为
p
(
x
k
∣
z
1
:
k
)
p(x_{k}|z_{1:k})
p(xk∣z1:k)。粒子滤波在于用大量粒子逼近真实状态（蒙特卡洛），因此，上述状态更新以及量测更新仅为1个粒子的k时刻粒子滤波递推过程，要想得到最终后验估计，需要对所有粒子的位置进行平均。
接下来以PDR/地磁序列融合为例描述粒子滤波递推过程。
1）PDR开始前，在给定位置附近区域分布粒子N个，权重均为
1
N
\cfrac 1N
N1；
2）PDR开始后，对N个已知位置粒子采用PDR进行位置更新；
3）利用粒子所在位置的指纹库地磁信息与实测地磁信息对比更新权重；
4）随步态递推步骤2）-3）。
卡尔曼滤波与粒子滤波关系
滤波的目的是估计状态的一阶矩（均值）和二阶矩（协方差矩阵/权重）。二者都采用序贯递推的方式进行状态估计，并且都有状态更新和量测更新两个阶段。
二者的本质区别在于，卡尔曼滤波依照“方差和最小”，粒子滤波遵循“最大后验概率”。其次，卡尔曼滤波主要目的在于解决高斯白噪声和线性问题，粒子滤波可以解决非线性问题。在多源融合定位领域中，卡尔曼滤波多用于融合GNSS/INS、WiFi-RTT/INS等，融合的绝对定位方法多为几何定位方法，如BLE的角度后方交会，GNSS、WiFi的距离后方交会。粒子滤波则多用于随机性较强的传感器融合，如PDR/指纹库定位，指纹库随机性较强，采用频率逼近概率进行最大后验估计。上述应用均为车载或行人导航定位。
卡尔曼滤波是一种确定性方法，粒子滤波是一种随机性方法，但是二者并不是完全对立的。
粒子滤波由于其随机性，在实际工程中有时会因为粒子滤波过于随机导致每次估计结果不一致，甚至会因为粒子退化和贫化问题导致滤波发散。有的学者利用卡尔曼滤波（扩展卡尔曼滤波、无迹卡尔曼滤波）估计后验协方差矩阵后，采用SVD分解、LU分解等方法将协方差矩阵转化为误差椭圆，在误差椭圆内分布粒子来限制粒子滤波的随机性。
参考文献
1、秦永元. 《卡尔曼滤波与组合导航原理》. 西北工业大学出版社。
2、严恭敏. 《捷联惯导算法与组合导航原理》. 西北工业大学出版社。
3、王坚. 《卫星定位原理与应用》. 测绘出版社。
4、黄小平. 《粒子滤波原理及应用》. 电子工业出版社。
5、牛小骥，陈起金. B站录播课程. https://www.bilibili.com/video/BV1na411Z7rQ/?spm_id_from=333.337.search-card.all.click.
6、《Magnetic-Based Indoor Localization Using Smartphone via a Fusion Algorithm》.
1478
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
