untitled.m

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

%X(i,1)-预测状态值；Y(i,1)-预测速度值；P1-实际测量状态值；P2-实际测量速度值；
%P-状态滤波误差；deta1-状态误差；deta2-速度误差；
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)
subplot(121);plot(fdata5);title('第五列原始数据');
subplot(122);plot(E5);title('第五列转换后的脉冲数据');
figure(2)
subplot(121);plotfdata6);title('第六列原始数据');
subplot(122);plot(E6);title('第六列转换后的脉冲数据');
figure(3)
subplot(121);plotfdata7);title('第七列原始数据');
subplot(122);plot(E7);title('第七列转换后的脉冲数据');
figure(4)
plot(fdata8);title('采样间隔');
for i=1:g-1
    T(i,1)=fdata8(i,1);
    Ts=sum(T);
    Ta=Ts/g;
end
P1=E5; 
for k=1:g-1; 
    for j=1:g-1
        for i=1:g-1
            Y(i,1)=(P1(i+1,1)-P1(i,1))/Ta;
            P2(i+1,1)=(E5(i+1,1)-E5(i,1))/fdata8(i,1);
            deta2(i,1)=Y(i,1)-P2(i,1);
            X(1,1)=E5(1,1);
            X(i+1,1)=X(i,1)+Ta*Y(i,1);
            deta1(i,1)=X(i,1)-P1(i,1);
            sigma=0.001; 
            alpha(i,1)=(deta1(i,1)+Ta*deta2(i,1))^2/(sigma+( deta1(i,1)+Ta*deta2(i,1))^2);
            beta(i,1)=(Ta*(deta1(i,1)*deta2(i,1))+Ta^2*(deta2(i,1)))/(sigma +( deta1(i,1)+Ta*deta2(i,1))^2); 
         end 
         X(j+1,1)=X(j,1)+Y(j,1)*Ta+alpha(j,1)*(P1(j+1,1)- X(j,1)-Y(j,1)*Ta);
         Y(j+1,1)=Y(j,1)+(beta(j,1)/Ta)*(P1(j+1,1)- X(j,1)-Y(j,1)*Ta);
    end
    Xs=sum(X);
    Xa=sum(X)/g;
    P(k)=P1(k,1)-X(k,1);
end
figure(5)
subplot(131);
plot(Y);title('预测速度');
subplot(132);
plot(P2);title('实际测量速度');
subplot(133);
plot(deta2);title('速度误差');
figure(6)
subplot(121);
plot(deta1);title('状态误差');
subplot(122);
plot(X),title('滤波后的数据显示');
figure(7)
plot(P),title('状态滤波误差');