User Tools
Differences
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 02:41] Brian Moore |
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 163: | Line 163: | ||
| $$ | $$ | ||
| - | It is worth stating explicitly that $R^{-1} \neq R(-\psi,-\phi,-\theta)$. If you yaw, then pitch, then roll into an orientation, you //cannot// anti-yaw, then anti-pitch, then anti-roll from that orientation to get back to the origin. You'd have to anti-roll, then anti-pitch, then anti-yaw. It must be multiplied by its //transpose// $R^{\mathrm {T}}$. | + | It is worth stating explicitly that $R^{-1} \neq R(-\psi,-\phi,-\theta)$. If you yaw, then pitch, then roll into an orientation, you //cannot// anti-yaw, then anti-pitch, then anti-roll from that orientation to get back to the origin. You'd have to anti-roll, then anti-pitch, then anti-yaw. It must be rotated completely in reverse. It must be multiplied by its //transpose// $R^{\mathrm {T}}$. |
| $$ | $$ | ||
| - | U \neg (R_{-\theta} R_{-\phi} R_{-\psi})(R_\theta R_\phi R_\psi) U \\ | + | U \neq (R_{-\theta} R_{-\phi} R_{-\psi})(R_\theta R_\phi R_\psi) U \\ |
| + | U = (R_{-\psi} R_{-\phi} R_{-\theta}) (R_\theta R_\phi R_\psi) U \\ | ||
| U = (R_{-\psi} (R_{-\phi} (R_{-\theta} R_\theta) R_\phi) R_\psi) U \\ | U = (R_{-\psi} (R_{-\phi} (R_{-\theta} R_\theta) R_\phi) R_\psi) U \\ | ||
| U = (R_{-\psi} (R_{-\phi} (I) R_\phi) R_\psi) U \\ | U = (R_{-\psi} (R_{-\phi} (I) R_\phi) R_\psi) U \\ | ||
| Line 225: | 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> | ||
