User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision Next revision | Previous revision | ||
|
cs:localization:rotation:start [2017/01/22 03:07] Brian Moore [Inverse Rotation] |
cs:localization:rotation:start [2018/04/29 11:25] (current) Brian Moore [Equivalent Rotations] |
||
|---|---|---|---|
| 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 187: | Line 187: | ||
| We often want to calculate where our vector is at after rotating first by $R_1$, then by $R_2$, and finally by $R_3$. If you recall from above, these complete Rotation matrices will behave exactly as the specific roll, pitch, and yaw matrices. That is to say, the rotations they perform are all relative to the fixed global $x,y,z$ axes. Which means that the rotation performed last, $R_3$, must be allowed to act on the vector first. | We often want to calculate where our vector is at after rotating first by $R_1$, then by $R_2$, and finally by $R_3$. If you recall from above, these complete Rotation matrices will behave exactly as the specific roll, pitch, and yaw matrices. That is to say, the rotations they perform are all relative to the fixed global $x,y,z$ axes. Which means that the rotation performed last, $R_3$, must be allowed to act on the vector first. | ||
| - | Were we to tell our submarine's control system to make a relative goal of $R(\psi_1,\phi_1,\theta_1)$, and then once accomplishing it make another relative rotation $R(\psi_2,\phi_2,\theta_2)$, and then finally tell it to make a third relative rotation $R(\psi_3,\phi_3,\theta_3)$, we would calculate the result as | + | Were we to tell our submarine's control system to perform a relative rotation of $R(\psi_1,\phi_1,\theta_1)$, and then once accomplishing it make another relative rotation $R(\psi_2,\phi_2,\theta_2)$, and then finally tell it to make a third relative rotation $R(\psi_3,\phi_3,\theta_3)$, we would calculate the result as |
| $$ | $$ | ||
| 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> | ||
