plot_trajectory

  Licence:
    This file is part of ACADO Toolkit  - (http://www.acadotoolkit.org/)

    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
    Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
    Developed within the Optimization in Engineering Center (OPTEC) under
    supervision of Moritz Diehl. All rights reserved.

    ACADO Toolkit is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 3 of the License, or (at your option) any later version.

    ACADO Toolkit 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 ACADO Toolkit; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

    Author: Niels Haverbeke, Boris Houska, Hans Joachim Ferreau, David Ariens
    Date: 2008-2010

0001 %  Licence:
0002 %    This file is part of ACADO Toolkit  - (http://www.acadotoolkit.org/)
0003 %
0004 %    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
0005 %    Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
0006 %    Developed within the Optimization in Engineering Center (OPTEC) under
0007 %    supervision of Moritz Diehl. All rights reserved.
0008 %
0009 %    ACADO Toolkit is free software; you can redistribute it and/or
0010 %    modify it under the terms of the GNU Lesser General Public
0011 %    License as published by the Free Software Foundation; either
0012 %    version 3 of the License, or (at your option) any later version.
0013 %
0014 %    ACADO Toolkit is distributed in the hope that it will be useful,
0015 %    but WITHOUT ANY WARRANTY; without even the implied warranty of
0016 %    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
0017 %    Lesser General Public License for more details.
0018 %
0019 %    You should have received a copy of the GNU Lesser General Public
0020 %    License along with ACADO Toolkit; if not, write to the Free Software
0021 %    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
0022 %
0023 %    Author: Niels Haverbeke, Boris Houska, Hans Joachim Ferreau, David Ariens
0024 %    Date: 2008-2010
0025 
0026 
0027 function [ ] = plot_trajectory( trajectory,statesIdx,useSubplots,plotType,isDiscretized )
0028 
0029     statesIdx = statesIdx(:);
0030     [dummy,cols] = size( trajectory );
0031     nx= cols - 1;
0032     [nPlots,dummy] = size( statesIdx );
0033 
0034     if ( nx > 12 )
0035         useSubplots = 1;
0036     end
0037 
0038     if ( useSubplots == 0 )
0039         for i=1:nPlots
0040             plotFigure( trajectory,statesIdx(i),useSubplots,plotType,isDiscretized );
0041         end
0042     else
0043         numOfSubplots = 2*3;
0044         for i=1:numOfSubplots:nPlots
0045             plotIdx = statesIdx(i:min([i+numOfSubplots-1,nPlots]));
0046             plotFigure( trajectory,plotIdx,useSubplots,plotType,isDiscretized );
0047         end
0048     end
0049 
0050 end
0051 
0052 
0053 
0054 function [ ] = plotFigure( trajectory,plotIdx,useSubplots,plotType,isDiscretized )
0055 
0056     figure;
0057 
0058     for i=1:length(plotIdx)
0059         if ( useSubplots == 1 )
0060             %% multiple plots
0061             subplot(2,3,i);
0062             hold on;
0063         end
0064 
0065         plotHandle = plot( trajectory(:,1),trajectory(:,i+1) );
0066         set(plotHandle,'LineWidth',2);
0067 
0068         switch plotType
0069             case 0
0070                 set(plotHandle,'Color','blue');
0071                 legendY = [ 'Differential State ',num2str(plotIdx(i)) ];
0072 
0073             case 1
0074                 set(plotHandle,'Color','red');
0075                 legendY = [ 'Algebraic State ',num2str(plotIdx(i)) ];
0076         end
0077 
0078         switch isDiscretized
0079             case 0
0080                 %% nothing to do
0081 
0082             case 1
0083                 set(plotHandle,'LineStyle','--');
0084                 set(plotHandle,'Marker','s');
0085         end
0086 
0087         ylabel( legendY,'FontSize',14 );
0088     end
0089 
0090 end