fog_arma21_model_kf.m

基于ARMA（2）模型的光纤陀螺建模及kalma滤波算法

%建立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