www.pudn.com > kalmanRA2.rar > kalmanRA2.m

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