MATLAB：我的quatrotate脚本出了什么问题？

debugcn 发表于 Dev

gr8flux

我的 MATLAB 版本没有包含 quatrotate 函数，所以我使用 MathWorks 提供的公式编写了自己的函数here。问题是，我没有得到他们在我的函数中的例子中得到的答案，或者当我手工计算它时。

在他们的示例下，如果我输入以下内容，则应该得到一个n向量[-1 1 1]：

q = [1 0 1 0]; r = [1 1 1]; n = quarotate(q, r)

n =

-1.0000 1.0000 1.0000

在我的函数中，我得到了：

[-3 1 1]

我在这里想念什么？我搜索得越多，我就越困惑。据我所知，答案应该是 [-3 1 1]。

这是我写的函数：

function [n] = quatrotate(q,r)
%Rotate a given acceleration vector by a given quaternion
%
%Inputs:
% q:      A matrix containing a set of quaternion vectors of the 
%         form q = [w,x,y,z]
% r:      A matrix containing a set of linear acceleration vectors 
%         of the form r= [i,j,k] (also known as [x,y,z])
% 
% Outputs:
% n:      The solved matrix containing the rotated vector of each linear 
%         acceleration component
% 

%This assumes that the quaternion is normalised (sqw + sqx + sqy + sqz =1), 
%if not it should be normalised before doing the conversion. 
%To normalise divide qx, qy, qz and qw by n where n=sqrt(qx2 + qy2 + qz2 + qw2)


 for k = 1:size(q,1)
     rot=[(1-2.*q(k,3).^2-2.*q(k,4).^2) 2.*(q(k,2).*q(k,3)+q(k,1).*q(k,4))...
          2.*(q(k,2).*q(k,4)-q(k,1).*q(k,3));2.*(q(k,2).*q(k,3)-q(k,1).*q(k,4))...
          (1-2.*q(k,2).^2-2.*q(k,4).^2) 2.*(q(k,3).*q(k,4)+q(k,1).*q(k,2));...
         2.*(q(k,2).*q(k,4)+q(k,1).*q(k,3)) 2.*(q(k,3).*q(k,4)-q(k,1).*q(k,2))...
          (1-2.*q(k,2).^2-2.*q(k,3).^2)];
     n(k,:) = rot*r(k,:)';
 end

提前致谢！

NKN

首先，您需要计算给定四元数的模数q：

for index = size(q,1):-1:1
    mod(index,:) = norm(q(index,:),2);
end

然后将其标准化：
```
qn = q./(mod* ones(1,4));
```
现在使用以下公式计算直接余弦矩阵：

    dcm = zeros(3,3,size(qn,1));
    dcm(1,1,:) = qn(:,1).^2 + qn(:,2).^2 - qn(:,3).^2 - qn(:,4).^2;
    dcm(1,2,:) = 2.*(qn(:,2).*qn(:,3) + qn(:,1).*qn(:,4));
    dcm(1,3,:) = 2.*(qn(:,2).*qn(:,4) - qn(:,1).*qn(:,3));
    dcm(2,1,:) = 2.*(qn(:,2).*qn(:,3) - qn(:,1).*qn(:,4));
    dcm(2,2,:) = qn(:,1).^2 - qn(:,2).^2 + qn(:,3).^2 - qn(:,4).^2;
    dcm(2,3,:) = 2.*(qn(:,3).*qn(:,4) + qn(:,1).*qn(:,2));
    dcm(3,1,:) = 2.*(qn(:,2).*qn(:,4) + qn(:,1).*qn(:,3));
    dcm(3,2,:) = 2.*(qn(:,3).*qn(:,4) - qn(:,1).*qn(:,2));
    dcm(3,3,:) = qn(:,1).^2 - qn(:,2).^2 - qn(:,3).^2 + qn(:,4).^2;

根据MATLAB文档，可以r通过计算得出向量的旋转dcm如下：

if ( size(q,1) == 1 ) 
    % Q is 1-by-4
    qout = (dcm*r')';
elseif (size(r,1) == 1) 
    % R is 1-by-3
    for i = size(q,1):-1:1
        qout(i,:) = (dcm(:,:,i)*r')';
    end
else
    % Q is M-by-4 and R is M-by-3
    for i = size(q,1):-1:1
        qout(i,:) = (dcm(:,:,i)*r(i,:)')';
    end
end

本文收集自互联网，转载请注明来源。

如有侵权，请联系[email protected] 删除。