kalmanra2.m

本程序是AR(2)模型进行数据建模

%光纤陀螺零漂数据的卡尔曼滤波法
%利用AR模型参数的最小二乘估计计算陀螺采样数据的fai1,fai2及系统的过程噪声方差dsf
clear
clc
format long
b=dlmread('tt.txt');%读数据文件

ax=b(:,3)*1000;
avax=mean(ax)
n=size(ax);%取得数据长度
for i=1:n
    ax(i)=ax(i)-avax;
end

for i=3:n
    k=i-2;
    y(k)=ax(i);%截取部分数据y
end

m=size(y);
y=y';
for i=2:(n-1)%截取部分数据x1
    l=i-1;
    x1(l)=ax(i);
end

for i=1:(n-2)%截取部分数据x2
    l=i;
    x2(l)=ax(i);
end

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

fai=inv(x'*x)*x'*y;%计算fai1,fai2
fai=fai';
dsf=y'*y-y'*x*inv(x'*x)*x'*y;%计算过程噪声方差

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];%过程噪声方差
r=dm*dm;%观测噪声方差
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('原始数据图')
% grid on
% subplot(312)
% plot(zz)
% title('滤波后数据图')
% grid on
% subplot(313)
% plot(xx)
% title('噪声图')
% grid on
% avzz=mean(zz)
% figure%画原数据图
% plot(ax)
% figure%画虑波后图
% 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)