quaterion.m

组合导航的程序

function y = quaterion(q,p)
y(1)=q(1)*p(1)-q(2)*p(2)-q(3)*p(3)-q(4)*p(4);
y(2)=q(1)*p(2)+q(2)*p(1)+q(3)*p(4)-q(4)*p(3);
y(3)=q(1)*p(3)+q(3)*p(1)+q(4)*p(2)-q(2)*p(4);
y(4)=q(1)*p(4)+q(4)*p(1)+q(2)*p(3)-q(3)*p(2);