%建立ARMA21模型,基于ARMA21模型卡爾曼濾波
close all
clear
clc
tic
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%讀數據
%%
disp('>>>>>>>>>>讀數據.....')
load('V1_Pretreat30000.mat');
V1_Pretreat30000=data1;
DATA=V1_Pretreat30000(1:3000,1);      %預處理后數據

LEN=size(DATA,1); 
T=1/300;                    %采樣時間
t=T:T:LEN*T;
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立ARMA21模型
%%
n=2;m=1;                    %ARMA模型階次
%%%%%%%%%%%%%%%%%%%%%%%%%%%%第一步:計算AR模型參數,－－－－－－－－－用前六個時刻的值去擬合下一時刻的輸出：－－－－
p=m+n+3;                    %AR階次
for i=p:LEN-1
    X(i-p+1,:)=flipud(DATA(i-p+1:i));
end
Y=DATA(p+1:LEN);

FAI=inv(X'*X)*X'*Y;         %最小二乘法求AR參數
disp(sprintf('AR6模型參數=[%0.4g;%0.4g;%0.4g;%0.4g;%0.4g;%0.4g]',.....
             FAI(1),FAI(2),FAI(3),FAI(4),FAI(5),FAI(6)));%n
AR_OUT=X*FAI;               %AR模型擬合值
Ae=Y-AR_OUT;                %擬合殘差
AR_OUT=[DATA(1:p);AR_OUT];  %AR模型擬合值
Ae=[zeros(p,1);Ae];         %擬合殘差

figure;
subplot(2,1,1);
plot(t,DATA,'.r');%grid
hold on;
plot(t,AR_OUT,'b');
ylabel('數據幅值/^o/s');legend('實際值','擬合值')
hold off;
subplot(2,1,2);
plot(t,Ae,'--c');%grid
ylabel('擬合殘差/^o/s');xlabel('時間/s'),
figure
psd(AR_OUT);
xlabel('頻率/Hz'),ylabel('功率譜密度/dB');
%%%%%%%%%%%%%%%%%%%%%%%%%%%第二步:計算ARMA模型參數
for i=n+1:LEN
    X1(i-n,:)=[flipud(DATA(i-n:i-1));flipud(Ae(i-m:i-1))]';
end
Y1=DATA(n+1:LEN);         

FAI1=inv((X1)'*(X1))*(X1)'*Y1;   %ARMA模型參數
disp(sprintf('ARMA21模型參數=[%0.4g;%0.4g;%0.4g]',FAI1(1),FAI1(2),FAI1(3)));%n
%%%%%%%%%%%%%%%%%%%%%%%%%%%
ARMA_OUT=X1*FAI1;                %ARMA模型擬合值
ARMA_OUT=[DATA(1:n);ARMA_OUT];   
A1=DATA-ARMA_OUT;                %擬合殘差
A1_mean=mean(A1);                %殘差均值
A1_var=var(A1);                  %殘差方差
disp(sprintf('擬合殘差均值=%0.4g',A1_mean));
disp(sprintf('擬合殘差方差=%0.4g',A1_var));

DATA1=DATA(1:n);
for i=n+1:LEN
    DATA1(i,1)=[flipud(DATA1(i-n:i-1));flipud(Ae(i-m:i-1))]'*FAI1;
end
W=DATA-DATA1;                   %預報誤差
W_var=var(W);                   %誤差方差
W_mean=mean(W);                 %誤差均值
disp(sprintf('預報誤差均值=%0.4g',W_mean));
disp(sprintf('預報誤差方差=%0.4g',W_var));
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
plot(t,A1*1);%grid
ylabel('擬合殘差/^o/s');xlabel('時間/s'),
figure;
plot(t,DATA*1,'b');hold on;
plot(t,ARMA_OUT*1,'--c');hold off;%grid
ylabel('數據幅值/^o/s');legend('實際值','擬合值')
figure
psd(A1);grid off
xlabel('頻率/Hz'),ylabel('功率譜密度/dB');
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%計算Allan方差&標準差
%%
DATA=A1;  
LEN=size(DATA,1);
%%%%%%%%%%%%%%%%%%%%%
Lm=floor(LEN/2-1);        %最大組長度
for L=1:Lm                %組長度
    Num=floor(LEN/L);     %組個數
    for N=1:Num      
        Wa(L,N)=mean(DATA((N-1)*L+1:N*L));        %各組均值序列
    end
    for j=1:(Num-1)  
        Aa(j)=(Wa(L,j+1)-Wa(L,j))^2;              %前后項差序列
    end
    Allan_var(L,1)=sum(Aa(1:(Num-1)))/(2*(Num-1));%Allan方差
    Allan_std(L,1)=sqrt(Allan_var(L));           %Allan標準差
end
%%%%%%%%%%%%%%%%%%%%%辨識噪聲源系數
X=zeros(Lm,5);
tao0=1/300;
for i=1:Lm
    tao(i)=i*tao0;                         %二次采樣相關時間
    X(i,:)=[sqrt(tao(i))^2,sqrt(tao(i)),1,sqrt(tao(i))^(-1),sqrt(tao(i))^(-2)];
end
C=inv(X'*X)*X'*Allan_std;               %矩陣左除法擬合各噪聲源系數
R=3600*3600*sqrt(2)*abs(C(1));             %速率斜坡R
K=3600*60*sqrt(3)*abs(C(2));               %速率隨機游走K
B=3600*abs(C(3))/0.6643;                   %零偏不穩定性B
N=3600*abs(C(4))/60;                       %角度隨機游走N
Q=10^6*pi*abs(C(5))/(180*sqrt(3));         %量化噪聲Q
disp(sprintf('速率斜坡:   R=%0.5g°/h^2',R));
disp(sprintf('速率隨機游走:K=%0.5g°/h^(3/2)',K));
disp(sprintf('零偏不穩定性:B=%0.5g°/h',B));
disp(sprintf('角度隨機游走:N=%0.5g°/h^(1/2)',N));
disp(sprintf('量化噪聲:   Q=%0.5gμrad',Q));
%%%%%%%%%%%%%%%%
figure
loglog(tao,Allan_std),
xlabel('相關時間/s'),ylabel('Allan標準差');
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%卡爾曼濾波
Q=A1_var*eye(n);                     %系統誤差方差
R=W_var;                             %量測誤差方差
A=[FAI1(1:n)';eye(n-1,n-1),zeros(n-1,1)];%轉移矩陣
B=[1,FAI1(n+1:n+m)';zeros(n-1,m+1)]; %噪聲矩陣
C=[1,zeros(1,n-1)];                  %量測矩陣
Pk=eye(n);                           %初始估計誤差方差
I=eye(n);    
Xk=flipud(DATA(1:n));                %初始狀態
Zk(1:n,1)=DATA(1:n,1);    
for i=1:LEN
    Pk1=A*Pk*A'+B*Q*B';              %狀態一步預測誤差方差
    Xk1=A*Xk;                        %狀態一步預測
    Kk=Pk1*C'*inv(C*Pk1*C'+R);       %濾波增益
    Pk=(I-Kk*C)*Pk1;                 %狀態估計誤差方差
    Xk=Xk1+Kk*(DATA(i)-C*Xk1);       %狀態估計
    Zk(i,1)=C*Xk;                    %濾波輸出
    PP1(i,1)=Pk(1,1);  
    PP2(i,1)=Pk(2,2);
    
end   
ARMA_KFe=DATA-Zk;                    %估計誤差
ARMA_KFe_var=var(ARMA_KFe);          %估計誤差方差
ARMA_KFe_mean=mean(ARMA_KFe);        %估計誤差均值
disp(sprintf('估計誤差均值=%0.4g',ARMA_KFe_mean));
disp(sprintf('估計誤差方差=%0.4g',ARMA_KFe_var));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%save ARMA21_KFe.mat ARMA_KFe Zk
figure;
plot(t,DATA*1,'b');hold on;;ylabel('數據幅值/^o/s');
plot(t,Zk*1,'--c');legend('實際值','估計值');%grid;
figure;
plot(t,ARMA_KFe*1);%grid
xlabel('時間/s'),ylabel('估計誤差/^o/s');
% figure
% plot(t,PP1);%grid%估計誤差方差
% ylabel('狀態1估計誤差方差');xlabel('時間/s');
% figure;
% plot(t,PP2);%grid
% ylabel('狀態2估計誤差方差');xlabel('時間/s');
figure
psd(ARMA_KFe);grid off
xlabel('頻率/Hz'),ylabel('功率譜密度/dB');
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%自適應卡爾曼濾波
% Q=A1_var*eye(n);                     %系統誤差方差
% R=W_var;                             %量測誤差方差
A=[FAI1(1:n)';eye(n-1,n-1),zeros(n-1,1)];%轉移矩陣
% B=[1,FAI1(n+1:n+m)';zeros(n-1,m+1)]; %噪聲矩陣
H=[1,zeros(1,n-1)];                  %量測矩陣
% Pk=eye(n);                           %初始估計誤差方差
I=eye(n);    
Xk=flipud(DATA(1:n));                %初始狀態
Zk(1:n,1)=DATA(1:n,1);   
C0=mean(ZK.^2);                        %量測的自相關函數初值
A=[H*A;H*A^2];
for i=1:LEN
%     Pk1=A*Pk*A'+B*Q*B';              %狀態一步預測誤差方差
    Xk1=A*Xk;                        %狀態一步預測
    Kk=Pk1*H'*inv(H*Pk1*H'+R);       %濾波增益
    Pk=(I-Kk*H)*Pk1;                 %狀態估計誤差方差
    Xk=Xk1+Kk*(DATA(i)-H*Xk1);       %狀態估計
    Zk(i,1)=H*Xk;                    %濾波輸出
    PP1(i,1)=Pk(1,1);  
    PP2(i,1)=Pk(2,2);
    
end   
ARMA_KFe=DATA-Zk;                    %估計誤差
ARMA_KFe_var=var(ARMA_KFe);          %估計誤差方差
ARMA_KFe_mean=mean(ARMA_KFe);        %估計誤差均值
disp(sprintf('估計誤差均值=%0.4g',ARMA_KFe_mean));
disp(sprintf('估計誤差方差=%0.4g',ARMA_KFe_var));
figure;
plot(t,DATA*1,'b');hold on;;ylabel('數據幅值/^o/s');
plot(t,Zk*1,'--c');legend('實際值','估計值');%grid;
figure;
plot(t,ARMA_KFe*1);%grid
xlabel('時間/s'),ylabel('估計誤差/^o/s');
% figure
% plot(t,PP1);%grid%估計誤差方差
% ylabel('狀態1估計誤差方差');xlabel('時間/s');
% figure;
% plot(t,PP2);%grid
% ylabel('狀態2估計誤差方差');xlabel('時間/s');
figure
psd(ARMA_KFe);grid off
xlabel('頻率/Hz'),ylabel('功率譜密度/dB');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%計算Allan方差&標準差
DATA=ARMA_KFe;
LEN=size(DATA,1); disp(sprintf('數據長度=%d',LEN));
%%%%%%%%%%%%%%%%%%%%%
Lm=floor(LEN/2-1);        %最大組長度
for L=1:Lm                %組長度
    Num=floor(LEN/L);     %組個數
    for N=1:Num      
        Wa(L,N)=mean(DATA((N-1)*L+1:N*L));        %各組均值序列
    end
    for j=1:(Num-1)  
        Aa(j)=(Wa(L,j+1)-Wa(L,j))^2;                %前后項差序列
    end
    Allan_var(L,1)=sum(Aa(1:(Num-1)))/(2*(Num-1));%Allan方差
    Allan_std(L,1)=sqrt(Allan_var(L));           %Allan標準差
end
%%%%%%%%%%%%%%%%%%%%%辨識噪聲源系數
X=zeros(Lm,5);
tao0=1/300;
for i=1:Lm
    tao(i)=i*tao0;                         %二次采樣相關時間
    X(i,:)=[sqrt(tao(i))^2,sqrt(tao(i)),1,sqrt(tao(i))^(-1),sqrt(tao(i))^(-2)];
end
C=inv(X'*X)*X'*Allan_std;                  %矩陣左除法擬合各噪聲源系數
R=3600*3600*sqrt(2)*abs(C(1));             %速率斜坡R
K=3600*60*sqrt(3)*abs(C(2));               %速率隨機游走K
B=3600*abs(C(3))/0.6643;                   %零偏不穩定性B
N=3600*abs(C(4))/60;                       %角度隨機游走N
Q=10^6*pi*abs(C(5))/(180*sqrt(3));         %量化噪聲Q
disp(sprintf('速率斜坡:   R=%0.5g°/h^2',R));
disp(sprintf('速率隨機游走:K=%0.5g°/h^(3/2)',K));
disp(sprintf('零偏不穩定性:B=%0.5g°/h',B));
disp(sprintf('角度隨機游走:N=%0.5g°/h^(1/2)',N));
disp(sprintf('量化噪聲:   Q=%0.5gμrad',Q));
%%%%%%%%%%%%%%%%
figure
loglog(tao,Allan_std),%grid on;
xlabel('相關時間/s'),ylabel('Allan標準差');
%%
toc