test_cone_error.m

来自「前人用四阶龙格库塔方法进行微分方程解算,用matlab编写的源代码,主要用于四元」· M 代码 · 共 26 行

clear 
close
glvs
load test_cone_gen_1deg_1Hz.mat
n = 2;          %子样数
qnb_calcu = qnb0;
for k=n:n:length(dthetak)
    dtheta = dthetak(k-n+1:k,:)';
    phi = cnscl(dtheta);         %圆锥误差补偿
    qnb_calcu = qmul(qnb_calcu,rv2q(phi));    %姿态更新
    err(k/n,:) = qq2phi(qnb_calcu,qnbk(k,:)')'; %求姿态误差
end
time = [1:length(err)]*n*th;
figure
subplot(3,1,1), plot(time,err(:,1)/glv.sec), ylabel('\it\phi_x\rm / arcsec'); grid on 
subplot(3,1,2), plot(time,err(:,2)/glv.sec), ylabel('\it\phi_y\rm / arcsec'); grid on
subplot(3,1,3), plot(time,err(:,3)/glv.sec), ylabel('\it\phi_z\rm / arcsec'); grid on
xlabel('\itt \rm / s'); 
% 理论圆锥误差
k2 = 1;
for k1=1:n+1
    k2 = k2*(2*k1-1);
end
epsilon = afa^2*(2*pi*f*th)^(2*n+1) * n*factorial(n) / (2^(n+1)*k2);
subplot(3,1,3), hold on, plot([0,time(end)],[0,epsilon*length(time)]/glv.sec,'r--')