circle_lgkt2.m

惯性导航精解算程序

function circle_lgkt2()
clear;
close all;
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 Rad];
Vv0 = [Vv*cos(fai0) -Vv*sin(fai0)];
aL0 = [-Ww^2*Rad*sin(fai0) -Ww^2*Rad*cos(fai0)]
hWaitBar = waitbar(0,'Please wait...');
for i=1:true_step 
    fai_true(i) = 0+Ww*dT*i;
    if fai_true(i)>2*pi
        fai_true(i) = fai_true(i)-2*pi;
    end
    VL_true(i,:) = [Vv*cos(fai_true(i)) -Vv*sin(fai_true(i))];
    aL_true(i,:) = [-Ww^2*Rad*sin(fai_true(i)) -Ww^2*Rad*cos(fai_true(i))];
    LL_true(i,:) = [Rad*sin(fai_true(i)) Rad*cos(fai_true(i))];
    
    fai1 = fai0+Ww*dT;
    if fai1>2*pi
    fai1 = fai1-2*pi;
    end
    aL1 = [-Ww^2*Rad*sin(fai1) -Ww^2*Rad*cos(fai1)];
    aL0 = [-Ww^2*Rad*sin(fai0) -Ww^2*Rad*cos(fai0)];
    Vv1 = Vv0+0.5*(aL0+aL1)*dT;
    LL(i,:) = LL0+0.5*(Vv0+Vv1)*dT;
    VL(i,:) = Vv1;
    aL(i,:) = [-Ww^2*Rad*sin(fai1) -Ww^2*Rad*cos(fai1)];
    LL0 = LL(i,:);
    Vv0 = VL(i,:);
    fai0 = fai1;
waitbar(i/true_step,hWaitBar,[num2str( fix(i/true_step*100) ),'%'])%
end
close(hWaitBar);
figure();plot(VL_true(:,1)-VL(:,1));title('ev_e');
figure();plot(VL_true(:,2)-VL(:,2));title('ev_n');
figure();plot(LL_true(:,1)-LL(:,1));title('el_e');
figure();plot(LL_true(:,2)-LL(:,2));title('el_n');