q2rv.m

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

function v = q2rv(q) 
	if(q(1)<0)
	    q = -q;
	end  
    n2 = acos(q(1));
    if n2>1e-100
        k = 2*n2/sin(n2);
    else
        k = 2;
    end
    v = k*q(2:4);