www.pudn.com > SINS-MatlabImplement.rar > sins.m, change:2008-03-24,size:476b


function [qnb, vn, pos] = sins(qnb_1, vn_1, pos_1, wm, vm, ts) 
    tss = ts*size(wm,2); 
    [phim,dvbm] = cnscl(wm,vm); 
    [wnie,wnen,rmh,rnh,gn] = earth(pos_1,vn_1); 
    wnin = wnie+wnen; 
    vn = vn_1 + qmulv(rv2q(-wnin*(1.0/2*tss)),qmulv(qnb_1,dvbm)) ... 
        + (gn-cross(wnie+wnin,vn_1))*tss; 
    vn1_1 = (vn+vn_1)/2; 
    pos = pos_1 + tss*[vn1_1(2)/rmh;vn1_1(1)/(rnh*cos(pos_1(1)));vn1_1(3)]; 
    qnb = qmul(qnb_1, rv2q(phim - qmulv(qconj(qnb_1),wnin*tss)));