## Cordic in MATLAB

Let’s z be a 2D point in the space as $$z = x + jy$$, if we want to rotate this point a given angle $$\theta$$, we get the following expressions:
$e^{j\theta} \cdot z = \left(\cos{\theta} + j \sin{\theta}\right)\left(x+jy\right) \\ = x\cos{\theta}-y\sin{\theta} + j \left(y \cos{\theta} + x \sin{\theta} \right) \\ = x’ + j y’$

Then, for a generic point, the rotation can be expressed as an equation system, where $$x’$$ and $$y’$$ are the new coordinates, $$\theta$$ is the rotation angle and $$x$$ and $$y$$ are the original coordinates:
$\begin{bmatrix} x’\\ y’ \end{bmatrix}= \begin{bmatrix} \cos{\theta} & -\sin{\theta}\\ \sin{\theta} & \cos{\theta} \end{bmatrix}\begin{bmatrix} x\\ y \end{bmatrix}$

This rotation can be coded in MATLAB as:


%% Function to rotate vector
function v = rotate(P, theta)
rot = [cos(theta) -sin(theta);
sin(theta) cos(theta)];
v = rot*P;
end



A possible implementation of the cordic algorithm could be:


%% Clear all previous values
clear all;

%% Define vectors
A = [2;-3];
O = [0;0];

%% Define accuracy
error_limit = 0.01;

%% Initialize variables
start_angle = pi/2; % 90º
current_angle = start_angle;
acc_angle = 0;

A_rotated = A;
steps = 0;

% Second quadrant (90º - 180º)
if(A_rotated(1) < 0 && A_rotated(2) > 0)
A_rotated = rotate(A_rotated, -pi/2);
acc_angle = pi/2;
% Third quadrant (180º - 270º)
elseif(A_rotated(1) < 0 && A_rotated(2) < 0)
A_rotated = rotate(A_rotated, -pi);
acc_angle = pi;
% Forth quadrant (270º - 360º)
elseif(A_rotated(1) > 0 && A_rotated(2) < 0)
A_rotated = rotate(A_rotated, -3*pi/2);
acc_angle = 3*pi/2;
end

%% Compute angle
% Keep rotating while error is too high
while(abs(A_rotated(2)) > error_limit)
% Represent current vector
quiver(0, 0,A_rotated(1), A_rotated(2));
% Keep previous vectors
hold on;
% Decrease angle rotation
current_angle = current_angle/2;
% (For debugging purposes)
current_angle_deg = current_angle*180/pi;
% Save current error
error = A_rotated(2);
% If y coordinate is still positive
if(error > 0)
% Rotate again conterclockwise
A_rotated = rotate(A_rotated, -1*current_angle);
% Accumulate rotated angle
acc_angle = acc_angle + current_angle;
% If y coordinate is negative
else
% Rotate vector clockwise
A_rotated = rotate(A_rotated, current_angle);
% Substract current angle to the accumulator because we have
% overcome the actual angle value
acc_angle = acc_angle - current_angle;
end

% (For debugging purposes)
acc_angle_deg = acc_angle*180/pi;
% Increase step counter
steps = steps + 1;
end

% Print angle, hypotenuse length and number of steps needed to compute
fprintf('Angle = %f\nHypotenuse = %f\nNumber of steps: %d\n', acc_angle*180/pi, A_rotated(1), steps);

%% Function to rotate vector
function v = rotate(P, theta)
rot = [cos(theta) -sin(theta);
sin(theta) cos(theta)];
v = rot*P;
end



I have coded an interactive applet to illustrate the algorithm. It has been done using the p5.js library. The error limit has been set to $$0.5$$.