This shows you the differences between two versions of the page.
Both sides previous revision Previous revision Next revision | Previous revision Last revision Both sides next revision | ||
cs:localization:rotation:start [2017/01/22 03:07] Brian Moore [Inverse Rotation] |
cs:localization:rotation:start [2018/04/29 11:24] Brian Moore [Complete Rotation] |
||
---|---|---|---|
Line 54: | Line 54: | ||
A full 3D rotation includes a roll, pitch, and yaw. With these three rotations, we can describe any arbitrary orientation. | A full 3D rotation includes a roll, pitch, and yaw. With these three rotations, we can describe any arbitrary orientation. | ||
- | Order of operation is important. The complete $R$ matrix describes the vehicle first yawing around its own z-axis, then pitching along its own y-axis, and then finally rolling about its own x-axis. As an example, the rotation $R([180 10 30])$ would have the submarine pointed to the left $30^o$, then pitched slightly upwards by $10^o$, and then rolling onto its back at $180^o$. | + | Order of operation is important. The complete $R$ matrix describes the vehicle first yawing around its own z-axis, then pitching along its own y-axis, and then finally rolling about its own x-axis. As an example, the rotation $R([180,10,30])$ would have the submarine pointed to the left $30^o$, then pitched slightly upwards by $10^o$, and then rolling onto its back at $180^o$. |
Line 226: | Line 226: | ||
$$ | $$ | ||
+ | |||
+ | ===== Code ===== | ||
+ | |||
+ | To produce a 3x3 rotation matrix from roll $\psi$, pitch $\phi$, and yaw $\theta$ use the following matlab code or it's C++ equivalent: | ||
+ | |||
+ | <code matlab> | ||
+ | |||
+ | function [R] = r3D(omega) % omega = [roll; pitch; yaw] | ||
+ | |||
+ | R = [cosd(omega(3)) -sind(omega(3)) 0;... | ||
+ | sind(omega(3)) cosd(omega(3)) 0;... | ||
+ | 0 0 1] * ... | ||
+ | [cosd(omega(2)) 0 sind(omega(2));... | ||
+ | 0 1 0;... | ||
+ | -sind(omega(2)) 0 cosd(omega(2))] * ... | ||
+ | [1 0 0;... | ||
+ | 0 cosd(omega(1)) -sind(omega(1));... | ||
+ | 0 sind(omega(1)) cosd(omega(1))]; | ||
+ | | ||
+ | end | ||
+ | </code> | ||
+ | |||
+ | To find an equivalent roll $\psi$, pitch $\phi$, and yaw $\theta$ //given// a 3x3 rotation matrix $R$ use the following code: | ||
+ | |||
+ | <code matlab> | ||
+ | |||
+ | function [omega] = ir3D(R) | ||
+ | |||
+ | theta = atan2di(R(2,1),R(1,1)); | ||
+ | psi = atan2di(R(3,2),R(3,3)); | ||
+ | phi = atan2di(-R(3,1),sqrt(sum(R(3,2:3).^2))); | ||
+ | |||
+ | if(abs(phi)==90) | ||
+ | theta = atan2di(-R(1,2),R(2,2)); | ||
+ | psi = 0; | ||
+ | end | ||
+ | |||
+ | |||
+ | omega = [psi;phi;theta]; %omega = [roll;pitch;yaw] | ||
+ | end | ||
+ | </code> | ||