h_speed1017.m

自己编写的kalman滤波算法的应用

function h_speed1017()
clc;
clear;
%Ts, maxcount, ae, an au, p_m, r_m, y_m, fp, fr, fy, 
%nav_data_m_fuc(0.01,2*600,  0,0,0,  0,0,0,  0,0,0   );

%nav_data_s_fuc(1,1,1);


%%%%%%%%%%%

%transfer alignment with my own method and dymamic equation ;



%由于安装误差角和姿态误差角的耦合影响，所以，是难以简单的区分的。

 %maxcount=20*60;
 
 o_miga=360.0/24*pi/180/3600;
 
 %fai=45*pi/180;
 
 %Ts=1;
 
 RR=6378137.0;
 
 g=9.8;
  
 Vx0=0;
 Vy0=0;
 

load data_m;
voutme=vout_e;
voutmn=vout_n;

rem=ratee;
rnm=raten;
rum=rateu;

manner=manner;

load data_s;
voutse=vout_e;
voutsn=vout_n;

T=0.1;

% 
 I10=eye(10);
 I5=eye(5);
 I8=eye(8);
% 
 H=[1,0,0,0,0,0,0,0; 0,1,0,0,0,0,0,0];
%  
% 
% %P0=diag([1*1,1*1,0.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, pi/180*pi/180,1*pi/180*pi/180, 1*pi/180*pi/180, 2*pi/180*2*pi/180]);%, 0.0001*g*0.0001*g, 0.0001*g*0.0001*g, 0.02*pi/180/3600*0.02*pi/180/3600, 0.02*pi/180/3600*0.02*pi/180/3600, 0.02*pi/180/3600*0.02*pi/180/3600]);
P0=diag([0.1*0.1,0.1*0.1,1*pi/180*1*pi/180, 1*pi/180*1*pi/180, 2*pi/180*2*pi/180,1*pi/180*1*pi/180, 1*pi/180*1*pi/180, 2*pi/180*2*pi/180]);
%P0=diag([0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1]);
Z=[0;0];
% 
R=diag([0.1*0.1,0.1*0.1]);%[0.00001*g*0.00001*g,0;0,0.00001*g*0.00001*g];
% 
Q=diag([0.001*g*0.001*g,0.001*g*0.001*g,0.1*pi/180/3600*0.1*pi/180/3600,0.1*pi/180/3600*0.1*pi/180/3600,0.1*pi/180/3600*0.1*pi/180/3600,0.01*pi/180*0.01*pi/180,0.01*pi/180*0.01*pi/180,0.01*pi/180*0.01*pi/180]);
%Q=diag([0.0005*g*0.0005*g,0.0005*g*0.0005*g,0.005*pi/180/3600*0.005*pi/180/3600,0.005*pi/180/3600*0.005*pi/180/3600,0.005*pi/180/3600*0.005*pi/180/3600,0.003*0.003,0.003*0.003,0.003*0.003]);

% 
%t=[1:maxcount*Ts]*vn;




X0=[0;0;0;0;0;0;0;0];


for i=1:maxcount*Ts*10
   
if manner==4
    fe=0;%0.05*vout_n(i)*sin(2.0*pi*i/100);
    fn=0.1*vout_n(i)*sin(2.0*pi*i/100);
    fu=aout_u(i);
else
   
    fe=aout_e(i);
    fn=aout_n(i);
    fu=aout_u(i);
end 
    cc11=c11(i);
    cc12=c12(i);
    cc13=c13(i);
    cc21=c21(i);
    cc22=c22(i);
    cc23=c23(i);
    cc31=c31(i);
    cc32=c32(i);
    cc33=c33(i);
    
    faii=fai(i);
    
    em=rem(i);
    nm=rnm(i);
    um=rum(i);
    
    
    Z(1)=-voutse(i)+voutme(i);
    Z(2)=-voutsn(i)+voutmn(i);
    
  A=[           0,                           (1.0*o_miga*sin(faii)+Vx0/RR*tan(faii)),           cc13*fn-cc12*fu,    -cc13*fe+cc11*fu ,  cc12*fe-cc11*fn,   -cc13*fn+cc12*fu,   cc13*fe-cc11*fu,  -cc12*fe+cc11*fn;
      (-1.0*o_miga*sin(faii)-Vx0/RR*tan(faii)),                   0,                            cc23*fn-cc22*fu,    -cc23*fe+cc21*fu,   cc22*fe-cc21*fn,   -cc23*fn+cc22*fu,   cc23*fe-cc21*fu,   -cc22*fe+cc21*fn;
             0,           0,                                                                0, um, -nm, 0,  -um, nm;
             0,           0,                                                               -um,  0, em, um, 0, -em;
             0,           0,                                                               nm, -em,  0, -nm, em, 0;
             0,           0,                                                               0,0,0,0,0,0;
             0,           0,                                                               0,0,0,0,0,0;
             0,           0,                                                               0,0,0,0,0,0;
             
    ];
    
%     B=[cc11,cc12, 0,0,0,0,0,0;
%        cc21,cc22, 0,0,0,0,0,0;
%         0,    0,  1,0,0,0,0,0;
%         0,    0,  0,1,0,0,0,0;
%         0,    0,  0,0,1,0,0,0;
%         0,    0,  0,0,0,1,0,0;
%         0,    0,  0,0,0,0,1,0;
%         0,    0,  0,0,0,0,0,1];
%     Q1=B*Q0*B';
    
    F=I8+A*T+0.5*A*A*T*T;
%     ob=obsv(F,H);
%     rank(ob)
%     
    
%     Q=(Q1+F*Q1*F')*T/2;
   
    P1=F*P0*F'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    X1=F*X0;
    X=X1+K*(Z-H*X1);
    P=(I8-K*H)*P1;
    P0=P;
    X0=X;
    
    
    r1(i)=X(3);
    r2(i)=X(4);
    r3(i)=X(5);
    
    result1(i)=P(3,3);
    result2(i)=P(4,4);
    result3(i)=P(5,5);
    %result4(i)=X(6);
    
    d1(i)=X(6);
    d2(i)=X(7);
    d3(i)=X(8);
end

result1=sqrt(result1)*180/pi;
result2=sqrt(result2)*180/pi;
result3=sqrt(result3)*180/pi;
%result4=result4*10000/g;
%result4(maxcount)

% figure;
% plot(result1,'g');
% hold on;
% plot(result2,'r');
% hold on;
% plot(result3);
% xlabel('时间(s)');
% ylabel('失调角估计误差(度)');
% %title('设定值为1度');

% 
% r1=r1*180/pi;
% r2=r2*180/pi;
% r3=r3*180/pi;
% figure;
% 
% plot(r1,'g');
% hold on;
% plot(r2,'r');
% hold on;
% plot(r3);
% ylabel('失调角估计(度)');
% xlabel('时间(s)×0.1');
% %title('设定值为1度');


d1=2*d1*180/pi*60;
d2=2*d2*180/pi*60;
d3=d3*180/pi*60;

figure;

 [b,a]=ellip(4,0.01,40,0.2/20);
% 
%d1=filter(b,a,d1);
%d2=filter(b,a,d2);
% d3=filter(b,a,d3);
x=0.1:0.1:60;

plot(x,d1);
hold on;
plot(x,d2,':');
hold on;
plot(x,d3,'-.');
ylabel('不对准角估计(分)');
xlabel('时间(s)×0.1');
%title('设定值为1度');
d1
d2
d3
%figure;
%plot(velocity_x);
%hold on;
%plot(velocity_y);