%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%思路：產(chǎn)生一組信號(hào)，用兩個(gè)傳感器去測(cè)量，之后分別通過kalaman濾波得到濾波后的數(shù)據(jù)，將兩組數(shù)據(jù)進(jìn)行簡(jiǎn)單凸組合融合，對(duì)比結(jié)果并分析。
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）;
%這里認(rèn)為兩個(gè)傳感器的過程噪聲是一樣的，測(cè)量噪聲不同；
%?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）;%無噪聲時(shí)的濾波軌跡
%?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是協(xié)方差矩陣;Xkf是濾波后的數(shù)據(jù)。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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）;%這里注意兩者的誤差矩陣是否相關(guān)？
P02=diag（[0.510.51]）;
for?i=