zhuxilan_file.m

shi yi ge bu cuo de bi ye she ji ti mu

clear
clc
data0=load('d:\data1.dat');
g=10;
fdata5=data0(1:g,5);%读第五列1～g行数据
fdata6=data0(1:g,6);
fdata7=data0(1:g,7);
fdata8=data0(1:g,8);
for i=1:g
    sgx=7405.7793;
    sgy=7402.4513;
    sgz=7397.5328;
    W5(i)=(fdata5(i)/sgx)*pi/180;
    W6(i)=(fdata6(i)/sgy)*pi/180;
    W7(i)=(fdata7(i)/sgz)*pi/180;
    V5(i)=W5(i)*1000/fdata8(i);
    V6(i)=W6(i)*1000/fdata8(i);
    V7(i)=W7(i)*1000/fdata8(i);
    V{i}=[V5(i) V6(i) V7(i)];
    A=[0.027233,0.693126,-0.720301;
        0.577689,0.577147,0.577214;
        -0.815802,0.431829,0.384694];
    B{i}=V{i}*A;
end
for j=2:g
    B{j}=[B{j-1};B{j}];
end
ZZ=B{g};
tp2=5.6008e-5;
tp1=4.6687e-5;
tp3=0;
S=[tp2 tp1 tp3];
E5=ZZ(:,1);
E6=ZZ(:,2);
E7=ZZ(:,3);

figure(1)
plot(E5);title('第五列脉冲数据');
figure(2)
plot(E6);title('第六列脉冲数据');
figure(3)
plot(E7);title('第七列脉冲数据');
figure(4)
plot(fdata8);title('采样间隔');
g=10;
for i=1:g-1
    T(i,1)=fdata8(i,1);
    Ts=sum(T);
    Ta=Ts/g;
end
g=10;
P1=E5;
for i=1:g-1
    Y=P1(1,1)/Ta;
    P2(i+1,1)=(E5(i+1,1)-E5(i,1))/Ta;
    deta2(i,1)=Y-P2(i,1);
end

g=10;
for i=1:g-1
    X(1,1)=E5(1,1);
    X(i+1,1)=X(i,1)+Ta*Y;
    deta1(i,1)=X(i,1)-P1(i,1);
end

g=10 ;
beta(1,1)=1;
alpha(1,1)=1;
for i=1:g-1

    alpha(i,1)=(deta1(i,1)+Ta*deta2(i,1))^2/(0.001+( deta1(i,1)+Ta*deta2(i,1))^2);
    beta(i,1)=(Ta*(deta1(i,1)*deta2(i,1))+Ta^2*(deta2(i,1)))/(0.001+( deta1(i,1)+Ta*deta2(i,1))^2);
end


%    X(1,0)=0;
%    Y(1,0)=0;
%    alpha(1,0)=1;
%    beta(1,0)=1;
sigma=0.001;
for j=1:9;
    for i=1:g-1
        E5(i,1)=P1(i,1);
        X(i+1,1)=X(i,1)+Y*Ta+alpha(i,1)*(P1(i+1,1)- X(i,1)-Y*Ta);
%         Y(i+1,1)=Y(i,i)+beta(i,1)/(Ta* P1(i+1,1)- X(i,1)-Y*Ta);
    end
    P(j)=P1(j,1)-X(j,1);
end

figure(5)
plot(X),title('滤波后的数据显示');
figure(6)
plot(P),title('滤波误差');