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

1 次查看（过去 30 天）
Dany
Dany 2013-4-17
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 个评论
Dany
Dany 2013-4-17
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.

请先登录，再进行评论。

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

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Geoscience 的更多信息

标签

产品

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by