is there a fast working function that can find rotation angles?

1 次查看（过去 30 天）

Dany 2013-4-17

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/72354-is-there-a-fast-working-function-that-can-find-rotation-angles

is there a built in matlab function that calculats rotation angles (pith, roll , heading) between two sets of points? i did it myself but it runs for a long time when working with lage data base. i want to know if there is a function that calculates it faster then the function i wrote, usually the built in functions are faster.

2 个评论
显示无隐藏无

James Tursa 2013-4-17

Please post your code and we can suggest improvements.

Dany 2013-4-17

在 MATLAB Online 中打开

load aVec.mat load bVec.mat load cVec.mat load dVec.mat

    %Position of antennas in ship's refference frame w.r.t LCF.
    Apos = [-6.4183; -0.1470; 5.3397];
    Bpos = [5.9047; -0.1665; 4.1665];
    Cpos = [-1.3156; -1.3016; 5.7900];
    Dpos = [-0.8031; 1.6337; 5.9014];
    %Creating functions and derivatives.
    syms K r p h X Y Z;
    R = [cos(r)*cos(h), sin(p)*sin(r)*cos(h)-cos(p)*sin(h),                         sin(p)*sin(h)+cos(p)*sin(r)*cos(h);
            cos(r)*sin(h), sin(p)*sin(r)*sin(h)+cos(p)*cos(h), cos(p)*sin(r)*sin(h)-sin(p)*cos(h);
            -sin(r), sin(p)*cos(r), cos(p)*cos(r)];
    dRr = diff(R,r); dRp = diff(R,p); dRh = diff(R,h);
    A = zeros(12,6); Lb = zeros(12,1); L0 = zeros(12,1); L = zeros(12,1);
    [a,~] = size(aVec);
    PRH = zeros(a,6);
    X0 = [0; 0; 0; 0; 0; 0];
    wb = waitbar(0,'Please wait... Calculation in progress...');
    for i = 1:a
            waitbar(i/a);
            Lb = [aVec(i,:)'; bVec(i,:)'; cVec(i,:)'; dVec(i,:)'];
            oldx = X0; dd = [1; 1; 1; 1; 1; 1; 1];
            while max(abs(dd(4:6))) > 0.0001
                r = X0(5); p = X0(4); h = X0(6);  
                %L0 initiation.
                L0(1:3,1) = X0(1:3)+eval(R)*Apos;
                L0(4:6,1) = X0(1:3)+eval(R)*Bpos;
                L0(7:9,1) = X0(1:3)+eval(R)*Cpos;
                L0(10:12,1) = X0(1:3)+eval(R)*Dpos;
                %Matrix A initiation.
                %Part 1.
                A(1:3,1:3) = eye(3);
                A(1:3,4:6) = [eval(dRp)*Apos, eval(dRr)*Apos, eval(dRh)*Apos];
                %Part 2.
                A(4:6,1:3) = eye(3);
                A(4:6,4:6) = [eval(dRp)*Bpos, eval(dRr)*Bpos, eval(dRh)*Bpos];
                %Part 3.
                A(7:9,1:3) = eye(3);
                A(7:9,4:6) = [eval(dRp)*Cpos, eval(dRr)*Cpos, eval(dRh)*Cpos];
                %Part 4.
                A(10:12,1:3) = eye(3);
                A(10:12,4:6) = [eval(dRp)*Dpos, eval(dRr)*Dpos, eval(dRh)*Dpos];
                L = Lb-L0;
                N = A'*A;
                U = A'*L;
                x = inv(N)*U;
                X0 = X0+x; 
                dd = x - oldx; 
                oldx = x;
            end
            V = A*x-L;
            s = V'*V/(size(A,1)-size(A,2));
            sigX = s*inv(N);
            dg = sqrt(diag(sigX));
            PRH(i,:) = [rad2deg(X0(4)), rad2deg(X0(5)), rad2deg(X0(6)), rad2deg(dg(4)), rad2deg(dg(5)), rad2deg(dg(6))];
    end

thats basically it. im using adjustment calculation for 3D transformation. because there are iterations it runs prety slow when there are lets say 6000 measurment epochs involved. i found the function "r = vrrotvec(a,b)" in Matlab help, but i dont know how to transform the results of this function to regular rotation angles.

thank you for your help.

请先登录，再进行评论。

请先登录，再回答此问题。