velupdat.m

来自「惯性导航系统传递对准系统模型程序」· M 代码 · 共 20 行

function vel_new = velupdat(vel_old2,vel_old1,td12,tdex,del_Vl,...
                             omega_el_l,DCMel,g,procflg,deltat)

if nargin<10,error('insufficient number of input arguments'),end
%速度更新程序；
g_vect_L = [0 0 -g]';       
omega_ie_e = [0 0 7.292115e-5]';
omega_ie_L = DCMel*omega_ie_e;

for i = 1:3,
   v_ex(i) = extrapol(vel_old1(i),vel_old2(i),td12,tdex);
end

S = skewsymm(v_ex);
if procflg == 0,
   vel_new = vel_old2' + del_Vl + ( S*(omega_el_l  + 2*omega_ie_L) + g_vect_L )*deltat;
else,
   vel_new = vel_old2' + del_Vl + ( g_vect_L )*deltat;
end