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


clear 
close 
glvs; 
ws2 = glv.g0/glv.Re;    % 修拉周期平方 
xi = sqrt(2)/2; Td = 20;  % 水平调平参数k1,k2,k3 
sigma = 2*pi*xi/(Td*sqrt(1-xi^2)); 
k1 = 3*sigma; k2 = sigma^2*(2+1/xi^2)/ws2-1; k3 = sigma^3/(glv.g0*xi^2); 
xi = sqrt(2)/2; Td = 50;  % 罗经回路控制参数kz1,kz2,kz3 
sigma = 2*pi*xi/(Td*sqrt(1-xi^2)); 
kz1 = 2*sigma; kz4 = kz1; kz2 = 4*sigma^2/ws2-1; kz3 = 4*sigma^4/glv.g0; 
VE = 0; VN = 0; PE = 0; PN = 0;  wnc = zeros(3,1); % 状态初始值 
L = 34*glv.deg; pos = [L; 0; 0];  % 初始位置 
[wnie, wnen, RMh, RNh, gn] = earth(pos, [0;0;0]); 
eb = [.01; .01; -.01]*glv.dph;  web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺漂移 
db = [100; -100; 100]*glv.ug; wdb =  [10; 10; 10]*glv.ug; % 加速度计零偏 
att = [0; 0; 30]*glv.deg; qnb0 = a2qnb(att); % 姿态真值 
phi = [-10.00; 10.00; 60.0]*glv.min; 
qnb = qaddphi(qnb0, phi);  % 加入姿态误差的计算四元数初值 
Ts = .1;   % 仿真步长 
t = 300;  % 总时间长度 
len = fix(t/Ts); phik=zeros(len,3); 
for k=1:len 
    wbib = qmulv(qconj(qnb0),wnie)+eb+web.*randn(3,1);  % 陀螺采样角速度(假设载体静止) 
    fbsf = qmulv(qconj(qnb0),-gn)+db+wdb.*randn(3,1);  % 加速度计采样比力 
 
    qnb = qmul( qnb, rv2q((wbib-qmulv(qconj(qnb),wnie+wnc))*Ts) );  % 带反馈控制项wnc的姿态更新 
    fn = qmulv(qnb, fbsf);  % 比力变换 
    phik(k,:) = qq2phi(qnb, qnb0)'; 
     
    if k<(50/Ts)     % 前50s水平调平 
        VE = VE + (fn(1)-k1*VE)*Ts; 
        PE = PE + VE*k3*Ts; 
        wnc(2,1) = VE*(1+k2)/glv.Re + PE; 
        VN = VN + (fn(2)-k1*VN)*Ts; 
        PN = PN + VN*k3*Ts; 
        wnc(1,1) = -VN*(1+k2)/glv.Re - PN; 
    else        % 罗经对准 
        VE = VE + (fn(1)-k1*VE)*Ts; 
        PE = PE + VE*k3*Ts; 
        wnc(2,1) = VE*(1+k2)/glv.Re + PE; 
        VN = VN + (fn(2)-k1*VN)*Ts; 
        PN = PN + VN*k3*Ts; 
        VN = VN + (fn(2)-kz1*VN)*Ts; 
        wnc(1,1) = -VN*(1+kz2)/glv.Re; 
        wnc(3,1) = (VN*kz3*Ts/wnie(2)+wnc(3,1))/(1+kz4*Ts); 
    end 
end 
time = [1:length(phik)]*Ts; 
figure 
subplot(3,1,1), plot(time,phik(:,1)/glv.min), ylabel('\it\phi_E\rm / arcmin'); grid on 
subplot(3,1,2), plot(time,phik(:,2)/glv.min), ylabel('\it\phi_N\rm / arcmin'); grid on 
subplot(3,1,3), plot(time,phik(:,3)/glv.min), ylabel('\it\phi_U\rm / arcmin'); grid on 
xlabel('\itt \rm / s');