%動力調諧陀螺零漂數據的卡爾曼濾波法
%利用AR模型參數的最小二乘估計
clear
clc

%load DriftX2005-10-10.txt;
load S-10F05.DAT;
x=S_10F05;
Data=x(:,2);
%load D1.txt;
%Data=DriftX2005_10_10(1:1000);
%Data=D1(1:1000);
% load DriftX2005-10-28.txt;
% Data=DriftX2005_10_28(1:1000);

yTrue=Data;
N=size(yTrue,2);   %有問題

A=[0.5204 0.4783;1 0];%10-10
% A=[0.2067 -0.02809;1 0];%10-10-ARMA
% A=[0.4734 0.5219;1 0];%10-28
% A=[0.4677 0.5278;1 0];%11-15

B=[1 0; 0 1];%過程噪聲矩陣
% B=[1 -0.1801; 0 0];%過程噪聲矩陣
C=[1 0];%量測矩陣

I=[1 0;0 1];

xEst=[0.07;0.07];
Dim=size(xEst,1);
pEst=0.001*eye(Dim,Dim);

Q=0.00007*eye(Dim,Dim);%過程噪聲方差
R=0.00005;%觀測噪聲方差
z=zeros(1,N);

for i=1:N
    xPred=A*xEst;
    pPred=A*pEst*A'+B*Q*B';
    K=pPred*C'*1/(C*pPred*C'+R);
    pEst=(I-K*C)*pPred;
    xEst=xPred+K*(yTrue(i)-C*xPred);
    z(i)=C*xEst;  
end


%==================采用Allan方差評濾波性能====================

kk=4; %最小二乘法下線性方程組的個數

for k=1:kk
    numSeq=k*30;%子序列的數目
    M=floor(N/numSeq);%每段子序列中包含的樣本數
    sFreq=N/24/3600;%采樣頻率   
    T(k)=M/sFreq;%相關時間
    %濾波前
    eachSeq_Mean=zeros(1,numSeq);
    eachSeq_Mean(1)=mean(yTrue(1:M));%第一個子序列的均值
    WCov=0;
    %濾波后
    eachSeq_Mean_Filter=zeros(1,numSeq);
    eachSeq_Mean_Filter(1)=mean(z(1:M));%第一個子序列的均值
    WCov_Filter=0;
    
    %***********計算Allan方差*****************
    for i=1:numSeq-1 
        %濾波前
        eachSeq_Mean(i+1)=mean(yTrue(1+M*i:M*(i+1)));%第i個子序列的均值
        WCov=WCov+(eachSeq_Mean(i+1)-eachSeq_Mean(i)).^2;
        %濾波后
        eachSeq_Mean_Filter(i+1)=mean(z(1+M*i:M*(i+1)));%第i個子序列的均值
        WCov_Filter=WCov_Filter+(eachSeq_Mean_Filter(i+1)-eachSeq_Mean_Filter(i)).^2;
    end
    w(k)=WCov/(2*(numSeq-1));%%濾波前Allan方差
    wFilter(k)=WCov_Filter/(2*(numSeq-1));%%濾波后Allan方差
end
for i=1:numSeq-1
    s1(i)=(eachSeq_Mean(i+1)-eachSeq_Mean(i)).^2;
    s2(i)=(eachSeq_Mean_Filter(i+1)-eachSeq_Mean_Filter(i)).^2;
end

%*************最小方差意義下的Allan方差系數A1 A2 A3***********
% sqrt(WCov_Filter)=A0+A(-1)/sqrt(T)+A(-2)/T;
%A(-2)->aCofficent(1);  A(-1)->aCofficent(2);  A(0)->aCofficent(3);
x1=1./T;
x2=1./sqrt(T);
xMatrix=[x1' x2' ones(kk,1)];
aCofficent=xMatrix\sqrt(w(:));%濾波前
aCofficent_Filter=xMatrix\sqrt(wFilter(:));%濾波后
% aCofficent_Filter=inv(xMatrix'*xMatrix)*xMatrix'*sqrt(wFilter(:));%濾波后

%濾波前
QVAR=abs(aCofficent(1)/1.732);
RandError=abs(aCofficent(2)/60);
zeroUnstable=abs(aCofficent(3)/0.6643);
%濾波后
QVAR_Filter=abs(aCofficent_Filter(1)/1.732);
RandError_Filter=abs(aCofficent_Filter(2)/60);
zeroUnstable_Filter=abs(aCofficent_Filter(3)/0.6643);


disp('************* original drifting measurement *****************');
disp(['measurement noise     =  ' num2str(QVAR)]);
disp(['random drifting       =  ' num2str(RandError)]);
disp(['zero unstable         =  ' num2str(zeroUnstable)]);
disp(' ');

disp('*************drifting after filtering  *****************');
disp(['measurement noise     =  ' num2str(QVAR_Filter)]);
disp(['random drifting       =  ' num2str(RandError_Filter)]);
disp(['zero unstable         =  ' num2str(zeroUnstable_Filter)]);
disp(' ');
%==============================plot=============================
figure(1);
clf
P1=plot(yTrue,'r:');hold on;
P2=plot(z,'b-','linewidth',2);hold off;
legend([P1 P2 ],'True measurement','Kalman filter');
figure(2);
clf
P3=plot(s1,'r:');hold on;
P4=plot(s2,'b:');hold off;
legend([P3 P4],'True measurement','Kalman filter');

ylabel('陀螺x軸漂移（度/小時）');