h_speedattitudematrix1017.m

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

function h_speedattitudematrix1017()
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;

    mcc11=c11;
    mcc12=c12;
    mcc13=c13;
    mcc21=c21;
    mcc22=c22;
    mcc23=c23;
    mcc31=c31;
    mcc32=c32;
    mcc33=c33;
    
rem=ratee;
rnm=raten;
rum=rateu;

pitchm=pitch;
rollm=roll;
yawm=yaw;


load data_s;
voutse=vout_e;
voutsn=vout_n;

pitchs=pitch;
rolls=roll;
yaws=yaw;

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;
%     0,0,1,0,0,0,0,0;
%     0,0,0,1,0,0,0,0;
%     0,0,0,0,1,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,0.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, 1*pi/180*1*pi/180,0.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, 1*pi/180*1*pi/180]);
%P0=diag([1000,1000,1000,1000,1000,1000,1000,1000]);
Z=[0;0;0;0;0];
% 
R=diag([0.1*0.1,0.1*0.1,3/60*pi/180*3/60*pi/180,3/60*pi/180*3/60*pi/180,3/60*pi/180*3/60*pi/180]);%[0.00001*g*0.00001*g,0;0,0.00001*g*0.00001*g];
% 
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]);
% 
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);
    
    sc=[cc11,cc12,cc13;
        cc21,cc22,cc23;
        cc31,cc32,cc33];
    mc=[mcc11(i),mcc12(i),mcc13(i);
        mcc21(i),mcc22(i),mcc23(i);
        mcc31(i),mcc32(i),mcc33(i)];
    msc=mc'*sc;
    
    
    faii=fai(i);
    
    em=rem(i);
    nm=rnm(i);
    um=rum(i);
    
    Vx0=voutse(i);
    
    Z(1)=(-voutse(i)+voutme(i));
    Z(2)=(-voutsn(i)+voutmn(i));
    
    Z(3)=msc(2,3);
    Z(4)=-msc(1,3);
    Z(5)=msc(1,2);
    
    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;
             
    ];
    
   H=[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];
    
    F=I8+A*T+0.5*A*A*T*T;
    
    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=d1*180/pi*60;
d2=d2*180/pi*60;
d3=d3*180/pi*60;
figure;

 [b,a]=ellip(4,0.01,40,0.16/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);