User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision | Next revision Both sides next revision | ||
|
cs:localization:rotation:start [2017/01/22 03:07] Brian Moore [Inverse Rotation] |
cs:localization:rotation:start [2017/01/22 03:16] Brian Moore |
||
|---|---|---|---|
| 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> | ||
