Homec4science
Diffusion test_package (master) Browse

R1517/src/kite_control/kite_identification_test.cpp
cb661f53e1edmaster

Last Change
Raw File

kite_identification_test.cpp
View Options

#include "kiteNMPF.h"
#define BOOST_TEST_MODULE kite_identification_test
#include <boost/test/included/unit_test.hpp>
#include <fstream>
#include "pseudospectral/chebyshev.hpp"
using namespace casadi;
BOOST_AUTO_TEST_SUITE( kite_identification_test )
BOOST_AUTO_TEST_CASE( first_id_test )
{
/** Load identification data */
std::ifstream id_data_file("id_data_state.txt", std::ios::in);
std::ifstream id_control_file("id_data_control.txt", std::ios::in);
const int DATA_POINTS = 26;
DM id_data = DM::zeros(13, DATA_POINTS);
DM id_control = DM::zeros(3, DATA_POINTS);
/** load state trajectory */
if(!id_data_file.fail())
{
for(uint i = 0; i < DATA_POINTS; ++i) {
for(uint j = 0; j < 13; ++j){
double entry;
id_data_file >> entry;
id_data(j,i) = entry;
}
}
}
else
{
std::cout << "Could not open : id state data file \n";
id_data_file.clear();
}
/** load control data */
if(!id_control_file.fail())
{
for(uint i = 0; i < DATA_POINTS; ++i){
for(uint j = 0; j < 3; ++j){
double entry;
id_control_file >> entry;
/** put in reverse order to comply with Chebyshev method */
id_control(j,DATA_POINTS - 1 - i) = entry;
}
}
}
else
{
std::cout << "Could not open : id control data file \n";
id_control_file.clear();
}
/** define kite dynamics */
std::string kite_params_file = "umx_radian.yaml";
KiteProperties kite_props = kite_utils::LoadProperties(kite_params_file);
AlgorithmProperties algo_props;
algo_props.Integrator = CVODES;
algo_props.sampling_time = 0.02;
std::shared_ptr<KiteDynamics> kite = std::make_shared<KiteDynamics>(kite_props, algo_props, true);
/** get dynamics function and state Jacobian */
Function DynamicsFunc = kite->getNumericDynamics();
SX X = kite->getSymbolicState();
SX U = kite->getSymbolicControl();
SX P = kite->getSymbolicParameters();
//DM Q = SX::diag(SX({1e3, 1e3, 1e3, 1e2, 1e2, 1e2, 1e1, 1e1, 1e1, 1e2, 1e2, 1e2, 1e2})); best so far
DM Q = SX::diag(SX({1e3, 1e3, 1e3, 1e2, 1e2, 1e3, 1e1, 1e1, 1e3, 1e2, 1e2, 1e2, 1e2})); //good one as well
//DM Q = SX::diag(SX({0, 0, 0, 0, 0, 0, 1e2, 1e2, 1e3, 1e3, 1e3, 1e3, 1e3}));
//DM R = 1e-4 * SX::diag(SX({1, 1, 1, 1}));
/** state bounds */
DM LBX = DM::vertcat({2.0, -DM::inf(1), -DM::inf(1), -4 * M_PI, -4 * M_PI, -4 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
-1.01, -1.01, -1.01, -1.01});
DM UBX = DM::vertcat({DM::inf(1), DM::inf(1), DM::inf(1), 4 * M_PI, 4 * M_PI, 4 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
1.01, 1.01, 1.01, 1.01});
/** control bounds */
DM LBU = DM::vec(id_control);
DM UBU = DM::vec(id_control);
/** parameter bounds */
//DM REF_P = DM::vertcat({4.483, 0.023, -0.1135, 0.005, -0.4118, 0.0248, -0.084, 7.170, -12.665, 0.085,
// -0.0228, 0.0686, -0.1451, -0.5634, -0.0367, 0.45, 0.132, -1.313, -0.037, 0.0055}); old
DM REF_P = DM::vertcat({3.9, 0.032, -0.1702, 0.0025, -0.6177, 0.0124, -0.126, 10.755, -13.78, 0.127,
-0.034, 0.0342, -0.072, -0.413, -0.0185, 0.675, 0.066, -0.656, -0.055, 0.0082});
DM LBP = REF_P - 0.3 * fabs(REF_P);
DM UBP = REF_P + 0.3 * fabs(REF_P);
LBP[0] = 4.5;
LBP[1] = 0.032;
// aggresive optimization of control derivatives
LBP(Slice(15, 20),0) = REF_P(Slice(15, 20),0) - 2 * fabs(REF_P(Slice(15, 20), 0));
UBP(Slice(15, 20),0) = REF_P(Slice(15, 20),0) + 2 * fabs(REF_P(Slice(15, 20),0));
LBP[15] = REF_P[15];
//LBP[15] = REF_P[15];
//LBP[17] = REF_P[17];
std::cout << "OK so far \n";
/** ----------------------------------------------------------------------------------*/
const int num_segments = 1;
const int poly_order = 25;
const int dimx = 13;
const int dimu = 3;
const int dimp = 20;
Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> spectral;
SX diff_constr = spectral.CollocateDynamics(DynamicsFunc, 0, 5);
diff_constr = diff_constr(casadi::Slice(0, poly_order * dimx));
SX varx = spectral.VarX();
SX varu = spectral.VarU();
SX varp = spectral.VarP();
SX opt_var = SX::vertcat(SXVector{varx, varu, varp});
SX lbg = SX::zeros(diff_constr.size());
SX ubg = SX::zeros(diff_constr.size());
/** set inequality (box) constraints */
/** state */
SX lbx = SX::repmat(LBX, poly_order + 1, 1);
SX ubx = SX::repmat(UBX, poly_order + 1, 1);
/** control */
//lbx = SX::vertcat( {lbx, SX::repmat(LBU, poly_order + 1, 1)} );
//ubx = SX::vertcat( {ubx, SX::repmat(UBU, poly_order + 1, 1)} );
lbx = SX::vertcat({lbx, LBU});
ubx = SX::vertcat({ubx, UBU});
/** parameters */
lbx = SX::vertcat({lbx, LBP});
ubx = SX::vertcat({ubx, UBP});
SX fitting_error = 0;
SX varx_ = SX::reshape(varx, 13, DATA_POINTS);
for (uint j = 0; j < DATA_POINTS; ++j)
{
SX measurement = id_data(Slice(0, id_data.size1()), j);
SX error = measurement - varx_(Slice(0, varx_.size1()), varx_.size2() - j - 1);
fitting_error = fitting_error + SX::sumRows( SX::mtimes(Q, pow(error, 2)) );
}
/** formulate NLP */
SXDict NLP;
Dict OPTS;
DMDict ARG;
NLP["x"] = opt_var;
NLP["f"] = fitting_error;
NLP["g"] = diff_constr;
OPTS["ipopt.linear_solver"] = "ma97";
OPTS["ipopt.print_level"] = 5;
OPTS["ipopt.tol"] = 1e-5;
OPTS["ipopt.acceptable_tol"] = 1e-4;
//OPTS["ipopt.max_iter"] = 20;
Function NLP_Solver = nlpsol("solver", "ipopt", NLP, OPTS);
/** set default args */
ARG["lbx"] = lbx;
ARG["ubx"] = ubx;
ARG["lbg"] = lbg;
ARG["ubg"] = ubg;
DM feasible_state = id_data(Slice(0, id_data.size1()), 0);
DM feasible_control = (UBU + LBU) / 2;
//ARG["x0"] = DM::vertcat(DMVector{DM::repmat(feasible_state, poly_order + 1, 1),
// DM::repmat(feasible_control, poly_order + 1, 1),
//
ARG["x0"] = DM::vertcat(DMVector{DM::repmat(feasible_state, poly_order + 1, 1), feasible_control, REF_P});
int idx_in = poly_order * dimx;
int idx_out = idx_in + dimx;
ARG["lbx"](Slice(idx_in, idx_out), 0) = feasible_state;
ARG["ubx"](Slice(idx_in, idx_out), 0) = feasible_state;
DMDict res = NLP_Solver(ARG);
DM result = res.at("x");
std::cout << result(Slice(result.size1() - 20, result.size1()));
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_SUITE_END()
c4science · Help