CODEWITHSUNDEEP
matlab
shunyo
shunyo

Reputation: 1307

Matrix computation without for loop

I am trying to get faster computation for a piece of code and want to get rid of the for loop. I have three vectors roll, pitch and heading. I have to get the rotation matrix for each value of roll,pitch and heading. Right now, my code is

for i=1:size(roll,1)
    R1_r = [1 0 0; 0 cos(roll(i)) -sin(roll(i)); 0 sin(roll(i)) cos(roll(i))];
    R2_p = [cos(pitch(i)) 0 sin(pitch(i)); 0 1 0; -sin(pitch(i)) 0 cos(pitch(i))];
    R3_h = [cos(head(i)) sin(head(i)) 0; -sin(head(i)) cos(head(i)) 0; 0 0 1];
    R = R3_h*R2_p*R1_r;
    x1(i,2:4) = (R*x(i,2:4)')';
end

I want to replace the entire for loop. Is there a way to do that?

Upvotes: 1

Views: 90

Answers (1)

Jonathan H
Jonathan H

Reputation: 7943

The following code will expand the last product for you:

syms roll pitch yaw x1 x2 x3

R1 = [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)];
R2 = [cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)];
R3 = [cos(yaw) sin(yaw) 0; -sin(yaw) cos(yaw) 0; 0 0 1];
R  = R3*R2*R1;
x  = [x1;x2;x3];

R*x

output:

 x2*(cos(roll)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll)) - x3*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)) + x1*cos(pitch)*cos(yaw)
 x2*(cos(roll)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw)) - x3*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)) - x1*cos(pitch)*sin(yaw)
                                                                          x3*cos(pitch)*cos(roll) - x1*sin(pitch) + x2*cos(pitch)*sin(roll)

Based on this, you should take the sines and cosines of your angles:

cp = cos(pitch); sp = sin(pitch);
cr = cos(roll);  sr = sin(roll);
cy = cos(yaw);   sy = sin(yaw);

and replace x1 -> x1(:,2), x2 -> x1(:,3) and x3 -> x1(:,4) in the above formula, changing the * signs with .* where needed.

The final code should look like this:

x1_new = x1(:,3).*(cr.*sy + cy.*sp.*sr) - x1(:,4).*(sr.*sy - cr.*cy.*sp) + x1(:,2).*cp.*cy;
x2_new = x1(:,3).*(cr.*cy - sp.*sr.*sy) - x1(:,4).*(cy.*sr + cr.*sp.*sy) - x1(:,2).*cp.*sy;
x3_new = x1(:,4).*cp.*cr - x1(:,2).*sp + x1(:,3).*cp.*sr;

Be careful not to replace the columns of x1 as you go; you need to store the results in new columns, otherwise you will corrupt the rest of the calculation. After these three lines you can replace safely the columns of x1.

Upvotes: 2

Related Questions