matlab中quat2angle,cord2quat.m

function q = cord2quat(d,dPrime)

% CORD2QUAT 根据旋转前后对应点的坐标(仅存在旋转)，求四元素

% d 是起始坐标系

% dPrime 是旋转后的坐标系

% reference：

% [1] https://blog.csdn.net/u013517182/article/details/53945005

%

% d1 = [1, 0, 0]; %X

% d2 = [0, 1, 0]; %Y

% d3 = [0, 0, 1]; %Z

%

% d1Prime = [0, 1, 0]; %x

% d2Prime = [-1, 0, 0]; %y

% d3Prime = [0, 0, 1]; %z

d1 = d(1,:); %X

d2 = d(2,:); %Y

d3 = d(3,:); %Z

d1Prime = dPrime(1,:); %x

d2Prime = dPrime(2,:); %y

d3Prime = dPrime(3,:); %z

A1 = [ 0 , (d1Prime-d1) ;

(d1-d1Prime)' , crossProductMatrix(d1Prime+d1)' ];

A2 = [ 0 , (d2Prime-d2) ;

(d2-d2Prime)' , crossProductMatrix(d2Prime+d2)' ];

A3 = [ 0 , (d3Prime-d3) ;

(d3-d3Prime)' , crossProductMatrix(d3Prime+d3)' ];

B = A1*A1'+A2*A2'+A3*A3';

% 开使求解优化问题

% A = [];

% b = [];

% Aeq = [];

% beq = [];

% lb = [-1 -1 -1 -1]; %设置下限

% ub = [1 1 1 1]; %设置上限

% func = @(x) [x(1), x(2), x(3), x(4)]*B*[x(1), x(2), x(3), x(4)]';

% nonlcon = @normalize;

% x = fmincon(func,[sqrt(2)/2 0 0 sqrt(2)/2],A,b,Aeq,beq,lb,ub,nonlcon);

% [yaw, pitch, roll] = quat2angle(x); %matlab自带的四元数转欧拉角的函数quat2angle

% function [c,ceq] = normalize(x)

% c = [];

% ceq = x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 - 1;

% end

% R = quat2dcm(x);

% angle2dcm(90/180*pi,0/180*pi,0/180*pi)

[V, D] = eig(B);

[~,Index] = min(diag(D));

q = V(:,Index);

function cpm = crossProductMatrix(x)

% 返回一个向量的叉乘矩阵

cpm = [0, -x(3), x(2);

x(3), 0, -x(1);

-x(2), x(1), 0];

end

end

一键复制

编辑

Web IDE

原始数据

按行查看

历史