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> | ||