[Simulink 개발] 시뮬링크 센서 축 회전변환

function [x_rot, y_rot] = Rotation(x,y,theta, yaw_car, yaw_traf)

In = [x, y, 1]';

yaw_car_deg = yaw_car/3.14 * 180;
yaw_traf_deg = yaw_traf/3.14 * 180;

yaw_deg = yaw_traf_deg-yaw_car_deg;

RotMat =   [[cosd(theta-yaw_deg) -sind(theta-yaw_deg) 0];
            [sind(theta-yaw_deg)  cosd(theta-yaw_deg) 0];
            [0               0         1]];
Out = RotMat*In;

x_rot = Out(1);
y_rot = Out(2);