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


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