多传感器分布式融合算法——加权最小二乘WLS融合/简单凸组合SCC融合

加权最小二乘WLS融合/简单凸组合SCC融合——多传感器分布式融合算法

原创不易，路过的各位大佬请点个赞

主要讲解算法：
         加权最小二乘融合WLS
         简单凸组合融合SCC

应用于: 多传感器网络协同目标跟踪/定位/导航

联系WX: ZB823618313

1. 分布式航迹融合

         根据系统需求(成本、安全性、可维护性等)以及外界环境(自然环境、人为对抗环境),信息融合系统的结构一般可划分为:集中式结构、分布式结构以及混合式结构。

          分布式航迹融合也称为传感器级融合或自主式融合。在这种结构中,每个传感器都有自己的处理器,进行一些预处理,然后把中间结果送到中心节点, 进行融合处理。由于各传感器都具有自己的局部处理器,能够形成局部航迹,所以在融合中心也主要是对局部航迹进行融合,所以这种融合方法通常也称为航迹融合(track fusion)。

这种结构因对信道要求低、系统生命力强、工程上易于实现、且对融合中心的计算能力要求低。

从不同的角度，分布式融合也可以分为不同类型。分布式结构也可以分为三种形式:
1）有融合中心的分布式结构
2）无融合中心,共享航迹的分布式结构
3）无融合中心,共享关联量测的分布式结构。`

笔记：

多传感器分布式融合也是估计融合领域最热门的邻域之一，也是目前两大学者在融合邻域关注的焦点

此外，分布式融合算法种类非常多，研究角度也各不相同。换句话说，分布式融合本身就是一个非常大的方向

本博客主要介绍分布式融合中最常用、最普遍的一种算法：加权最小二乘融合/简单凸组合融合

          分布式航迹融合算法非常众多：

1. 简单凸组合SCC融合算法
2. 加权最小二乘WLS融合
3. 协方差交叉CI融合算法
4. 分布式信息滤波器算法
5. Bar-Shalom-Campo融合算法
6. 联邦卡尔曼滤波器算法
7. …

本博客主要讲前两种融合算法。实际上它们可以被认为是一种融合算法的不同叫法。它们的理论是一样的，即、利用局部估计的协方差对局部估计进行加权融合得到全局估计。

下面主要介绍WLS 和SCC融合的优势和缺点，以及它们比较适用的场景。

2. 加权最小二乘WLS融合

加权最小二乘WLS融合是最简单、最容易实现、鲁棒性最高、工程应用最广泛的分布式融合算法之一。

2.1 从分布式融合挑战——到——加权最小二乘WLS融合

          由于航迹融合中各传感器局部估计误差相关、各传感器量测噪声相关、局部动态模型不同以及系统的网络结构和信息模式复杂等因素，导致分布式融合算法的设计和开发变得极为复杂。

          这些因素一般直接影响算法的融合精度、计算效率、融合器的鲁棒性等等。

          是在实际工程中，各个传感器在不同的区域分布。它们相对独立地工作和观测目标，可以忽略各传感器之间噪声的相关性。

          此外，局部估计误差之间的相关性一般是未知的。这些信息在融合过程无法利用。

          从而加权最小二乘WLS融合算法应运而生。它忽略了局部估计误差的相关性和各传感器量测噪声的相关性，得到一种结构简单且计算效率高的解析融合器。

          加权最小二乘WLS融合算法应该是工程中应用最广泛和最成功的融合算法之一。这也是为什么它是最经典的分布式哦

2.2 加权最小二乘WLS融合结构

          下图为加权最小二乘WLS融合算法的结构图。从图中可以看出，各个局部传感器向融合传输局部估计及其协方差。融合中心统一融合处理接受的局部估计及其协方差。

图1：加权最小二乘WLS融合结构

\
特点
1）加权最小二乘WLS融合中通信是单向的，即局部传感器向融合中心传输处理后的信息，而融合中心不向局部传感器传输任何信息；

2）各个局部传感器独立工作，无任何协作；

3）融合中心只需要局部估计和它对应的协方差；

4）单个局部传感器的损坏并不影响全局估计的工作；

2.3 加权最小二乘WLS融合算法

2.3.1 问题描述：目标运动和量测模型

         假设有$M$个传感器用于观测同一个目标， 且目标状态转移方程为

$$x_k=f(x_{k-1})+w_{k-1}\text{\hspace{1em}(1)}$$

其中$x_k$目标在$k$时刻的状态，$w_{k-1}$过程噪声。

         传感器量测方程为

$$z_k^i=h(x_k)+v_k^i\text{\hspace{1em}(2)}$$

其中$z_k^i$为第$i$个传感器在$k$时刻的量测数据，$v_k^i$第$i$个传感器的量测噪声。

         一般假设$w_k$和$v_k$为零均值高斯白噪声，其方差分别为$Q_k$和$R_k$的高斯白噪声，即$w_k\sim (0,Q_k)$, $v_k\sim (0,R_k)$，且满足如下关系(线性高斯假设)为：
$$\begin{array}{rl}E[w_i{v}_j^{\prime }]& =0\\ E[w_i{w}_j^{\prime }]& =0i\ne j\\ E[v_i{v}_j^{\prime }]& =0i\ne j\end{array}$$

2.3.2 局部估计（航迹）生成

         每个传感器的利用观测到的数据实现目标状态的估计。定义
$${\hat{x}}_{k|k}^i,P_{k|k}^i$$
为传感器$i$产生的目标的局部估计和协方差。总共有$M$个传感器，因此就有$M$个类似的局部估计。这些局部估计都被传送融合中心进行融合处理。

如何产生这些局部航迹，则是一个经典的滤波问题：

1. 如果(1)和(2)都是线性的，用卡尔曼滤波KF/鲁棒卡尔曼滤波产生
2. 如果(1)和(2)都是非线性的，用EKF/UKC/CKF/QKF/CDKF/DDF/SRQUKF/PF等
   非线性滤波参见专栏：非线性滤波-目标跟踪应用 //https://blog.csdn.net/weixin_44044161/category_11056621.html
3. 如果是机动目标跟踪，同交互式多模型IMM等等
   IMM参见专栏：非线性滤波-目标跟踪应用 //https://blog.csdn.net/weixin_44044161/category_10931736.html

2.3.3 加权最小二乘WLS融合

         融合中心利用某种融合算法对接受的$M$个传感器的局部估计及其协方差进行融合处理，得到最终的全局估计及其协方差。

         加权最小二乘WLS融合算法为：

$$\begin{gathered} {\hat{x}}_{k|k}=(\sum _{i=1}^M(P_{k|k}^i{)}^{-1}{)}^{-1}{\hat{x}}_{k|k}^i \\ {P}_{k|k}=(\sum _{i=1}^M(P_{k|k}^i{)}^{-1}{)}^{-1} \end{gathered}$$

由于简单凸联合融合算法实现起来特别容易,所以它得到了广泛的应用。

然而,当各传感器的局部估计误差相关时,它是次优的。例如,当其中一个航迹是系统航迹,而另一个为传感器航迹时,或者当存在过程噪声时,就是这种情况。

但是,当两条航迹都是传感器航迹并且不存在过程噪声,两个传感器在初始时刻的估计误差也不相关时,简单凸联合融合算法则是最优的。也就是说,它能够得到和中心式融合相同的结果。

加权最小二乘WLS融合 简单吧！！！！！！！！！！！！！！！！！！！！

2.4 简单凸组合SCC融合算法

        实际上，简单凸组合SCC融合和加权最小二乘WLS融合是同一种算法。SCC的原理和WLS的原理一样。好比同一个人有不同的名字。

3. 实验场景

讲了这么多，还不如直接弄一个实例。

既然是第一篇关于分布式融合的博客，实例直接上比较复杂的。哈哈哈哈哈哈

3.1 算法种类

算法：基于简单凸组合SCC的容积卡尔曼滤波CKF

3.2 参数设置

**运动模型：
仅匀转弯CT模型 $X=[x,y,\dot{x},\dot{y},w{]}^T$
X k + 1 = [ 1 sin ⁡ ( ω T ) ω 0 − 1 − cos ⁡ ( ω T ) ω 0 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) 0 0 0 0 0 1 ] X k + W k X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}&0\\0&\cos(\omega T)&0&-\sin(\omega T)&0\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}&0\\0&\sin(\omega T)&0&\cos(\omega T)&0\\0&0&0&0&1\end{bmatrix}X_{k} + W_k Xk+1​= ​10000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)0​00100​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)0​00001​ ​Xk​+Wk​
其中$W_k$为零均值白噪声，其方差为：
Q k = q k 2 [ T 3 / 3 T 2 / 2 0 0 0 T 2 / 2 T 0 0 0 0 0 T 3 / 3 T 2 / 2 0 0 0 T 2 / 2 T 0 0 0 0 0 q w ] Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0& 0 \\T^2/2&T&0&0 &0\\0&0&T^3/3&T^2/2 &0 \\0&0& T^2/2&T&0\\0&0&0&0&q_w\end{bmatrix} Qk​=qk2​ ​T3/3T2/2000​T2/2T000​00T3/3T2/20​00T2/2T0​0000qw​​ ​
或者为(两种形式都可以用，下面一代码形式给出)

传感器： 多个主动雷达
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \end{gathered}$$
$[x_0,y_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k{]}^{\prime }$。雷达量测方差为
$$R_k=\text{cov}(v_k)=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_b^2\end{array}\right]$$

3.3 实验结果

3.3.1 跟踪轨迹

3.3.2 位置均方误差RMSE

3.3.2 位置均方误差RMSE

4. 部分代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% date: 
% 任务：基于分布式融合算法的多雷达目标跟踪
% 一级算法：单雷达CKF
% 二级算法： 简单凸组合SCC融合
% 目标模型：CT
% 性能指标：跟踪轨迹，RMSE均方根误差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; 
close all; 
clc;
%% initial parameter
n=4; %dimension of the target ;
T=1; %sample time
M=3; %number of rader
N=150; %the runs atime
MC=50; %Monte Carlo Runs
chan=4; %channel, for the class of fiter
w_mu=[0,0]'; % mean of process noise 
v_mu=[0,0]'; % mean of measurement noise
%% target model
%covariance of process noise
q_x=1; %m/s^2
q_y=q_x;
Qk=diag([q_x^2,q_y^2]); 
% state matrix
T_f=T; %sample time of fusion center
w=-pi/180*2.5;
Fk= [1 sin(w*T_f)/w 0 -(1-cos(w*T_f))/w
       0 cos(w*T_f)   0 -sin(w*T_f)
       0 (1-cos(w*T_f))/w 1 sin(w*T_f)/w
       0  sin(w*T_f) 0 cos(w*T_f) ]; %
Gk= [ T_f^2/2  0
       T_f     0
       0      T_f^2/2
       0      T_f ]; %
% covariance of measurement noise (radar)




% 雷达初始坐标及速度，可以自己设定，
xp(:,1)=[2, 0, 3 ,0 , 4, 0]; % %第一个传感器的位置，可设置[x坐标， 速度， y坐标， 速度，z坐标， 速度]，xyz对应传感器三维位置中间的0不能变，只是为了适应维数
xp(:,2)=[4, 0, 7 ,0 , 3, 0];% %第2个传感器的位置[x坐标， 0， y坐标， 0，z坐标， 0]
xp(:,3)=[1, 0, 1 ,0 , 1, 0];% %第3个传感器的位置[x坐标， 0， y坐标， 0，z坐标， 0]

%% define parameter
sV=zeros(n,N,MC,1); % state
eV=zeros(n,N,MC,chan); %estimation




figure
plot(sV(1,:,1,1),sV(3,:,1,1),'-k',eV(1,:,1,1),eV(3,:,1,1),'-r',eV(1,:,1,2),eV(3,:,1,2),'--b',eV(1,:,1,3),eV(3,:,1,3),'--g')
xlabel('m');ylabel('m');
legend('State','CKF-sensor 1 ','CKF-sensor 2','SCC')
title('the compararison in x-y coordinate system')
figure;
ii=1:N;
plot(ii,sV(1,:,1,1),'k',ii,eV(1,:,1,1),'r',ii,eV(1,:,1,2),'--b',ii,eV(1,:,1,3),'--g');
xlabel('time(k)');ylabel('m');%zlabel('m');
legend('State','CKF-sensor 1 ','CKF-sensor 2','SCC')
title('the compararison in x coordinate system')

原创不易，路过的各位大佬请点个赞