jianbo_lib/matlab/compVq.m

function Vq = compVq(q, a)

q1 = q(1);
q2 = q(2);
q3 = q(3);
q4 = q(4);

ax = a(1);
ay = a(2);
az = a(3);

v_dot_q1 = [2*ax*q1 - 2*ay*q4 + 2*az*q3;...
            2*ax*q4 + 2*ay*q1 - 2*az*q2;...
            2*ay*q2 - 2*ax*q3 + 2*az*q1];

v_dot_q2 = [2*ax*q2 + 2*ay*q3 + 2*az*q4;...
            2*ax*q3 - 2*ay*q2 - 2*az*q1;...
            2*ax*q4 + 2*ay*q1 - 2*az*q2];
 
v_dot_q3 = [2*ay*q2 - 2*ax*q3 + 2*az*q1;...
            2*ax*q2 + 2*ay*q3 + 2*az*q4;...
            2*ay*q4 - 2*ax*q1 - 2*az*q3];

v_dot_q4 = [2*az*q2 - 2*ay*q1 - 2*ax*q4;...
            2*ax*q1 - 2*ay*q4 + 2*az*q3;...
            2*ax*q2 + 2*ay*q3 + 2*az*q4];
        
Vq = [v_dot_q1, v_dot_q2, v_dot_q3, v_dot_q4];