diff --git a/src/kite_control/kite_identification_test.cpp b/src/kite_control/kite_identification_test.cpp new file mode 100644 index 0000000..7e61e66 --- /dev/null +++ b/src/kite_control/kite_identification_test.cpp @@ -0,0 +1,189 @@ +#include "kiteNMPF.h" + +#define BOOST_TEST_MODULE kite_identification_test +#include +#include +#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 kite = std::make_shared(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 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()