%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%思路：產生一組信號，用兩個傳感器去測量，之后分別通過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=