学习笔记10--多传感器融合定位技术

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》。
此专栏是关于《自动驾驶汽车定位技术》书籍的笔记.

4.多传感器融合定位技术

4.1 多传感器融合定位简介

• 多传感器融合：将不同传感器对某一目标或环境特征描述的信息融合成统一的特征表达信息及其处理的过程；
• 多传感器数据融合实际上是模拟人脑综合处理复杂问题的过程，通过对各种传感器及其观测信息的合理支配与使用，将各种传感器在空间和时间上的互补与冗余信息，依据某种优化准则加以组合，产生对观测环境或对象的一致性解释和描述，实现多个传感器共同或联合操作，提高整个传感器系统的有效性；
• 数据融合的目标：利用各种传感器的独立观测信息，对数据进行多级别、多方位和多层次的处理，产生新的有意义的信息；
• 自动驾驶汽车定位主要模式：DR、GNSS、GNSS/DR组合定位；引入地图匹配后，组合出新的模式：DR/MM、GNSS/MM、GNSS/DR/MM；多传感器融合定位系统可在6中模式中自动切换以提高整个系统的定位精度和可靠性；

4.1.1 多传感器融合定位系统体系结构

多传感器融合定位系统体系结构主要包括：松耦合(Loose Coupling)、紧耦合(Tight Coupling)、深耦合(Deep Coupling)；

1. 松耦合
   松耦合系统中，GNSS给INS提供位置信息，二者硬件上相互独立且可随时断开连接，分别输出定位信息与速度信息到融合滤波器，融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后进行输出；
   
2. 紧耦合
   紧耦合将由GNSS码环与载波跟踪环解算得到的伪距、伪距率与由惯性导航系统结合自身信息和卫星星历进行计算得到的伪距、伪距率做差，得到伪距与伪距率的测量残差，将其作为融合滤波器的输入观测量，得到惯性导航系统计算误差以完成对惯性导航系统的校正并获得位置与速度的最有估计值；
   
3. 深耦合
   深耦合系统相对于紧耦合系统，增加了INS单元对GNSS接收机的辅助；利用INS单元结合星历可以对伪距与载波的多普勒频移进行估计，利用估计结果辅助接收机的捕获与跟踪环路，可以有效地提高GNSS接收机跟踪环路的动态性与灵敏度；

4.1.2 多传感器融合定位系统分层

多传感器融合定位系统划分为：数据层融合、特征层融合、决策层融合；

1. 数据层融合(像素级融合)

   • 数据层融合，首先将传感器的观测数据融合，然后从融合的数据中提取特征向量，并进行判断识别；
   • 数据层融合需要传感器是同质的(传感器观测的是同一物理量)，如果多个传感器是异质的(传感器观测的不是同一个物理量)，则数据只能在特征层或决策层融合；

2. 特征层融合

   特征层融合先从每种传感器提供的观测数据中提取有代表性的特征，这些特征融合成单一的特征向量，然后运用模式识别的方法进行处理；

3. 决策层融合

   决策层融合指在每个传感器对目标做出识别后，再将多个传感器的识别结果进行融合，属于高层次融合；

4.2 多传感器融合定位系统原理

• 多传感器融合定位系统输入主要来自GNSS-RTK、惯性导航系统和地图匹配定位系统；
• 融合定位系统对其数据进行预处理、数据配准和数据融合等处理后，输出汽车自身的速度、位置和姿态等信息；
• 数据预处理可以考虑为传感器初始化及校准，传感器初始化相对于系统坐标系独立地校准每一个传感器；一旦完成传感器初始化，就可以利用各传感器对共同目标采集得到的数据进行数据配准；
• 数据配准：把来自一个或多个传感器的观测或点迹数据与已知或已经确认的事件归并到一起，保证每个事件集合所包含的观测与点迹数据来自同一个实体的概率较大；
• 传感器的配准主要包括：时间配准和空间配准；

4.2.1 时间配准

• 时间配准：将关于同一个目标的各传感器不同步的量测信息同步到同一时刻；
• 时间配准的一般做法：将各传感器数据统一到扫描周期较长的一个传感器数据上；常用的方法：最小二乘法(Least Squares，LS)和内插外推法；

最小二乘法时间配准：

假设有两类传感器，分别表示为传感器1和传感器2，其采样周期分别为$\tau$和$T$，且两者之比为：$\tau :T=n$，如果第一类传感器1对目标状态的最近一次更新时刻为： t k − 1 t_{k-1} tk−1，下一次更新时刻为： t k = t k − 1 n T t_k=t_{k-1} nT tk=tk−1 nT，意味着在传感器1连续目标状态更新之间传感器2有$n$次测量值；

用 Z n = [ z 1 , z 2 , … , z n ] T Z_n=[z_1,z_2,\dots,z_n]^T Zn=[z1,z2,…,zn]T表示传感器2在 t k − 1 ～ t k t_{k-1}～t_k tk−1～tk时刻的$n$个位置量测构成的测量矩阵， z n z_n zn和传感器1在 t k t_k tk时刻的量测值同步，若用 U = [ z , z ˙ ] T U=[z,\dot{z}]^T U=[z,z˙]T表示 z 1 , z 2 , … , z n z_1,z_2,\dots,z_n z1,z2,…,zn融合后的量测值及其导数构成的列向量，则传感器2的量测值 z i z_i zi可以表示为：
z i = z ( i − n ) T z ˙ v i ； i = 1 , 2 , … , n z_i=z (i-n)T\dot{z} v_i；i=1,2,\dots,n zi=z (i−n)Tz˙ vi；i=1,2,…,n
其中： v i v_i vi表示量测噪声；

上式的向量形式：
Z n = W n U V n Z_n=W_nU V_n Zn=WnU Vn
其中： V n = [ v 1 , v 2 , … , v n ] T V_n=[v_1,v_2,\dots,v_n]^T Vn=[v1,v2,…,vn]T，其均值为零，协方差阵为：
c o v ( V n ) = d i a g { σ r 2 , σ r 2 , … , σ r 2 } cov(V_n)=diag\{\sigma_r^2,\sigma_r^2,\dots,\sigma_r^2\} cov(Vn)=diag{σr2,σr2,…,σr2}
其中： σ r 2 \sigma_r^2 σr2为融合以前的位置量测噪声方差；
W n = [ 1 1 … 1 ( 1 − n ) T ( 2 − n ) T … ( n − n ) T ] T W_n= \begin{bmatrix} 1 & 1 & \dots & 1\\ (1-n)^T & (2-n)T & \dots & (n-n)T \end{bmatrix}^T Wn=[1(1−n)T1(2−n)T……1(n−n)T]T
根据最小二乘法准则得到目标函数：
J = V n T V n = [ Z n − W n U ^ ] T [ Z n − W n U ^ ] J=V_n^TV_n=[Z_n-W_n\hat{U}]^T[Z_n-W_n\hat{U}] J=VnTVn=[Zn−WnU^]T[Zn−WnU^]
要使$J$为最小，在$J$两边对 U ^ \hat{U} U^求偏导并令其等于零：
∂ J ∂ U ^ = − 2 ( W n T Z n − W n T W n U ^ ) = 0 \frac{\partial{J}}{\partial\hat{U}}=-2(W_n^TZ_n-W_n^TW_n\hat{U})=0 ∂U^∂J=−2(WnTZn−WnTWnU^)=0
解得：
U ^ = [ z ˙ , z ˙ ^ ] = ( W n T W n ) − 1 W n T Z n \hat{U}=[\dot{z},\hat{\dot{z}}]=(W_n^TW_n)^{-1}W_n^TZ_n U^=[z˙,z˙^]=(WnTWn)−1WnTZn
相应的误差协方差阵为：
R U ^ = ( W n T W n ) − 1 σ r 2 R_{\hat{U}}=(W_n^TW_n)^{-1}\sigma_r^2 RU^=(WnTWn)−1σr2
对传感器2的$n$个量测值进行融合得 t k t_k tk时刻的量测值及量测噪声方差分别为：
z ^ t k = c 1 ∑ i = 1 n z i c 2 ∑ i = 1 n i ⋅ z i ， 其 中 ： c 1 = − 2 / n ， c 2 = 6 / [ n ( n 1 ) ] \hat{z}_{t_k}=c_1\sum_{i=1}^nz_i c_2\sum_{i=1}^ni·z_i，其中：c_1=-2/n，c_2=6/[n(n 1)] z^tk=c1i=1∑nzi c2i=1∑ni⋅zi，其中：c1=−2/n，c2=6/[n(n 1)]

4.2.2 空间配准

• 空间配准：借助于多传感器对空间共同目标的量测结果对传感器的偏差进行估计和补偿；
• 对于同一系统内采用不同坐标系的各传感器的量测值，定位时必须将它们转换成同一坐标系中的数据，对于多个不同子系统，各子系统采用的坐标系是不同的，所以在融合处理各子系统间信息前，需要将它们转换到同一量测坐标系中，处理后需将结果转换成各子系统坐标系的数据，再传送给各子系统；

• 图3-26：由于传感器1(传感器2)存在斜距和方位角偏差 Δ r 1 、 Δ θ 1 ( Δ r 2 、 Δ θ 2 ) \Delta{r_1}、\Delta\theta_1(\Delta{r_2}、\Delta\theta_2) Δr1、Δθ1(Δr2、Δθ2)，导致在系统平面上出现两个目标，而实际只有一个真实目标，需要进行空间配准；

• 图3-27：

  • r 1 、 θ 1 r_1、\theta_1 r1、θ1分别表示传感器1的斜距和方位角量测值；
  • r 2 、 θ 2 r_2、\theta_2 r2、θ2分别表示传感器2的斜距和方位角量测值；
  • ( x s 1 , y s 1 ) (x_{s1},y_{s1}) (xs1,ys1)表示传感器1在导航坐标平面上的位置；
  • ( x s 2 , y s 2 ) (x_{s2},y_{s2}) (xs2,ys2)表示传感器2在导航坐标平面上的位置；
  • ( x 1 , y 1 ) (x_1,y_1) (x1,y1)表示传感器1在导航坐标系上的量测值；
  • ( x 2 , y 2 ) (x_2,y_2) (x2,y2)表示传感器2在导航坐标系上的量测值；

• 方程推导：
  { x 1 = x s 1 r 1 sin ⁡ θ 1 y 1 = y s 1 r 1 cos ⁡ θ 1 x 2 = x s 2 r 2 sin ⁡ θ 2 y 2 = y s 2 r 2 cos ⁡ θ 2 (1) \begin{cases} & x_1 = x_{s1} r_1\sin\theta_1 \\ & y_1 = y_{s1} r_1\cos\theta_1 \\ & x_2 = x_{s2} r_2\sin\theta_2 \\ & y_2 = y_{s2} r_2\cos\theta_2 \end{cases}\tag{1} ⎩⎪⎪⎪⎨⎪⎪⎪⎧x1=xs1 r1sinθ1y1=ys1 r1cosθ1x2=xs2 r2sinθ2y2=ys2 r2cosθ2(1)
  如果忽略噪声，则有：
  { r 1 = r 1 ′ Δ r 1 θ 1 = θ 1 ′ Δ θ 1 r 2 = r 2 ′ Δ r 2 θ 2 = θ 2 ′ Δ θ 2 (2) \begin{cases} & r_1 = r_1' \Delta{r_1} \\ & \theta_1 = \theta_1' \Delta{\theta_1} \\ & r_2 = r_2' \Delta{r_2} \\ & \theta_2 = \theta_2' \Delta{\theta_2} \end{cases}\tag{2} ⎩⎪⎪⎪⎨⎪⎪⎪⎧r1=r1′ Δr1θ1=θ1′ Δθ1r2=r2′ Δr2θ2=θ2′ Δθ2(2)
  其中：

  • r 1 ′ 、 θ 1 ′ r_1'、\theta_1' r1′、θ1′分别表示目标相对于传感器1的真实斜距和方位角；
  • r 2 ′ 、 θ 2 ′ r_2'、\theta_2' r2′、θ2′分别表示目标相对于传感器2的真实斜距和方位角；
  • Δ r 1 、 Δ θ 1 \Delta{r_1}、\Delta{\theta_1} Δr1、Δθ1分别表示传感器1的斜距和方位角偏差；
  • Δ r 2 、 Δ θ 2 \Delta{r_2}、\Delta{\theta_2} Δr2、Δθ2分别表示传感器2的斜距和方位角偏差；

  将式(2)代入式(1)，将得到的方程相对于 Δ r 1 、 Δ θ 1 、 Δ r 2 、 Δ θ 2 \Delta{r_1}、\Delta\theta_1、\Delta{r_2}、\Delta\theta_2 Δr1、Δθ1、Δr2、Δθ2进行一阶泰勒级数展开：
  { x 1 − x 2 ≈ sin ⁡ θ 1 Δ r 1 − sin ⁡ θ 2 Δ θ 2 r 1 cos ⁡ θ 1 Δ θ 1 − r 2 cos ⁡ θ 2 Δ θ 2 y 1 − y 2 ≈ cos ⁡ θ 1 Δ r 1 − cos ⁡ θ 2 Δ r 2 − r 1 sin ⁡ θ 1 Δ θ 1 r 2 sin ⁡ θ 2 Δ θ 2 (3) \begin{cases} & x_1-x_2≈\sin\theta_1\Delta{r_1}-\sin\theta_2\Delta\theta_2 r_1\cos\theta_1\Delta\theta_1-r_2\cos\theta_2\Delta\theta_2 \\ & y_1-y_2≈\cos\theta_1\Delta{r_1}-\cos\theta_2\Delta{r_2}-r_1\sin\theta_1\Delta\theta_1 r_2\sin\theta_2\Delta\theta_2 \end{cases}\tag{3} {x1−x2≈sinθ1Δr1−sinθ2Δθ2 r1cosθ1Δθ1−r2cosθ2Δθ2y1−y2≈cosθ1Δr1−cosθ2Δr2−r1sinθ1Δθ1 r2sinθ2Δθ2(3)

• 常用的与目标运动航迹无关的偏差估计方法：实时质量控制法(Real Time Quality Control，RTQC)、最小二乘法、极大似然法(Maximum Likelihood，ML)、基于卡尔曼滤波器的空间匹配算法等；

  • 实时质量控制法和最小二乘法完全忽略传感器量测噪声的影响，认为公共坐标系中的误差来源于传感器的配准误差；
  • 广义最小二乘法(Generalized Least Square，GLS)和基于卡尔曼滤波器方法考虑了传感器量测噪声的影响，但只有在量测噪声相对小时，才会产生好的性能；
  • 精确极大似然(Exact Maximum Likelihood，EML)空间配准算法可以克服前两种局限性；

4.3 多传感器融合误差分析

多传感器配准误差主要来源：

• 传感器的误差，即传感器本身因制造误差带来的偏差；
• 各传感器参考坐标系中量测的方位角、高低角和斜距偏差；通常是因量测系统解算传感器数据时造成的误差；
• 相对于公共坐标系的传感器的位置误差和计时误差；位置误差通常由传感器导航系统偏差引起，计时误差通常由传感器时钟偏差所致；
• 各传感器采用的定位算法不同，从而引起单系统内局部定位误差；
• 各传感器本身的位置不确定，为融合处理而进行坐标转换时产生偏差；
• 坐标转换的精度不够，为了减少系统的计算负担而在投影变换时采用了一些近似方法所导致的误差；

4.4 多传感器融合算法

4.4.1 数据融合算法概述

• 融合算法分为：随机类和人工智能类；

  • 随机类多传感器融合：综合估计法、贝叶斯估计法、D-S证据推理、最大似然估计法、卡尔曼滤波算法、鲁棒估计等；
  • 人工智能类多传感器融合：模糊逻辑法、神经网络算法、专家系统；

• 动态空间模型，如：卡尔曼滤波器采用高斯-马尔可夫线性模型，用状态方程描述状态随时间演变的过程，用观测方程描述与状态有关的噪声变量，数学映射为：
  { 状 态 方 程 ： X k = f ( X k − 1 , W k ) 观 测 方 程 ： L k = h ( X k , V k ) (1) \begin{cases} & 状态方程：X_k=f(X_{k-1},W_k) \\ & 观测方程：L_k=h(X_k,V_k) \end{cases}\tag{1} {状态方程：Xk=f(Xk−1,Wk)观测方程：Lk=h(Xk,Vk)(1)
  上两式称为动态空间模型；

  其中：

  • X k ∈ R k x X_k\in{R^{k_x}} Xk∈Rkx：系统在$k$时刻的状态；
  • L k ∈ R k x L_k\in{R^{k_x}} Lk∈Rkx：系统状态 X k X_k Xk的观测值；
  • W k 、 V k W_k、V_k Wk、Vk：过程和观测噪声；

常用算法简单说明：

1. 综合平均法

   该算法把来自多个传感器的众多数据进行综合平均；适用于用同样传感器检测同一个目标的情况；如果对一个检测目标进行了$k$次检测，其平均值 S ‾ = ∑ i = 1 k W i S i / ∑ i = 1 k W i \overline{S}=\sum_{i=1}^kW_iS_i/\sum_{i=1}^kW_i S=∑i=1kWiSi/∑i=1kWi，其中， W i W_i Wi为分配给第$i$次检测的权值；

2. 贝叶斯估计法

   贝叶斯推理技术主要用来进行决策层融合；贝叶斯估计法通过先验信息和样本信息合成为后验分布，对检测目标做出判断；贝叶斯估计是一个不断预测和更新的过程；

3. D-S证据推理

   D-S证据推理是目前数据融合技术中常用的算法，该算法通常用来对检测目标的大小、位置及存在与否进行推断，采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理；

   提取的特征参数构成该理论中的证据，利用这些证据构成相应的基本概率分布函数，对于所有的命题赋予一个信任度；基本概率分布函数及其相应的分辨框合称为一个证据体；

   每个传感器相当于一个证据体，多个传感器数据融合，实质上是在同一个分辨框下，利用Dempster合并规则将各个证据体合并成一个新的证据体，产生新证据体的过程就是D-S证据推理数据融合；

4. 卡尔曼滤波算法

   联合卡尔曼滤波器设计思想：先分散处理、再全局融合，即在诸多非相似子系统中选择一个信息全面、输出率高、可靠性绝对保证的子系统作为公共参考系统，与其他子系统两两结合，形成若干子滤波器；各子滤波器并行运行，获得建立在子滤波器局部观测基础上的局部最优估计，这些局部最优估计在于主滤波器内按融合算法合成，从而获得建立在所有观测基础上的全局估计；

5. 模糊逻辑法

   针对数据融合中所检测的目标特征具有某种模糊性的现象，利用模糊逻辑算法来对检测目标进行识别和分类；

6. 神经网络算法

   要使神经网络算法在实际融合系统中得到应用，无论在网络结构设计或是算法规则方面，还有很多基础工作要做，如：网络模型、网络层次、每层的节点、网络学习策略、神经网络算法与传统分类算法的关系和综合应用等；

7. 专家系统

   专家系统是一组计算机程序，获取专家们在某个特定领域内的知识，然后根据专家的知识或经验导出一组规则，由计算机做出本应由专家做出的结论；

各种融合算法比较：

4.4.2 卡尔曼滤波算法

卡尔曼滤波分为：线性卡尔曼滤波、扩展卡尔曼滤波、级联式和联邦式卡尔曼滤波、无迹卡尔曼滤波等；

1. 最小方差估计

   最小方差估计：指以均方误差最小作为估计准则的估计，满足下式(1)：
   E { [ X − X ^ ( Z ) ] [ X − X ^ ( Z ) ] T } ≤ E { [ X − r ( z ) ] [ X − r ( Z ) ] T } (1) E\{[X-\hat{X}(Z)][X-\hat{X}(Z)]^T\}≤E\{[X-r(z)][X-r(Z)]^T\}\tag{1} E{[X−X^(Z)][X−X^(Z)]T}≤E{[X−r(z)][X−r(Z)]T}(1)

   • $X$：$m$维系统状态变量矢量；
   • $Z$：$n$维观测矢量；
   • X ^ ( Z ) \hat{X}(Z) X^(Z)：用观测矢量$Z$计算得出的关于$X$的最小方差估计；
   • E { ⋅ } E\{·\} E{⋅}：取均值；
   • $r(Z)$：由其他方法得到的$X$的估计值；

   最小方差估计是无偏的，即残差的均值为0，满足下式(2)：
   E { X ~ } = E { X − X ^ ( Z ) } = 0 (2) E\{\tilde{X}\}=E\{X-\hat{X}(Z)\}=0\tag{2} E{X~}=E{X−X^(Z)}=0(2)
   最小方差估计的均方误差就是估计误差的方差，满足下式(3)：
   E { X ~ X ~ T } = E { [ X ~ − E ( X ~ ) ] [ X ~ − E ( X ~ ) ] T } (3) E\{\tilde{X}\tilde{X}^T\}=E\{[\tilde{X}-E(\tilde{X})][\tilde{X}-E(\tilde{X})]^T\}\tag{3} E{X~X~T}=E{[X~−E(X~)][X~−E(X~)]T}(3)

2. 卡尔曼滤波估计

   如果将估计值 X ^ ( Z ) \hat{X}(Z) X^(Z)规定为观测矢量$Z$的线性函数，即：
   X ^ ( Z ) = A Z b (4) \hat{X}(Z)=AZ b\tag{4} X^(Z)=AZ b(4)

   • $A、b$：分别为$m\times n$阶的矩阵和$n$维矢量；
   • $A、b$值仍旧按照最小方差估计来选择，则这样的估计被称为线性最小方差估计；

3. 卡尔曼滤波说明

   • 卡尔曼滤波是一种递推线性最小方差估计，估计准则仍是方差最小估计技术；
   • 卡尔曼滤波是一种递推线性最小方差估计，估计值是观测值的线性函数；
   • 卡尔曼滤波是用状态方程和观测方程来描述系统和观测值的，主要适用于线性动态系统；

4. 卡尔曼滤波方程

   卡尔曼滤波常采用离散化模型描述系统，离散系统即用离散化后的差分方程描述连续系统；假设离散化后的系统状态方程和观测方程为：
   { X k = Φ k , k − 1 X k − 1 Γ k − 1 W k − 1 Z k = H k X k V k (5) \begin{cases} & X_k=\Phi_{k,k-1}X_{k-1} \Gamma_{k-1}W_{k-1} \\ & Z_k=H_kX_k V_k \end{cases}\tag{5} {Xk=Φk,k−1Xk−1 Γk−1Wk−1Zk=HkXk Vk(5)

   • X k X_k Xk：$k$时刻的$n$维状态矢量，被估计矢量；
   • Z k Z_k Zk：$k$时刻的$m$维观测矢量；
   • Φ k , k − 1 \Phi_{k,k-1} Φk,k−1：$k-1$时刻到$k$时刻的系统一步转移矩阵$(n\times n)$；
   • W k − 1 W_{k-1} Wk−1：$k-1$时刻的系统噪声$(r维)$；
   • Γ k − 1 \Gamma_{k-1} Γk−1：系统噪声矩阵$(n\times r)$，表征由$k-1$时刻到$k$时刻的各个系统噪声分别影响各个状态的程度；
   • H k H_k Hk：$k$时刻的观测矩阵$(m\times n)$；
   • V k V_k Vk：$k$时刻的$m$维观测噪声；

   卡尔曼滤波要求 { W k } \{W_k\} {Wk}和 { V k } \{V_k\} {Vk}是互不相关的零均值的高斯白噪声序列：
   { E { W k W j T } = Q k δ k j E { V k V j T } = R k δ k j (6) \begin{cases} & E\{W_kW_j^T\}=Q_k\delta_{kj} \\ & E\{V_kV_j^T\}=R_k\delta_{kj} \end{cases}\tag{6} {E{WkWjT}=QkδkjE{VkVjT}=Rkδkj(6)

   • Q k 、 R k Q_k、R_k Qk、Rk：分别为系统噪声和观测噪声的方差矩阵，在卡尔曼滤波中要求分别是已知值的非负矩阵和正定矩阵；

   • δ k j \delta_{kj} δkj：迪利克雷函数；
     δ k j = { 0 , k ≠ j 1 , k = j (7) \delta_{kj}= \begin{cases} 0, & k ≠ j \\ 1, & k =j \end{cases}\tag{7} δkj={0,1,k=jk=j(7)

   初始状态的一、二阶统计特性：
   { E { X 0 } = m x 0 v a r { X 0 } = C x 0 (8) \begin{cases} & E\{X_0\}=m_{x_0} \\ & var\{X_0\}=C_{x_0} \end{cases}\tag{8} {E{X0}=mx0var{X0}=Cx0(8)

   • v a r { ⋅ } var\{·\} var{⋅}表示求方差；卡尔曼滤波要求 m x 0 、 C x 0 m_{x_0}、C_{x_0} mx0、Cx0为已知量，且要求 X 0 、 { W k } 、 { V k } X_0、\{W_k\}、\{V_k\} X0、{Wk}、{Vk}都互不相关；

5. 离散卡尔曼滤波的计算流程

   假如在$k-1$时刻已经获得了对 X k − 1 X_{k-1} Xk−1的最优估计值 X ^ k − 1 \hat{X}_{k-1} X^k−1，且在$k$时刻又观测到 Z k Z_k Zk，则当前时刻$k$的最优估计 X ^ k \hat{X}_k X^k可以用两者的线性组合表示，即
   X ^ k = A X ^ k − 1 B Z k (9) \hat{X}_k=A\hat{X}_{k-1} BZ_k\tag{9} X^k=AX^k−1 BZk(9)
   其中：$A、B$待定但需保证不同维，满足：
   E ( ( X k − ( A X ^ k − 1 B Z k ) ) ( X k − ( A X ^ k − 1 B Z k ) ) T ) ≤ E ( ( X k − ( A ~ X ^ k − 1 B ~ Z k ) ) ( X k − ( A ~ X ^ k − 1 B ~ Z k ) ) T ) (10) \begin{aligned} &E((X_k-(A\hat{X}_{k-1} BZ_k))(X_k-(A\hat{X}_{k-1} BZ_k))^T)\\ &≤E((X_k-(\tilde{A}\hat{X}_{k-1} \tilde{B}Z_k))(X_k-(\tilde{A}\hat{X}_{k-1} \tilde{B}Z_k))^T) \end{aligned}\tag{10} E((Xk−(AX^k−1 BZk))(Xk−(AX^k−1 BZk))T)≤E((Xk−(A~X^k−1 B~Zk))(Xk−(A~X^k−1 B~Zk))T)(10)
   其中： A ~ 、 B ~ \tilde{A}、\tilde{B} A~、B~分别为与$A、B$同维的任意矩阵；

   卡尔曼滤波按照如下流程计算：

   状态一步预测方程：
   X ^ k ∣ k − 1 = Φ k , k − 1 X ^ k − 1 (11) \hat{X}_{k|k-1}=\Phi_{k,k-1}\hat{X}_{k-1}\tag{11} X^k∣k−1=Φk,k−1X^k−1(11)
   状态估计计算方程为：
   X ^ k = X ^ k ∣ k − 1 K k ( Z k − H k X ^ k ∣ k − 1 ) (12) \hat{X}_k=\hat{X}_{k|k-1} K_k(Z_k-H_k\hat{X}_{k|k-1})\tag{12} X^k=X^k∣k−1 Kk(Zk−HkX^k∣k−1)(12)
   其中： K k K_k Kk为卡尔曼滤波增益；

   滤波增益方程：
   K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T R k ) − 1 (13) K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T R_k)^{-1}\tag{13} Kk=Pk∣k−1HkT(HkPk∣k−1HkT Rk)−1(13)
   一步预测均方误差方程为：
   P k ∣ k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T Γ k − 1 Q k − 1 Γ k − 1 T (14) P_{k|k-1}=\Phi_{k,k-1}P_{k-1}\Phi_{k,k-1}^T \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T\tag{14} Pk∣k−1=Φk,k−1Pk−1Φk,k−1T Γk−1Qk−1Γk−1T(14)
   估计均方误差方程为：
   P k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T K k R k K k T (15) P_k=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T K_kR_kK_k^T\tag{15} Pk=(I−KkHk)Pk∣k−1(I−KkHk)T KkRkKkT(15)

6. 扩展卡尔曼滤波

   假设上式(5)的状态方程和观测方程是非线性的，则系统状态方程和观测方程为：
   { X k = f ( X k − 1 ) W k Z k = h ( X k ) V k (16) \begin{cases} & X_k=f(X_{k-1}) W_k \\ & Z_k=h(X_k) V_k \end{cases}\tag{16} {Xk=f(Xk−1) WkZk=h(Xk) Vk(16)
   状态一步预测方程：
   X ^ k ∣ k − 1 = f ( X ^ k − 1 ) W k ， 其 中 ： W k 为 过 程 噪 声 (17) \hat{X}_{k|k-1}=f(\hat{X}_{k-1}) W_k，其中：W_k为过程噪声\tag{17} X^k∣k−1=f(X^k−1) Wk，其中：Wk为过程噪声(17)
   扩展卡尔曼滤波算法：将非线性方程线性化的滤波算法，是解决非线性滤波问题常用的一种方法；

7. 联邦卡尔曼滤波
   
   联邦卡尔曼滤波一般分为：先基于局部传感器进行滤波，然后再进行主滤波；

   先假设状态向量从$k-1$时刻的 X k − 1 X_{k-1} Xk−1转移到$k$时刻的 X k X_k Xk，动力学模型：
   X k = Φ k , k − 1 X k − 1 W k (18) X_k=\Phi_{k,k-1}X_{k-1} W_k\tag{18} Xk=Φk,k−1Xk−1 Wk(18)
   其中： Φ k , k − 1 \Phi_{k,k-1} Φk,k−1为时间$k-1$到$k$的状态转移矩阵， W k W_k Wk为动力学模型误差， W k W_k Wk与 W k − 1 W_{k-1} Wk−1不相关；

   设在$k$时刻有$r$个传感器，各传感器相应的观测方程为：
   L i k = A i k X k Δ i k (19) L_{ik}=A_{ik}X_k \Delta_{ik}\tag{19} Lik=AikXk Δik(19)
   其中： A i k A_{ik} Aik为传感器$i$的观测方程设计矩阵， L i k 、 Δ i k L_{ik}、\Delta_{ik} Lik、Δik分别为传感器$i$的观测矢量和误差矢量；

   假设各传感器观测误差与动态模型误差不相关，各传感器观测误差互不相关；
   E ( W k ) = 0 , E ( Δ i k ) = 0 (20) E(W_k)=0,E(\Delta_{ik})=0\tag{20} E(Wk)=0,E(Δik)=0(20)

   E [ Δ i k Δ i k T ] = ∑ i k = P i k − 1 ， E [ Δ i k Δ i k T ] = 0 (21) E[\Delta_{ik}\Delta_{ik}^T]=\sum_{ik}=P_{ik}^{-1}，E[\Delta_{ik}\Delta_{ik}^T]=0\tag{21} E[ΔikΔikT]=ik∑=Pik−1，E[ΔikΔikT]=0(21)

   E ( W k W k − 1 T ) = 0 , E ( W k W k T ) = ∑ W k (22) E(W_kW_{k-1}^T)=0,E(W_kW_k^T)=\sum_{W_k}\tag{22} E(WkWk−1T)=0,E(WkWkT)=Wk∑(22)

   由各传感器得到局部滤波解为：
   X ^ i k = [ I − K i k A i k ] X ‾ k K i k L i k (23) \hat{X}_{ik}=[I-K_{ik}A_{ik}]\overline{X}_k K_{ik}L_{ik}\tag{23} X^ik=[I−KikAik]Xk KikLik(23)
   其中：
   K i k = ∑ X ‾ k A i k T ( A i k ∑ X ‾ k A i k T ∑ i k ) (24) K_{ik}=\sum_{\overline{X}_k}A_{ik}^T(A_{ik}\sum_{\overline{X}_k}A_{ik}^T \sum_{ik})\tag{24} Kik=Xk∑AikT(AikXk∑AikT ik∑)(24)

   ∑ X ‾ k = Φ k , k − 1 ∑ X ^ k − 1 Φ k , k − 1 T ∑ W k (25) \sum_{\overline{X}_k}=\Phi_{k,k-1}\sum_{\hat{X}_{k-1}}\Phi_{k,k-1}^T \sum_{W_k}\tag{25} Xk∑=Φk,k−1X^k−1∑Φk,k−1T Wk∑(25)

   ∑ X ^ i k = ( I − K i k A i k ) ∑ X ^ k (26) \sum_{\hat{X}_{ik}}=(I-K_{ik}A_{ik})\sum_{\hat{X}_k}\tag{26} X^ik∑=(I−KikAik)X^k∑(26)

   设主传感器的观测矢量为 L m k L_{mk} Lmk，则主滤波解为：
   X ^ m k = [ I − K m k A m k ] X ‾ k K m k L m k (27) \hat{X}_{mk}=[I-K_{mk}A_{mk}]\overline{X}_k K_{mk}L_{mk}\tag{27} X^mk=[I−KmkAmk]Xk KmkLmk(27)
   式中 K m k 、 A m k K_{mk}、A_{mk} Kmk、Amk和 K i k 、 A i k K_{ik}、A_{ik} Kik、Aik类似；

   由联邦卡尔曼滤波算法信息分享原理，得到最终融合滤波解 X ^ f k \hat{X}_{fk} X^fk及相应的权矩阵 P X ^ f k P_{\hat{X}_{fk}} PX^fk为：
   P X ^ f k = P X ^ 1 k P X ^ 2 k ⋯ P X ^ r k P X ^ m k (28) P_{\hat{X}_{fk}}=P_{\hat{X}_{1k}} P_{\hat{X}_{2k}} \dots P_{\hat{X}_{rk}} P_{\hat{X}_{mk}}\tag{28} PX^fk=PX^1k PX^2k ⋯ PX^rk PX^mk(28)

   P X ^ f k X ^ f k = P X ^ 1 k X ^ 1 k P X ^ 2 k X ^ 2 k ⋯ P X ^ r k X ^ r k P X ^ m k X ^ m k (29) P_{\hat{X}_{fk}}\hat{X}_{fk}=P_{\hat{X}_{1k}}\hat{X}_{1k} P_{\hat{X}_{2k}}\hat{X}_{2k} \dots P_{\hat{X}_{rk}}\hat{X}_{rk} P_{\hat{X}_{mk}}\hat{X}_{mk}\tag{29} PX^fkX^fk=PX^1kX^1k PX^2kX^2k ⋯ PX^rkX^rk PX^mkX^mk(29)

   其中： P X ^ 1 k , P X ^ 2 k , … , P X ^ m k P_{\hat{X}_{1k}},P_{\hat{X}_{2k}},\dots,P_{\hat{X}_{mk}} PX^1k,PX^2k,…,PX^mk分别为局部传感器滤波输出的状态估计矢量的权矩阵； P X ^ f k P_{\hat{X}_{fk}} PX^fk为联邦滤波算法输出的状态矢量的权矩阵(或信息矩阵)，它们是相应协方差矩阵的逆矩阵；
   P X ^ i k = ∑ X ^ i k − 1 ， P X ^ m k = ∑ X ^ m k − 1 (30) P_{\hat{X}_{ik}}=\sum_{\hat{X}_{ik}}^{-1}，P_{\hat{X}_{mk}}=\sum_{\hat{X}_{mk}}^{-1}\tag{30} PX^ik=X^ik∑−1，PX^mk=X^mk∑−1(30)
   则有：
   X ^ f k = P X f k − 1 ( P X ^ 1 k X ^ 1 k P X ^ 2 k X ^ 2 k ⋯ P X ^ r k X ^ r k P X ^ m k X ^ m k ) (31) \hat{X}_{fk}=P_{X_{fk}}^{-1}(P_{\hat{X}_{1k}}\hat{X}_{1k} P_{\hat{X}_{2k}}\hat{X}_{2k} \dots P_{\hat{X}_{rk}}\hat{X}_{rk} P_{\hat{X}_{mk}}\hat{X}_{mk})\tag{31} X^fk=PXfk−1(PX^1kX^1k PX^2kX^2k ⋯ PX^rkX^rk PX^mkX^mk)(31)

这篇好文章是转载于：学新通技术网