trajectory.m

姿态定位完整的matlab算法

function trajectory
global Re e g0 wie 
Re = 6378160;   e = 1/298.3;   wie = 7.2921151467e-5;   g0 = 9.7803267714;
th = 0.002;       %龙格-库塔步长
deg = pi/180; min = deg/60;

    att = pi/180*[0; 0; 0];	 %[pitch roll azimuth]  
	vb = [0; 0; 0];          %[0 vby 0]       
	pos = [34*deg+14.76289014*min; 108*deg+54.57983*min; 380];  %[latitude longitude height]

    %rk(1:3) att(pitch roll azimuth);   rk(2:6) vn(vnE vnN vnU);   rk(7:9) pos(lti lgi hgt)
    %rk(10:12) wm;  rk(13:15) vm;         
    rk = [att; Att2Mat(att)*vb; pos; zeros(6,1)];
    
    fid = fopen('e:/ygm/vehicle/trace2.bin','wb');  %4 good
    wat2 = getwat(0);
    for k=2:2:1000*1000
        wat0 = wat2;        k1 = funrk(rk,         wat0);
        wat1 = getwat(k-1); k2 = funrk(rk+th/2*k1, wat1);
                            k3 = funrk(rk+th/2*k2, wat1);
        wat2 = getwat(k);   k4 = funrk(rk+  th*k3, wat2);
        rk = rk + th/6*(k1+2*k2+2*k3+k4);
        if mod(k,10)==0                               %10 ms
            fwrite(fid, [rk; k4(10:15)], 'real*8');   %保存数据, k4(10:15): [wb, fb]
            if mod(k,1000)==0
                step=k/1000,                          %进度显示
            end
        end
    end
    fclose(fid);
    
function drk = funrk(rk, wat)        
global Re e g0 wie 
    %通用变量计算
	si = sin(rk(1)); ci = cos(rk(1)); sj = sin(rk(2)); cj = cos(rk(2)); sk = sin(rk(3)); ck = cos(rk(3));
	slti = sin(rk(7)); clti = cos(rk(7)); tlti = slti/clti; slti2 = slti^2; slti4 = slti2^2;
    RM = Re*(1-2*e+3*e*slti2); RN = Re*(1+e*slti2); RMh = RM + rk(9); RNh = RN + rk(9);
    wnie = wie * [0; clti; slti];	wnen = [-rk(5)/RMh; rk(4)/RNh; rk(4)/RNh*tlti];
    Cnb = [ cj*ck+si*sj*sk, ci*sk,  sj*ck-si*cj*sk;
           -cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck;
           -ci*sj,          si,     ci*cj ];
	%姿态增量
	datt = wat(1:3);
	%速度增量
	Cnt = [ ck, ci*sk, -si*sk; 
           -sk, ci*ck, -si*ck; 
            0,  si,     ci ];
	dvn = Cnt*wat(4:6);
	%位置增量
    dpos = [rk(5)/RMh; rk(4)/(RNh*clti); rk(6)];
	%角增量
    wnin = wnie + wnen;
	a2w = [ cj, 0,  sj*ci; 
            0,  1, -si; 
            sj, 0, -cj*ci ];
    wbnb = a2w*wat(1:3);
	dwm = Cnb'*wnin + wbnb;
	%比力增量
    g = g0*(1+5.27094e-3*slti2+2.32718e-5*slti4) - 3.086e-6*rk(9);
	gn = [0; 0; -g];
  	dvm = Cnb' * (dvn+cross(2*wnie+wnen,rk(4:6))-gn);
    %rk增量
    drk = [datt; dvn; dpos; dwm; dvm];
    
function wat = getwat(k)          %轨迹设置
%姿态角变化率 wat(1:3) [wPitch wRoll wAzimuth]; 轨迹加速度 wat(4:6) [atx aty atz]
%阶段   动作      起始时间(s)     航向azimuth(deg)  俯仰pitch(deg)     速度(m/s)
%阶段   动作              持续时间(s)       倾斜roll(deg)   加速度(m/s^2)   
%       停止       0      100     0         0       0       0          0
        if k<=100*1000
            wat=[0; 0; 0;    0; 0; 0];
%       加速       100    20      0         0       0       1          0
        elseif k<=120*1000
            wat=[0; 0; 0;     0; 1; 0];
%       匀速       120    80      0         0       0       0          20
        elseif k<=200*1000
            wat=[0; 0; 0;    0; 0; 0];
%       锥运动     200    100     0         0       0       0          20
        elseif k<=300*1000;
            k=k-200*1000; t = k/1000; a = 1*pi/180; w = 2*pi*5;
            if k<=1*1000          %进入锥运动 1s
                wat = [a*sin(w*1); a*cos(w*1); 0;     0; 0; 0];
            elseif k<=99*1000     %锥运动 98s
                wat = [a*w*cos(w*t); -a*w*sin(w*t); 0;     0; 0; 0];
            elseif k<=100*1000    %退出锥运动 1s
                wat = -[a*sin(w*99); a*cos(w*99); 0;     0; 0; 0];
            end
%       减速       300    10      0         0       0        -1         20
        elseif k<=310*1000
            wat=[0; 0; 0;    0; -1; 0];
%       右转弯     310    3       0/15      0       0        0          10
        elseif k<=313*1000
            v=10; w=45*pi/180/3; a=v*w;
            wat=[0; 0; w;     a; 0; 0]; 
%       匀速       313    97      45        0       0         0         10
        elseif k<=400*1000
            wat=[0; 0; 0;    0; 0; 0];
%       进入爬升   400    5       45        0       0/3       0         10
        elseif k<=405*1000
            v=10; w=15*pi/180/5; a=v*w;
            wat=[w; 0; 0;   0; 0; a];
%       匀速       405    80      45        0       15        0         10
        elseif k<=485*1000
            wat=[0; 0; 0;    0; 0; 0];
%       退出爬升   485    15      45        0       15/-1     0         10
        elseif k<=500*1000
            v=10; w=15*pi/180/15; a=v*w;
            wat=[-w; 0; 0;   0; 0; -a];
%       左转弯     500    31.5    45/-10    0        0        0          10
        elseif k<=531.5*1000
            v=10; w=315*pi/180/31.5; a=v*w;
            wat=[0; 0 ; -w;     -a; 0; 0]; 
%       匀速       531.5  68.5    90        0        0        0          10
        elseif k<=600*1000
            wat=[0; 0; 0;    0; 0; 0];
%       减速       600    10      90        0        0        -1         10
        elseif k<=610*1000
            wat=[0; 0; 0;    0; -1; 0];
%       停止       610    90      90        0        0        0           0
        elseif k<=700*1000
            wat=[0; 0; 0;    0; 0; 0];
%       划船运动   700    100     90        0        0        0           0
        elseif k<=800*1000;
            k=k-700*1000; t = k/1000; a = 1*pi/180; p = 1.0e-3; w = 2*pi*2;
            if k<=1*1000          %进入划船运动 1s
                wat = [a*cos(w*1); 0; 0;     0; -p*w*sin(w*1); 0];
            elseif k<=99*1000     %划船运动 98s
                wat = [-a*w*sin(w*t); 0; 0;     0; -p*w^2*cos(w*t); 0];
            elseif k<=100*1000    %退出划船运动 1s
                wat = -[a*cos(w*99); 0; 0;     0; -p*w*sin(w*99); 0];
            end
%       加速       800    10      90        0        0         2          0
        elseif k<=810*1000
            wat=[0; 0; 0;    0; 2; 0];
%       匀速       810    90      90        0        0         0          20
        elseif k<=900*1000
            wat=[0; 0; 0;    0; 0; 0];
%       刹车       900    4       90        0        0         -5         20
        elseif k<=904*1000
            wat=[0; 0; 0;    0; -5; 0];
%       停止       904    96      90        0        0         0          0
        else
            wat=[0; 0; 0;    0; 0; 0];
        end