parameter_estimation_tutorial2.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00035 #include <acado_optimal_control.hpp>
00036 #include <acado_gnuplot.hpp>
00037 
00038 
00039 int main( ){
00040 
00041     USING_NAMESPACE_ACADO
00042 
00043 
00044     // INTRODUCE THE VARIABLES:
00045     // -------------------------
00046 
00047     DifferentialState      phi;    // the angle phi
00048     DifferentialState     dphi;    // the first derivative of phi w.r.t. time
00049 
00050     Parameter                l;    // the length of the pendulum
00051     Parameter            alpha;    // frictional constant
00052     Parameter                g;    // the gravitational constant
00053 
00054     Control                  F;    // force acting on the pendulum
00055                                    // (control input)
00056 
00057 
00058     double const m = 1.0;
00059 
00060 
00061     // DEFINE A DIFFERENTIAL EQUATION:
00062     // -------------------------------
00063 
00064     DifferentialEquation f;
00065 
00066     f << dot(phi ) == dphi;
00067     f << dot(dphi) == -(g/l)*sin(phi) - alpha*dphi + F/m;
00068 
00069 
00070     // REMARK: Note that the parameters g, and l are not independent.
00071     //         Only one of these parameters can be estimated from
00072     //         the measurements of the dynamic motion.
00073     // -----------------------------------------------------------------
00074 
00075 
00076     // DEFINE A MEASUREMENT FUNCTION:
00077     // ------------------------------
00078 
00079     Function h;
00080     h <<   phi;  // The state phi is being measured.
00081 
00082 
00083    // DEFINE THE INVERSE OF THE VARIANCE-COVARIANCE MATRIX OF THE MEASUREMENTS:
00084    // -------------------------------------------------------------------------
00085     DMatrix S(1,1);
00086     S(0,0) = 1.0/pow(0.1,2);  // (1 over the variance of the measurement)
00087                               // HERE: the standard deviation of the measurement is
00088                               // assumed to be 0.1, thus S = 1/(0.1)^2.
00089 
00090 
00091     // READ THE MEASUREMENT FROM A DATA FILE:
00092     // --------------------------------------
00093 
00094     VariablesGrid measurements;
00095     measurements.read( "parameter_estimation_data2.txt" );
00096 
00097     if( measurements.isEmpty() == BT_TRUE )
00098         printf("The file \"parameter_estimation_data2.txt\" can't be opened.");
00099 
00100 
00101 
00102     // READ THE CONTROL INPUT FROM A DATA FILE:
00103     // ----------------------------------------
00104 
00105     VariablesGrid F_reference;
00106     F_reference.read( "parameter_estimation_controls.txt" );
00107 
00108     if( F_reference.isEmpty() == BT_TRUE )
00109         printf("The file \"parameter_estimation_controls.txt\" can't be opened.");
00110 
00111 
00112 
00113     // DEFINE A PARAMETER ESTIMATION PROBLEM:
00114     // --------------------------------------
00115     OCP ocp( measurements.getTimePoints() );
00116 
00117     ocp.minimizeLSQ( S, h, measurements );
00118     ocp.subjectTo( f );
00119 
00120     ocp.subjectTo( 0.0 <= alpha <= 4.0  );
00121     ocp.subjectTo( 0.0 <=   l   <= 2.0  );
00122 
00123     ocp.subjectTo( F == F_reference(0)  );
00124     ocp.subjectTo( g == 9.81            );
00125 
00126 
00127     // SETUP AN PLOT WINDOW:
00128     // ---------------------------------------------------
00129     GnuplotWindow window( PLOT_NEVER );
00130     
00131     window.addSubplot( phi,  "The angle  phi" );
00132     window.addSubplot( dphi, "The angular velocity  dphi   " );
00133     window.addSubplot( l,    "The length of the pendulum  l" );
00134     window.addSubplot( alpha,"Frictional constant  alpha   " );
00135     window.addSubplot( F,    "Control input (force) F" );
00136 
00137 
00138     // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
00139     // ---------------------------------------------------
00140     ParameterEstimationAlgorithm algorithm(ocp);
00141 
00142         algorithm << window;
00143     algorithm.initializeDifferentialStates( "parameter_estimation_data2.txt" );
00144         algorithm.set(LEVENBERG_MARQUARDT, 1e-5);
00145 
00146     algorithm.solve();
00147 
00148 
00149     // GET THE OPTIMAL PARAMETERS:
00150     // -----------------------------------
00151     VariablesGrid parameters;
00152     algorithm.getParameters( parameters );
00153 
00154 
00155     printf("\n\nResults for the parameters: \n");
00156     printf("-----------------------------------------------\n");
00157     printf("   l      =  %.3e   \n", parameters(0,0)  );
00158     printf("   alpha  =  %.3e   \n", parameters(0,1)  );
00159     printf("   g      =  %.3e   \n", parameters(0,2)  );
00160     printf("-----------------------------------------------\n\n\n");
00161 
00162 
00163     // PLOT THE RESULT:
00164     // ---------------------------------------------------
00165     algorithm.getPlotWindow( window );
00166 
00167     window.addData( 0, measurements(0) );
00168 
00169         window.plot( );
00170 
00171     return 0;
00172 }
00173 
00174 
00175 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:23