test_align_compass.m

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

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