%Allan方差分析
load xia826b0.dat;
c=xia826b0(1000:5000).';
NA=length(c);

K=NA/4;
for (i=1:K)
    b_aver(i)=mean(c(1:i:NA));
end

for m=1:K-1
    si=0.0;
    for j=1:K-m
        si=si+(b_aver(j+m)-b_aver(j))^2;
    end
    sigma2(m)=si/(K-m)/2;
end
figure(4);
subplot(2,1,1);
loglog(1:K-1,sqrt(sigma2(1:K-1)));

load  xia.dat;
b=xia.';
b0=b-mean(b);
N=length(b0);
figure(1)
hist(b);

 %AR模型參數(shù)最小二乘估計（2階）
  %方法參看《測量數(shù)據(jù)建模與參數(shù)估計》第210頁，王正明 易東云，國防科大出版社，1996.7
 jieci=2;  %階次
 Y=b0(jieci+1:N).';
 for i=1:(N-jieci)
     for j=1:jieci
       X(i,j)=b0(i+jieci-j);
     end
 end 
fai=((X.'*X)^(-1))*(X.')*Y;
sigma2=(Y'*Y-Y'*X*(inv(X'*X))*X'*Y)/(N-jieci);%參數(shù)意義參看《測量數(shù)據(jù)建模與參數(shù)估計》，第210頁



%小波分解
[c1,l] = wavedec(b0,5,'db3');
a5 = wrcoef('a',c1,l,'db4',5);
figure(2)
subplot(6,2,1)
plot(b0);
title('原始信號及各層重構(gòu)信號低頻');
Ylabel('b');
subplot(6,2,2)
plot(b0);
title('原始信號及各層重構(gòu)信號高頻');
Ylabel('b');

%對分解結(jié)構(gòu)[c1,l]中的各低頻部分進行重構(gòu)，并顯示結(jié)果
for iii=1:5
    decmp=wrcoef('a',c1,l,'db3',6-iii);
    subplot(6,2,2*iii+1);
    plot(decmp);
    Ylabel(['a',num2str(6-iii)]);
end
%高頻
for iii=1:5
    decmp=wrcoef('d',c1,l,'db3',6-iii);
    subplot(6,2,2*iii+2);
    plot(decmp);
    Ylabel(['d',num2str(6-iii)]);
end


%下面對b01.dat進行濾波
load xia826b0.dat;
Z=(xia826b0(2000:length(xia826b0))).';
N2=length(Z);

%參數(shù)設(shè)定
A=[2+fai(1),-1-2*fai(1)+fai(2),fai(1)-2*fai(2),fai(2);
   1,0,0,0;
   0,1,0,0;
   0,0,1,0];
B=[1 0 0 0;
   0 0 0 0;
   0 0 0 0;
   0 0 0 0; ];
H=[1,0,0,0];

%初值選擇
X0((1:4),1)=[0,0,0,0].';
P0=zeros(4,4,N2);
K=zeros(4,1,N2);
P0(:,:,1)=[1 0 0 0 
      0 1 0 0 
      0 0 1 0 
      0 0 0 1 ];            %可以為序列的方差
P1=zeros(4,4,N2);
Q=[sigma2 0 0 0 
     0 sigma2 0 0 
     0 0 sigma2 0 
     0 0 0 sigma2];
R=sigma2;

%濾波的遞推計算
for k=2:N2
    P1(:,:,k)=A*P0(:,:,k-1)*A.'+B*Q*B.';                     %P1(k)=P(k|k-1),P0(k)=P(k|k)
    K(:,:,k)=P1(:,:,k)*H.'*inv(H*P1(:,:,k)*H.'+R);           %R陣為0，所以簡化為此式，下同
    X1((1:4),k)=A*X0((1:4),k-1);                             %X1(k)=X(k|k-1),X0(k)=X(k|k)
    X0((1:4),k)=X1((1:4),k)+K(:,:,k)*(Z(k)-H*X1((1:4),k));
    P0(:,:,k)=(eye(4,4)-K(:,:,k)*H)*P1(:,:,k)*(eye(4,4)-K(:,:,k)*H).'+K(:,:,k)*R*K(:,:,k).';   %eye(3,3)為3階單位陣
end

%for k=1:N2
 %  P(k|k-1)=A*P(k-1|k-1)*A.'+B*Q(k-1)B.';
 %  K(k)=P(k|k-1)*H(k)'*[H(k)*P(k|k-1)*H(k).'+R(k)]^(-1);
 %  X(k|k-1)=A*X(k-1|k-1);
 %  X(k|k)=X(k|k-1)+K(k)*[Z(k)-H(k)*X(k|k-1)];
 %  P(k|k)=[I-K(k)*H(k)]*P(k|k-1)*[I-K(k)*H(k)].'+K(k)*R(k)*K(k).';
%end

Y1=H*X0;%;濾波輸出值
figure(3);
subplot(2,1,1)
plot(Z(10:length(Z)));
subplot(2,1,2)
plot(Y1(10:length(Y1)));


%計算漂移
bs1=sqrt(var(Z(100:5000)));
bs2=sqrt(var(Y1(100:5000)));

dd=Y1(1000:5000);
NA1=length(dd);
K=NA1/4;
for (i=1:K)
    b_aver(i)=mean(dd(1:i:NA1));
end

for m=1:K-1
    si=0.0;
    for j=1:K-m
        si=si+(b_aver(j+m)-b_aver(j))^2;
    end
    sigma21(m)=si/(K-m)/2;
end
figure(4);
subplot(2,1,2);
loglog(1:K-1,sqrt(sigma21(1:K-1)));