test_sins.m

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

clear
% close
glvs
ts = 0.1;        %采样周期
n = 3;    % 子样数
att0=[0;0;0]*glv.deg; vn0=[0;0;0]; pos0=[0*glv.deg;0;0];   %初始值设置
qnb0 = a2qnb(att0);
vn = vn0; pos = pos0;
[wnie, wnen, RMh, RNh, gn] = earth(pos, vn0);
wbib = qmulv(qconj(qnb0),wnie);
fb = qmulv(qconj(qnb0),-gn);
wm=wbib*ts; vm=fb*ts;   %静止时角增量和比力增量
eb = [0.01; 0.01; 0.01]*glv.dph; web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺误差
db =  [100; 100; 100]*glv.ug;  wdb =  [10; 10; 10]*glv.ug; % 加速度计误差
phi = [0.5;0.5;3]*glv.min; qnb = qaddphi(qnb0,phi); % 加入姿态误差
kk = 1;
t = 3600;  % 总时间长度
len = fix(t/ts/n); errphi=zeros(len,3); errvn=errphi; errpos=errphi;
for k=0:n:len*n
    for k1=1:n
        wme(:,k1) = wm+(eb+web.*randn(3,1))*ts;  % 构造多子样数据
        vme(:,k1) = vm+(db+wdb.*randn(3,1))*ts;
    end
    [qnb,vn,pos]=sins(qnb,vn,pos,wme,vme,ts); 
    vn(3) = 0;  % 限定高度发散
    errphi(kk,:) = qq2phi(qnb,qnb0)'; %求姿态误差
    errvn(kk,:) = (vn-vn0)';
    errpos(kk,:) = (pos-pos0)';
    kk = kk+1;
end
time = [1:length(errphi)]*ts*n;
figure,
subplot(3,1,1),plot(time,errphi/glv.min), ylabel('\it\phi\rm / arcmin'); grid on
subplot(3,1,2),plot(time,errvn), ylabel('\it\delta V\rm / m/s');  grid on
subplot(3,1,3),plot(time,[errpos(:,1:2)*glv.Re,errpos(:,3)]), ylabel('\it\delta P\rm / m'); grid on
xlabel('\itt\rm / s');