%光纖陀螺零漂數(shù)據(jù)的卡爾曼濾波法
%利用AR模型參數(shù)的最小二乘估計(jì)計(jì)算陀螺采樣數(shù)據(jù)的fai1,fai2及系統(tǒng)的過(guò)程噪聲方差dsf
clear
clc
format long
b=dlmread('tt.txt');%讀數(shù)據(jù)文件

ax=b(:,3)*1000;
avax=mean(ax)
n=size(ax);%取得數(shù)據(jù)長(zhǎng)度
for i=1:n
    ax(i)=ax(i)-avax;
end

for i=3:n
    k=i-2;
    y(k)=ax(i);%截取部分?jǐn)?shù)據(jù)y
end

m=size(y);
y=y';
for i=2:(n-1)%截取部分?jǐn)?shù)據(jù)x1
    l=i-1;
    x1(l)=ax(i);
end

for i=1:(n-2)%截取部分?jǐn)?shù)據(jù)x2
    l=i;
    x2(l)=ax(i);
end

x=[x1   
    x2];%形成x陣
x=x';

fai=inv(x'*x)*x'*y;%計(jì)算fai1,fai2
fai=fai';
dsf=y'*y-y'*x*inv(x'*x)*x'*y;%計(jì)算過(guò)程噪聲方差

fai1=fai(1)%卡爾曼濾波賦初值
fai2=fai(2)
da=dsf(1)/(n(1)-2)%5969
dm=0.2;
a=[ fai1,   fai2  ;  1 , 0];
b=[1 ,0 ;  0 ,0;];
I=[1,0;0,1];
p=[1,0;0,1];
x=[0;0];
q=[da,0;0,da];%過(guò)程噪聲方差
r=dm*dm;%觀測(cè)噪聲方差
c=[1,0];

for i=1:n
    z(i)=ax(i);
xk_1=a*x;
pk_1=a*p*a'+b*q*b';
k=pk_1*c'*1/(c*pk_1*c'+r);
p=(I-k*c)*pk_1;
x=xk_1+k*(z(i)-c*xk_1);
zz(i)=c*x;
end
for i=1:n
    zz(i)=zz(i)+avax;
end
zz=zz';
for i=1:n
    ax(i)=ax(i)+avax;
end
xx=ax-zz;
figure
 plot(1:n,ax,'r',1:n,zz,'b')
% figure
% subplot(311)
% plot(ax)
% title('原始數(shù)據(jù)圖')
% grid on
% subplot(312)
% plot(zz)
% title('濾波后數(shù)據(jù)圖')
% grid on
% subplot(313)
% plot(xx)
% title('噪聲圖')
% grid on
% avzz=mean(zz)
% figure%畫(huà)原數(shù)據(jù)圖
% plot(ax)
% figure%畫(huà)慮波后圖
% plot(zz)
% 
% 
% figure
% 
% for i=1:30%濾波放大圖
%     aa(i)=zz(i)
% end
% plot(aa)
% zz=zz'
% xx=ax-zz;
% t=max(abs(xx))
% figure
% 
% plot(xx)
% mean(ax)
% 
% mean(zz)