minimizeLSQ

Adds an Least Square term minimized. 
 Adds an Least Square term of the form
 0.5* sum_i || ( h(t_i,x(t_i),u(t_i),p(t_i),...) - r ) ||^2_S_i
 Here the sum is over all grid points of the objective grid. The
 Matrix S is assumed to be symmetric and positive (semi-) definite.
 The Function r is called reference and can be specified by the user. 

  Usage:
    >> ocp.minimizeLSQ(h)
    >> ocp.minimizeLSQ(h, r)
    >> ocp.minimizeLSQ(S, h, r)

  Parameters: 
    h       the LSQ-Function    (1 x n  FUNCTION)
    S       a weighting matrix  (n x n  MATRIX)
    r       the reference       
         -either a 1 x n VECTOR  with references for each expression in h
         -or a q x (n+1) MATRIX with the first column timepoints and the next
          columns references for each expression in h (the difference
          being the fact that you can set a time dependant reference)

  Example:
    >> S = [10 0; 0 1];
    >> h = {x1, x2};
    >> r = [0.2, 1.4];
    >> ocp.minimizeLSQ(S, h, r);

    >> r = [0    0.2  1.4;
            1    0.4  1.5;
            1.5  0.4  1.3];
           %time x1   x2
    >> ocp.minimizeLSQ(S, h, r);

  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: David Ariens
    Date: 2010

0001 function minimizeLSQ(obj, varargin)
0002 %Adds an Least Square term minimized.
0003 % Adds an Least Square term of the form
0004 % 0.5* sum_i || ( h(t_i,x(t_i),u(t_i),p(t_i),...) - r ) ||^2_S_i
0005 % Here the sum is over all grid points of the objective grid. The
0006 % Matrix S is assumed to be symmetric and positive (semi-) definite.
0007 % The Function r is called reference and can be specified by the user.
0008 %
0009 %  Usage:
0010 %    >> ocp.minimizeLSQ(h)
0011 %    >> ocp.minimizeLSQ(h, r)
0012 %    >> ocp.minimizeLSQ(S, h, r)
0013 %
0014 %  Parameters:
0015 %    h       the LSQ-Function    (1 x n  FUNCTION)
0016 %    S       a weighting matrix  (n x n  MATRIX)
0017 %    r       the reference
0018 %         -either a 1 x n VECTOR  with references for each expression in h
0019 %         -or a q x (n+1) MATRIX with the first column timepoints and the next
0020 %          columns references for each expression in h (the difference
0021 %          being the fact that you can set a time dependant reference)
0022 %
0023 %  Example:
0024 %    >> S = [10 0; 0 1];
0025 %    >> h = {x1, x2};
0026 %    >> r = [0.2, 1.4];
0027 %    >> ocp.minimizeLSQ(S, h, r);
0028 %
0029 %    >> r = [0    0.2  1.4;
0030 %            1    0.4  1.5;
0031 %            1.5  0.4  1.3];
0032 %           %time x1   x2
0033 %    >> ocp.minimizeLSQ(S, h, r);
0034 %
0035 %  Licence:
0036 %    This file is part of ACADO Toolkit  - (http://www.acadotoolkit.org/)
0037 %
0038 %    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
0039 %    Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
0040 %    Developed within the Optimization in Engineering Center (OPTEC) under
0041 %    supervision of Moritz Diehl. All rights reserved.
0042 %
0043 %    ACADO Toolkit is free software; you can redistribute it and/or
0044 %    modify it under the terms of the GNU Lesser General Public
0045 %    License as published by the Free Software Foundation; either
0046 %    version 3 of the License, or (at your option) any later version.
0047 %
0048 %    ACADO Toolkit is distributed in the hope that it will be useful,
0049 %    but WITHOUT ANY WARRANTY; without even the implied warranty of
0050 %    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
0051 %    Lesser General Public License for more details.
0052 %
0053 %    You should have received a copy of the GNU Lesser General Public
0054 %    License along with ACADO Toolkit; if not, write to the Free Software
0055 %    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
0056 %
0057 %    Author: David Ariens
0058 %    Date: 2010
0059 %
0060 
0061 index = length(obj.minLSQTermh);
0062 
0063 if (length(varargin) == 1) % ocp.minimizeLSQ(h)
0064     h = varargin{1};
0065 
0066     obj.minLSQTermh{index+1} = acado.Function(h);
0067     obj.minLSQTermr{index+1} = {};
0068     obj.minLSQTermS{index+1} = {};
0069     
0070 elseif (length(varargin) == 2)  %ocp.minimizeLSQ(h, r)
0071     h = varargin{1};
0072     r = varargin{2};
0073 
0074     obj.minLSQTermh{index+1} = acado.Function(h);
0075     obj.minLSQTermr{index+1} = obj.checkVectorMatrix(r);
0076     obj.minLSQTermS{index+1} = {};
0077     
0078 elseif (length(varargin) == 3)  %ocp.minimizeLSQ(S, h, r)
0079     h = varargin{2};
0080     r = varargin{3};
0081     S = varargin{1};
0082 
0083     obj.minLSQTermh{index+1} = acado.Function(h);
0084     obj.minLSQTermS{index+1} = acado.Matrix(S);
0085     obj.minLSQTermr{index+1} = obj.checkVectorMatrix(r);
0086 
0087     
0088 else %error
0089    error('ERROR: Invalid minimizeLSQ call. <a href="matlab: help acado.OCP.minimizeLSQ">help acado.OCP.minimizeLSQ</a>'); 
0090 end
0091 
0092 
0093 end