verft.m

姿态定位完整的matlab算法

function VerFt
%验证 Ft 展开式对错
global Re e wie g0 ppm ug deg min sec hur dph   %全局变量

glb;

    for k = 1:1:20
        x=10e10*randn(31,1); 
        Cnb=randn(3,3);  vn=randn(3,1);  pos=randn(3,1); wb=randn(3,1); fb=randn(3,1);  vnD=randn(3,1); posD=randn(3,1);
        %%%%%  fi    dvn   dpos   dKG   eb    dKA   db  22  dposD dKD  26  dvnS  dposS
     %  Ft = [ -S1   M2    M13    -S2   -Cnb  o3    o3      o3    o31      o3    o3;
        dx0 = getdx(x, Cnb, vn, pos, wb, fb, vnD, posD, tao);   %Ft 不展开
        Ft = getf(Cnb, vn, pos, wb, fb, vnD, posD, tao);        %Ft 展开
        dx1 = Ft*x;  
        err(k,:)=(dx1-dx0)'./(abs(dx0)+10e-13*ones(31,1))';        %计算(运算顺序先后不同)存在误差，若相对误差10e-10，则在误差范围之内(?)
    end   
figure;
subplot(4,4,1);  plot(1/min*err(:,1:3));        ylabel('fx fy fz(min)');       
subplot(4,4,2);  plot(err(:,4:6));              ylabel('dVnx dVny dVnz(m/s)');
subplot(4,4,3);  plot(Re*err(:,7:8));        ylabel('dLti dLgi(m)');      
subplot(4,4,4);  plot(err(:,9));                ylabel('dH(m)');
subplot(4,4,5);  plot(1/ppm*err(:,10:12));      ylabel('dKGx dKGy dKGz(ppm)'); 
subplot(4,4,6);  plot(1/dph*err(:,13:15));      ylabel('ebx eby ebz(deg/h)');    
subplot(4,4,7);  plot(1/ppm*err(:,16:18));      ylabel('dKAx dKAy dKAz(ppm)');
subplot(4,4,8);  plot(1/ug*err(:,19:21));       ylabel('dbx dby dbz(ug)');
subplot(4,4,9);  plot(Re*err(:,22:23));      ylabel('dLtiD dLgiD(m)');
subplot(4,4,10); plot(err(:,24));               ylabel('dHD(m)');
subplot(4,4,11); plot(err(:,25));               ylabel('dKD');            
subplot(4,4,13); plot(err(:,26:28));            ylabel('dVnxS dVnyS dVnzS(m/s)');
subplot(4,4,14); plot(Re*err(:,29:30));      ylabel('dLtiS dLgiS(m)');
subplot(4,4,15); plot(err(:,31));               ylabel('dHS(m)');           

function dx = getdx(x, Cnb, vn, pos, wb, fb, vnD, posD, tao)
global Re e wie  
%%%%%  fi    dvn   dpos   dKG   eb    dKA   db  22  dposD dKD  26  dvnS  dposS
    fi=x(1:3);  dvn=x(4:6); dpos=x(7:9);  dKG=x(10:12);  eb=x(13:15);  dKA=x(16:18); db=x(19:21);
    dposD=x(22:24); dKD=x(25); dvnS=x(26:28); dposS=x(29:31);
    %%
    sl = sin(pos(1)); cl = cos(pos(1)); tl = sl/cl; secl = 1/cl; secl2 = secl^2; sl2 = sl^2;
    RM = Re*(1-2*e+3*e*sl2); RN = Re*(1+e*sl2); 
    f_RMh = 1/(RM + pos(3)); f_RNh = 1/(RN + pos(3)); f_RMh2 = f_RMh^2; f_RNh2 = f_RNh^2;
    wnie = wie * [0; cl; sl];    wnen = [-vn(2)*f_RMh; vn(1)*f_RNh; vn(1)*f_RNh*tl];
    wnin = wnie + wnen;    

    dwnie = wie*dpos(1)*[0; -sl; cl];
    dwnen = [-f_RMh*dvn(2)+f_RMh2*vn(2)*dpos(3); f_RNh*dvn(1)-f_RNh2*vn(1)*dpos(3); f_RNh*tl*dvn(1)-f_RNh2*vn(1)*tl*dpos(3)+f_RNh*vn(1)*secl2*dpos(1)];
    dwnin = dwnie+dwnen;
    dwnib = Cnb*(dKG.*wb+eb);
    dfn   = Cnb*(dKA.*fb+db);
   
    dfi = -cross(wnin,fi)+dwnin-dwnib; 
    ddvn = cross(Cnb*fb,fi)-cross(2*wnie+wnen,dvn)+cross(vn,2*dwnie+dwnen)+dfn;
    ddLti = f_RMh*dvn(2)-f_RMh2*vn(2)*dpos(3);
    ddLgi = f_RNh*secl*dvn(1)+f_RNh*vn(1)*secl*tl*dpos(1)+f_RNh2*vn(1)*secl*dpos(3);
    ddHgt = dvn(3);
    ddKG = [0; 0; 0];
    deb = -1./tao(1:3).*eb;
    ddKA = [0; 0; 0];
    ddb = [0; 0; 0];
    %%
    slD = sin(posD(1)); clD = cos(posD(1)); tlD = slD/clD; seclD = 1/clD; slD2 = slD^2;
    RMD = Re*(1-2*e+3*e*slD2); RND = Re*(1+e*slD2); 
    f_RMhD = 1/(RM+posD(3)); f_RNhD = 1/(RN+posD(3)); f_RMhD2 = f_RMhD^2; f_RNhD2 = f_RNhD^2;
 
    dvnD = cross(vnD,fi)+vnD*dKD;
    
    ddLtiD = f_RMhD*dvnD(2)-f_RMhD2*vnD(2)*dposD(3);
    ddLgiD = f_RNhD*seclD*dvnD(1)+f_RNhD*vnD(1)*seclD*tlD*dposD(1)+f_RNhD2*vnD(1)*seclD*dposD(3);
    ddHgtD = dvnD(3);
    ddKD = -1/tao(4)*dKD;
    %%
    ddvnS = -1./tao(5:7).*dvnS;
    ddposS = -1./tao(8:10).*dposS;
    %%
    dx = [dfi; ddvn; ddLti;ddLgi;ddHgt; ddKG; deb; ddKA; ddb;    ddLtiD;ddLgiD;ddHgtD; ddKD;   ddvnS; ddposS] ;