凸组合matlab详细代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%思路：产生一组信号，用两个传感器去测量，之后分别通过kalaman滤波得到滤波后的数据，将两组数据进行简单凸组合融合，对比结果并分析。
function main
clc;clear;
T=1;
N=80/T;

X=zeros（4N）;
X1=zeros（4N）;
X2=zeros（4N）;
X1（:1）=[-100220020];
X2（:1）=[-100220020];
Z=zeros（2N）;
Z1=zeros（2N）;
Z1（:1）=[X（11）X（31）];
Z2=zeros（2N）;
Z2（:1）=[X（11）X（31）];
delta_w=1e-2;
Q1=delta_w*diag（[0.510.51]） ;
R1=100*eye（2）;
%这里认为两个传感器的过程噪声是一样的，测量噪声不同；
% Q2=delta_w*diag（[0.510.51]）;
R2=80*eye（2）;

F=[1T00;0100;001T;0001];
H=[1000;0010];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for t=2:N
%     X（:t）=F*X（:t-1）;
%     Z（:t）=H*X（:t）;%无噪声时的滤波轨迹
% end
m=5000;
for j=1:m
for t=2:N
    X1（:t）=F*X1（:t-1）+sqrtm（Q1）*randn（41）;
    Z1（:t）=H*X1（:t）+sqrtm（R1）*randn（21）; %带噪声的轨迹
end
Xkf1=zeros（4N）;
Xkf1（:1）=X1（:1）;
P01=eye（4）;
for i=2:N
    Xn=F*Xkf1（:i-1）;
    P1=F*P01*F‘+Q1;
    K=P1*H‘*inv（H*P1*H‘+R1）;
    Xkf1（:i）=Xn+K*（Z1（:i）-H*Xn）;
    P01=（eye（4）-K*H）*P1;
end
%P0是协方差矩阵;Xkf是滤波后的数据。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
%     X2（:t）=F*X2（:t-1）+sqrtm（Q1）*randn（41）;
    Z2（:t）=H*X1（:t）+sqrtm（R2）*randn（21）;  %
end
Xkf2=zeros（4N）;
Xkf2（:1）=X1（:1）;
% P02=0.5*eye（4）;%这里注意两者的误差矩阵是否相关？
P02=diag（[0.510.51]）;
for i=