多速率多传感器数据融合估计（一）

多速率多传感器数据融合估计（一）

对于多速率多传感器数据融合问题此处给出三种方法:
1.状态分块线性模型的建立
2.分布式无反馈数据融合算法
3.分布式有反馈数据融合算法
分两章阐述。

问题描述

设同步多速率系统的动态模型如下所示：

$\overline{C_i}(k)=[C_i((k-1)M_i+1)I_{n_i,1},C_i((k-1)M_i+2)I_{n_i,2}，\dots ,C_i(k{M}_i)I_{n_i,M_i}]$

$M为m(n_1,n_2,\dots ,n_N)表示n_1,n_2,\dots ,n_N的最小公倍数$

$M_i=M/n_i(i=1,2,\dots ,N);I_{n_i,j}是n\ast nM维矩阵，由M块n维矩阵构成，其中j\ast n_i块为单位矩阵$

状态分块的思想是将融合周期（大周期）为M时间区间内的所有采集到的数据看做一个数据块，通过对状态进行扩维的方法统一进行融合，原先是nx1维，扩维后是nMx1维，这种方法村在的问题是由于融合周期时间较长所以在状态估计上存在采集到的数据没有及时的进行融合噪声状态估计的时延。

算法推导

下面分别推导状态转移方程与观测方程：

通过利用状态转移方程代入，可以逐步乘转移矩阵得到下一步估计（同时记得加噪声项与控制项）

最后可以得到

将上述式子写成矩阵形式即可以得到重构的状态转移矩阵A与重构的G项以及$\Gamma$项。

下面推导观测方程

这里可能会造成迷惑，矩阵的维度应该是怎么样的，此处$X(k)为nM\ast 1维的，前面的长矩阵为n\ast nM维的，一共有M_i项$

此处$I_{n_i}$的作用可能难以理解，这里$I_{n_i}$的作用是选择$i$传感器得到的数据，因为$i$传感器的采集间隔为$n_i$ 这样单位矩阵放在最后就取到了第$n_i$块数据，也就算了第$n_i$块状态向量，即为传感器$i$采集到的数据，将$I_{n_i}$与0矩阵块结合可以提取第$(k-1)M_i+j$次采集的数据。


从上述可以推得$\overline{C_i}$应该是$M_{i}s\ast nM维矩阵$结合了$C_i$与$I_{n_i}$为对角线构成的方阵。即为重构的$\overline{C_i}$。
得到了重构的各项结合kalman滤波。

滤波算法

分别对每个传感器在每个融合周期进行标准Kalman滤波（算法略）并且在每个融合周期进行多个传感器的数据融合，对多个传感器的估计进行融合（采用上述式子）。

仿真实例

此处算法写成了函数形式，仿真可以自行设定，主函数（仿真是简单的追踪加速度轨迹）在下篇给出。

function [k_v1,k_v2] = method1(A,C1,C2,P0,Qk,R1,R2,X0,n1,n2,M,N,Z1,Z2)
%
% 
M1=M/n1;
M2=M/n2;
%% A1阵的建立
A1=cell(M,M);
for i=1:M
    for j=1:M
        if j==M
    A1(i,j)={power(A,i)};
        else
          A1(i,j)={zeros(size(A,1),size(A,2))};
        end
    end
end
A1=cell2mat(A1);
%% C帽矩阵的建立
c1=cell(M1,1);
c2=cell(M2,1);
for i=1:M1
    c1(i)={C1*creat_Ini(2,n1,i,M)};   %书上存在错误c帽矩阵不是对角线矩阵
end
for i=1:M2
   c2(i)= {C2*creat_Ini(2,n2,i,M)};
end
c1=cell2mat(c1);
c2=cell2mat(c2);
%% P阵的建立
P=cell(M,M);
for i=1:M
    for j=1:M
        if j==M&&i==M
            P(i,j)={P0};
        else
            P(i,j)={zeros(2,2)};
        end
    end
end
P=cell2mat(P);
%% Q阵的建立
Q=cell(M,M);
for i=1:M
    for j=1:M
        if i==j
            Q(i,j)={Qk};
        else
            Q(i,j)={zeros(2,2)};
        end
    end
end
Q=cell2mat(Q);
%%  R量测噪声矩阵
R_1=buildmat(R1,M/n1);
R_2=buildmat(R2,M/n2);%R帽的生成  
%% X状态矩阵
X=cell(1,N/M);
X(1)={X0};
%% 滤波
k=1;

while k<=49
   
        temp = A1*cell2mat(X(1,k));        %状态推测的转移
        P_new = A1*P*A1'+Q;               %对状态不确定新规定转移
        %传感器1
        Kk1 = P_new*c1'/(c1*P_new*c1'+R1);         %  传感器1  卡尔曼系数k的确定
        temp_1= temp+Kk1*(cell2mat(Z1(k+1))'-c1*temp);     %传感器1  对下一阶段进行推测
        P1= (eye(4,4)-Kk1*c1)*P_new;                %传感器1  的不确定阵的更新
        %传感器2
        Kk2 = P_new*c2'/(c2*P_new*c2'+R2);         %  传感器2  卡尔曼系数k的确定
        temp_2= temp+Kk2*(cell2mat(Z2(k+1))-c2*temp);     %传感器2  对下一阶段进行推测
        P2= (eye(4,4)-Kk2*c2)*P_new;                %传感器2  的不确定阵的更新
       %对传感器1，2的融合
        a1= inv(inv(P1)+inv(P2))*inv(P1);
        a2=inv(inv(P1)+inv(P2))*inv(P2);
        X(k+1)={a1*temp_1+a2*temp_2};
        P=inv(inv(P1)+inv(P2)); 
       k=k+1;
end
%%  性能观测
%实际值  v1  1x100   v2 1x50
%对估计值的提取
etr1=[1 0 0 0
    0 0 1 0];         %提取k_v1
etr2=[0 0 1 0];       %提取k_v2
k_v1=cell(1,N/M);
k_v2=cell(1,N/M);
for i=1:N/M
    k_v1(i)={(etr1*cell2mat(X(i)))'};
    k_v2(i)={etr2*cell2mat(X(i))};
    
end

k_v1=cell2mat(k_v1);
k_v2=cell2mat(k_v2);   %提取出估计值   k_v1  1x100    k_v2  1x50

end

function [Ini] =creat_Ini(a,n,j,M)
Ini=cell(1,M);
eye1=eye(a,a);
for i=1:M
    if i==j*n
        Ini(1,j*n)={eye1};
    else
    Ini(1,i)={zeros(a,a)};
    end
end
Ini=cell2mat(Ini);
end

function [T] = buildmat(A,Q)
%输入参数为：A矩阵与A矩阵在对角线上的个数
%输出参数为：对A矩阵在对角线上排列后的个数
[m,n]=size(A);%取A矩阵的行列数
B=zeros(m,n);%建立填充的零矩阵
P=cell(Q);%建立QXQ的元胞数组
for i=1:Q
    for j=1:Q
        if i==j%对角线上赋A
            P(i,j)={A};
        else%其余赋0
            P(i,j)={B};
        end
    end
T=cell2mat(P);%将元胞数组转化为矩阵
end