test_cone_gen.m

前人用四阶龙格库塔方法进行微分方程解算,用matlab编写的源代码,主要用于四元素微分方程的实时解算

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');