function theta_normalized = normalize_theta(theta);

% Function to bound any angle between -pi and pi:
% inputs: theta (rad)
% outputs: theta_normalized (rad) 
% -pi < theta_normalized < pi

[row col] = size(theta);
theta = theta - floor(theta/(2.0*pi))*2.0*pi;
for i = 1:length(theta)
    if theta(i) > pi
        theta_normalized(i) = theta(i) - 2.0*pi;
    elseif theta(i) < -pi
        theta_normalized(i) = theta(i) + 2.0*pi;
    else
        theta_normalized(i) = theta(i);
    end
end

if row > col
    theta_normalized = theta_normalized';
end