circle_lgkt4.m

惯性导航精解算程序

function circle_lgkt4()
dT = .1;
totaltime = 3600;

fai0 =0;
Rad = 10^4;%标称轨迹 匀速圆周运动，半径Rad,转速Ww
Ww = 2*pi/(0.5*3600);%半小时一圈
Vv= Rad*Ww;
tt = [0:dT:totaltime]';
true_step = size(tt,1);
LL0 = [0 0];
Vv0 = [Vv*sin(fai0) Vv*cos(fai0)];
aL0 = [Ww^2*Rad*cos(fai0) -Ww^2*Rad*sin(fai0)]
hWaitBar = waitbar(0,'Please wait...');
for i=1:true_step 
    k1 = aL0;
    fai1 = fai+dT/2*Ww;
    aL1 = [Ww^2*Rad*cos(fai1) -Ww^2*Rad*sin(fai1)];
    k2 = aL1;
    k3 = aL1;
    fai2 = fai+dT*Ww;
    aL2 = [Ww^2*Rad*cos(fai2) -Ww^2*Rad*sin(fai2)];
    k4 = aL2;
    
    Vv1 = Vv0+dT/6*(k1+2*k2+2*k3+k4);
    
    fai1 = fai0+Ww*dT;
    if fai1>2*pi
    fai1 = fai1-2*pi;
    end
    aL1 = [Ww^2*Rad*cos(fai1) -Ww^2*Rad*sin(fai1)];
    aL0 = [Ww^2*Rad*cos(fai0) -Ww^2*Rad*sin(fai0)];
    Vv1 = Vv0+0.5*(aL0+aL1)*dT;
    LL(i,:) = LL0+0.5*(Vv0+Vv1)*dT;
    VL(i,:) = Vv1;
    aL(i,:) = [Ww^2*Rad*cos(fai0) -Ww^2*Rad*sin(fai0)];
    LL0 = LL(i,:);
    Vv0 = VL(i,:)
waitbar(i/true_step,hWaitBar,[num2str( fix(i/true_step*100) ),'%'])%
end
close(hWaitBar);