2019-01-10

Author: rodralez

correct_yaw.m

@@ -0,0 +1,44 @@
+function yaw_new = correct_yaw (yaw)
+% correct_yaw: guarantee that yaw angle is between -pi and pi.
+%
+% INPUT:
+%   yaw, Nx1 vector (radians).
+%
+% OUTPUT:
+%   yaw_new, Nx1 vector (radians).
+%
+%
+%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+%   This file is part of NaveGo, an open-source MATLAB toolbox for
+%   simulation of integrated navigation systems.
+%
+%   NaveGo is free software: you can redistribute it and/or modify
+%   it under the terms of the GNU Lesser General Public License (LGPL)
+%   version 3 as published by the Free Software Foundation.
+%
+%   This program is distributed in the hope that it will be useful,
+%   but WITHOUT ANY WARRANTY; without even the implied warranty of
+%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+%   GNU Lesser General Public License for more details.
+%
+%   You should have received a copy of the GNU Lesser General Public
+%   License along with this program. If not, see
+%   <http://www.gnu.org/licenses/>.
+%
+% References:
+%
+% Version: 001
+% Date:    2019/01/09
+% Author:  Rodrigo Gonzalez <[email protected]>
+% URL:     https://github.com/rodralez/navego
+
+yaw_new = yaw;
+
+idx = (yaw > pi);
+yaw_new(idx) = yaw(idx) - 2*pi;
+
+idx = (yaw < -pi);
+yaw_new(idx) = yaw(idx) + 2*pi;
+
+end