ts_jquan.asv

数据融合的问题

function[ABiaoL,ADiag,AMatrix,JiBiaoL,JiDiag,JiMatrix]= TS_JQuan(P11,P12,...
    P13,P21,P22,P23,P31,P32,P33);
%--------------------------------------------------------------------------
%                           三种加权算法仿真通式
%--------------------------------------------------------------------------
% 函数输入部分P11,P12,P13,P21,P22,P23,P31,P32,P33分别为稳态滤波误差互协方差阵
% 函数返回三种加权的加权系数，以及迹
% 本函数只适用于三传感器
%--------------------------------------------------------------------------
%--------------------------按标量加权信息融合算法----------------------------
Ps=[trace(P11),trace(P12),trace(P13);trace(P21),trace(P22),trace(P23);...
    trace(P31),trace(P32),trace(P33)];
e=[1 1 1]';
ABiaoL(1,1:3)=e'*inv(Ps)*inv(e'*inv(Ps)*e);
 sum1=ABiaoL(1,1)*[ABiaoL(1,1)*P11+ABiaoL(1,2)*P12+ABiaoL(1,3)*P13];
sum2=ABiaoL(1,2)*[ABiaoL(1,1)*P21+ABiaoL(1,2)*P22+ABiaoL(1,3)*P23];
sum3=ABiaoL(1,3)*[ABiaoL(1,1)*P31+ABiaoL(1,2)*P32+ABiaoL(1,3)*P33];
JiBiaoL=trace(sum1+sum2+sum3);
%--------------------------按对角阵加权-------------------------------------
Pd1(:,:)=[P11(1,1) P12(1,1) P13(1,1);P21(1,1) P22(1,1) P23(1,1);...
    P31(1,1) P32(1,1) P33(1,1)];
Pd2(:,:)=[P11(2,2) P12(2,2) P13(2,2);P21(2,2) P22(2,2) P23(2,2);...
    P31(2,2) P32(2,2) P33(2,2)];
ADiag(1,:)=e'*inv(Pd1)/[e'*inv(Pd1)*e];
ADiag(2,:)=e'*inv(Pd2)/[e'*inv(Pd2)*e];
DiagP1=inv(e'*inv(Pd1)*e);
DiagP2=inv(e'*inv(Pd2)*e);
JiDiag=DiagP1+DiagP2;
%-------------------------按矩阵加权----------------------------------------
Pm=[P11,P12,P13;P21,P22,P23;P31,P32,P33];
e=[eye(2);eye(2);eye(2)];
AMatrix=inv(e'*inv(Pm)*e)*e'*inv(Pm);
MatrixP0=inv(e'*inv(Pm)*e);
JiMatrix=trace(MatrixP0);