www.pudn.com > SINS-MatlabImplement.rar > test_cone_gen.m, change:2009-02-05,size:1018b


clear 
close 
glvs 
afa = 1.0*glv.deg;  %半锥角 
f = 1;    %锥运动频率(Hz) 
th = 0.01;       %采样时间 
w = 2*pi*f;     %锥运动频率 
time = 10;  %仿真时间长度 
len = fix(time/th);  %仿真步数 
t = 0;          %初始姿态四元数 
qnb0 = [cos(afa/2); sin(afa/2)*cos(w*t); sin(afa/2)*sin(w*t); 0];   
for k=1:len 
    % 姿态四元数真值 
    t = k*th; 
    qnbk(k,:) = [cos(afa/2); sin(afa/2)*cos(w*t); sin(afa/2)*sin(w*t); 0]'; 
    % 角增量 
    t = (k-1)*th; 
    dthetak(k,:) = [-2*sin(afa)*sin(w*th/2)*sin(w*(t+th/2)); 
        2*sin(afa)*sin(w*th/2)*cos(w*(t+th/2)); 
        -2*w*th*(sin(afa/2))^2 ]'; 
end 
save test_cone_gen_1deg_1Hz.mat   afa f th qnb0 qnbk dthetak 
figure 
subplot(3,1,1), plot([1:len]*th,dthetak(:,1)/glv.sec), ylabel('\it\Delta\theta_x\rm / arcsec'); grid on  
subplot(3,1,2), plot([1:len]*th,dthetak(:,2)/glv.sec), ylabel('\it\Delta\theta_y\rm / arcsec'); grid on 
subplot(3,1,3), plot([1:len]*th,dthetak(:,3)/glv.sec), ylabel('\it\Delta\theta_z\rm / arcsec'); grid on 
xlabel('\itt \rm / s');