%动力调谐陀螺零漂数据的卡尔曼滤波法
%利用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轴漂移(度/小时)');