ziji.rar_Allan方差_allan方差计算_直线拟合

clear all; A=xlsread('xxa.xls')'; %原始数据 l=length(A); b=0*ones(1,l); %提取常值 li=l-29; for j=1:li n=j; m=n+29; for i=n:m b(1,j)=A(1,i)+b(1,j); end b(1,j)=b(1,j)/30; end for i=1:li C(1,i)=A(1,(29+i)); end for i=1:li %除去常值 M(1,i)=C(1,i)-b(1,i); end e=mean(M'); vara=var(M'); % b=0*ones(1,l); %求残差 for j=2:li z(1,j)=A(1,j+1)-A(1,j); end varb=var(z'); Q=varb/1000; R=vara/10; % Q=2.7115e-007/100; % R=2.7115e-007; w=0+sqrt(Q)*randn(1,l); v=0+sqrt(R)*randn(1,l); x(1,1)=e; P(1)=5; % for i=1:l % x(1,i+1)=x(1,i)+w(i); % % y(1,i)=x(1,i)+v(i); % end a=0.99; Rr(1)=R; for i=1:li-1 x1(1,i)=x(1,i); %状态一步预测 P1(i)=P(i)+Q; %一步预测方差 ee(i+1)=M(1,i+1)-x1(1,i)+v(i+1); %新息 Dk=(1-a)/(1-a^(i+1)); % Rr(i+1)=(1-Dk)*Rr(i)+Dk*(ee(i+1)*ee(i+1)-P(i)); K=P1(i)/(P1(i)+Rr(i+1)); %滤波增益阵 dk=P(i)+Q; tk=ee(i+1)'*P(i)*ee(i+1); UU=K*dk*K'; [V1,D1]=eig(UU); r=max(max(D1)); ck=sqrt(0.004*r); ss=ck^2/r; if tk<=ss S(i+1)=1; else S(i+1)=ck/sqrt(tk*r); end x(1,i+1)=x1(1,i)+K*S(i+1)*ee(i+1); %状态估计 抗野值 %x(1,i+1)=x1(1,i)+K*ee(i+1); %状态估计 P(i+1)=P1(i)-K*P1(i); %方差更新 end c=mean(A'); Mean=mean(x')+c Varx=var(x') t1=1:li; t2=1:length(x); figure(1); plot(A); title('原始漂移信号'); figure(2); plot(M); title('原始漂移信号'); figure(7); plot(t1,M+c,'r',t2,x+c,'b'); title('比较'); figure(4); plot(x); title('滤波后'); figure(5); plot(P); title('误差');