diff --git a/src/casadi_test.cpp b/src/casadi_test.cpp
index 1c6c21e..0f468b4 100644
--- a/src/casadi_test.cpp
+++ b/src/casadi_test.cpp
@@ -1,123 +1,125 @@
#include "casadi/casadi.hpp"
#include "yaml-cpp/yaml.h"
#include "ctime"
#include "chrono"
#include "ros/time.h"
#include "eigen3/Eigen/Dense"
using namespace casadi;
int main()
{
//load config information
//YAML::Node config = YAML::LoadFile("umx_radian.yaml");
//const std::string plane = config["name"].as<std::string>();
//const double aspect_ratio = config["geometry"]["AR"].as<double>();
SX sym = SX::sym("sym",4);
SX x = SX::sym("x",3);
std::vector<SX> y{sym, x, 0};
SXVector vect{sym,x,0};
SX var = SX::vertcat({sym, x, 0});
Slice slice(1,5);
auto sliced = var(slice,0);
sliced(Slice(1,4),0) = x;
SX z = SX::sym("z");
SX z2 = SX::sym("z2");
Function f = Function("test", {z,z2}, {z + z2});
SXVector res = f(SXVector{z,z2});
SX func = SX::vertcat(res);
SX jac = SX::jacobian(func,z);
DM Matr = DM::diag(pow(DMVector{1,2,3,4},2));
std::chrono::time_point<std::chrono::system_clock> now = std::chrono::system_clock::now();
auto duration = now.time_since_epoch();
auto seconds = std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
double ros_time = static_cast<double>(seconds);
printf("Local time is %f \n", ros_time * 1e-6);
ros::Time::init();
double ros_true_time = ros::Time::now().toSec();
printf("True Local time is %f \n", ros_true_time);
std::cout << "Local time is: " << ros_time * 1e-6 << "\n";
DM H = DM::horzcat(DMVector{DM::zeros(7,6), DM::eye(7)});
std::cout << H << "\n";
SX J = SX::diag(x);
SX Jx = SX::mtimes(J, x);
std::cout << "\n" << "J " << Jx << "\n";
std::pair<double, double> t_pair = std::pair<double, double>(1.5,2);
std::cout << "Pair First: " << t_pair.first << " Pair Second: " << t_pair.second << "\n";
DM vect_test = DM::vertcat({0,1});
vect_test = cos(vect_test);
std::cout << "Vector COS: " << vect_test << "\n";
//////////////////////////////////////////////////////////
auto grid = range(0,10);
DMVector grid_DM(grid.begin(), grid.end());
std::cout << "GRID: " << DM::vertcat(grid_DM) + 1 << "\n";
DM c = DM::vertcat(grid_DM);
c = c * c;
DM _c = pow(-1, c);
std::cout << "C: " << c << " => " << _c << "\n";
std::cout << "INF: " << -DM::inf(10) << "\n";
SX Z = SX::sym("Z", 3,4);
printf("COLS: %d, ROWS: %d \n", Z.size1(), Z.size2());
std::cout << SX::vec(Z) << "\n";
SX q = SX::sym("q",3);
+ std::cout << q(0) << "\n";
SX r = SX::sym("r", 3);
Function X2 = Function("X2", {q}, {pow(q,2)});
SXVector z_vec = SX::horzsplit(Z,1);
SXVector result = X2(*(z_vec.begin()));
std::cout << result << "\n";
std::cout << SX::blockcat({{SX::eye(3), SX::zeros(3,3)}, {SX::zeros(3,3), SX::zeros(3,3)}}) << "\n";
std::cout << SX::diagcat({SX::eye(5), 5 * SX::eye(4)}) << "\n";
std::cout << "INF DIFF: " << DM::inf(1) - DM::inf(1) << "\n";
std::cout << pow(Matr, 2) << "\n";
std::cout << jac << " " << "\n";
//std::cout << "aircraft name: " << plane << " " <<"with Aspect Ratio= "<< aspect_ratio <<"\n";
///////////////////////////////////////////////////////////
//// EIGEN TEST //////
Eigen::VectorXd eig_vec = Eigen::VectorXd::Map(c.nonzeros().data(), c.nonzeros().size());
std::cout << eig_vec << "\n";
std::cout << DM::sumRows(c) << "\n";
DM EYE = DM::eye(7);
Eigen::Matrix<double, 7, 7> eig_mat = Eigen::Matrix<double, 7, 7>::Map(DM::densify(EYE).nonzeros().data(), 7, 7);
std::cout << eig_mat << "\n";
std::cout << "\n" << "----------------------------------------------------------------------------------" << "\n";
int32_t number = 1900;
std::cout << "\n CAST DOUBLE" << static_cast<double>(number) << "\n";
std::string func_name = "RK4_somethNing";
if (func_name.find("RK4") != std::string::npos)
std::cout << "Found RK4 \n";
/** CAST BACK */
Eigen::Matrix<SXElem, 2, 1> sol;
sol(0) = SXElem::sym("lox");
sol(1) = SXElem::sym("pidor");
std::vector<SXElem> sold;
sold.resize(static_cast<size_t>(sol.size()));
Eigen::Map<Eigen::Matrix<SXElem, 2, 1>>(&sold[0], sol.size()) = sol;
+
return 0;
}
diff --git a/src/kite_control/kite_control_test.cpp b/src/kite_control/kite_control_test.cpp
index 6e8b75c..307b6a6 100644
--- a/src/kite_control/kite_control_test.cpp
+++ b/src/kite_control/kite_control_test.cpp
@@ -1,295 +1,311 @@
#include "kiteEKF.h"
#include "kiteNMPF.h"
#define BOOST_TEST_MODULE kite_control_test
#include <boost/test/included/unit_test.hpp>
#include <fstream>
#include "pseudospectral/chebyshev.hpp"
using namespace casadi;
BOOST_AUTO_TEST_SUITE( kite_control_suite_test )
BOOST_AUTO_TEST_CASE( integrator_test )
{
AlgorithmProperties algo_props;
algo_props.Integrator = CVODES;
algo_props.sampling_time = 0.02;
/** create a rigid body object */
RigidBodyKinematics rigid_body = RigidBodyKinematics(algo_props);
Function fIntegrator = rigid_body.getNumericIntegrator();
Function fJacobian = rigid_body.getNumericJacobian();
Function fDynamics = rigid_body.getNumericDynamcis();
/** perform integration step */
DM init_state = DM::vertcat({-0.318732, 0.182552, 0.254833, 1.85435, -0.142882, -0.168359,
-0.229383, -0.0500282, -0.746832, 0.189409, -0.836349, -0.48178, 0.180367});
DM dynamics = fDynamics(DMVector{init_state});
std::cout << "Dynamics: " << dynamics[0] << "\n";
DM control = DM::zeros(3);
//DMDict args = {{"x0", init_state}, {"p", control}};
DMDict args = {{"x0", init_state}};
DMDict out = fIntegrator(args);
DM new_state = out["xf"];
std::cout << "Integration: init_state: " << init_state << "\n";
std::cout << "Integration: new_state: " << new_state << "\n" << "\n";
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( ekf_test )
{
/** make test data */
double dt = 0.0084;
DM control = DM::vertcat({0,0,0});
DM measurement = DM::vertcat({1.4522, -3.1274, -1.7034, -0.5455, -0.2382, -0.2922, -0.7485});
DM x_est = DM::vertcat({6.0026, -0.3965, 0.1705,
0.4414, -0.2068, 0.9293,
1.4634, -3.1765, -1.7037,
-0.5486, -0.2354, -0.2922, -0.7471});
/** reference estimation from matlab */
DM reference_est = DM::vertcat({5.9982, -0.3819, 0.1637,
0.3578, -0.1900, 0.8774,
1.4522, -3.1274, -1.7034,
-0.5455, -0.2382, -0.2922, -0.7485});
/** create a filter and perform estimation */
std::string kite_config_file = "umx_radian.yaml";
KiteProperties kite_props = kite_utils::LoadProperties(kite_config_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
KiteEKF estimator = KiteEKF(kite_props, algo_props);
estimator.setControl(control);
estimator.setEstimation(x_est);
std::chrono::time_point<std::chrono::system_clock> start = kite_utils::get_time();
estimator._estimate(measurement, dt);
x_est = estimator.getEstimation();
//estimator.propagate(dt);
std::chrono::time_point<std::chrono::system_clock> stop = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
auto latency = static_cast<double>(duration.count());
std::cout << "EKF_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
//BOOST_CHECK(DM::norm_inf(x_est - reference_est).nonzeros()[0] < 0.01);
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( collocation_test )
{
/** load data from the log file */
std::ifstream log_file;
int nrows = 100;
int ncols = 14;
std::vector<std::vector<double>> data_matrix(nrows, std::vector<double>(ncols));
log_file.open("kite_state_cut.log", std::ios::in);
if(!log_file)
std::cerr << "Unable to open lof file: " << "kite_state_cut.log" << "\n";
for (int i = 0; i < nrows; ++i)
for(int j = 0; j < ncols; ++j)
log_file >> data_matrix[i][j];
DM kite_states(data_matrix);
/** create a controller and solve an OCP */
std::string kite_config_file = "umx_radian.yaml";
KiteProperties kite_props = kite_utils::LoadProperties(kite_config_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
/** set up the path */
SX x = SX::sym("x");
double radius = 2.31;
double altitude = 0.00;
SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude});
Function path_fun = Function("path", {x}, {Path});
std::shared_ptr<KiteDynamics> kite = std::make_shared<KiteDynamics>(kite_props, algo_props);
/** Kite Dynamics debugging */
Function DYNAMO = kite->getNumericDynamics();
DM x0 = DM({1.5, 0, 0, 0, 0, 0, 0, 1.0, 0, 1, 0, 0.0, 0.0});
DM u0 = DM({0.1, 0, 0});
DMVector eval = DYNAMO(DMVector{x0,u0});
std::cout << "Kite Dynamics: " << eval[0] << "\n";
KiteNMPF controller = KiteNMPF(kite, path_fun);
/** set control constraints */
double angle_sat = kmath::deg2rad(8.0);
DM lbu = DM::vertcat({0, -angle_sat, -angle_sat, -10});
DM ubu = DM::vertcat({0.3, angle_sat, angle_sat, 10});
controller.setLBU(lbu);
controller.setUBU(ubu);
/** set variable constraints */
DM lbx = DM::vertcat({0.5, -0.5, -DM::inf(1), -2 * M_PI, -2 * M_PI, -2 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
-1, -1, -1, -1, -DM::inf(1), -DM::inf(1)});
DM ubx = DM::vertcat({12, 5, DM::inf(1), 2 * M_PI, 2 * M_PI, 2 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
1, 1, 1, 1, DM::inf(1), DM::inf(1)});
controller.setLBX(lbx);
controller.setUBX(ubx);
/** set reference velocity */
DM vel_ref = 0.05;
controller.setReferenceVelocity(vel_ref);
/** set scaling of system ODEs */
double vx_max = ubx.nonzeros()[0];
double vy_max = ubx.nonzeros()[1];
double wx_max = ubx.nonzeros()[3];
double wy_max = ubx.nonzeros()[4];
double wz_max = ubx.nonzeros()[5];
double rsphere = 5;
//DM Sx = DM::diag(DM::vertcat(DMVector{1/vx_max, 1/vy_max, 1, 1/wx_max, 1/wy_max, 1/wz_max,
// 1/rsphere, 1/rsphere, 1/rsphere, 1, 1, 1, 1, 1/(2*M_PI), 1/vx_max}));
DM Sx = DM::eye(15);
/** control */
DM Su = DM::eye(4);
controller.setStateScaling(Sx);
controller.setControlScaling(Su);
/** create path following NLP */
controller.createNLP();
/** solve for initial conditions */
DM X0 = DM::vertcat({1.5, 0, 0, 0, 0, 0,
0, -1.0, 0, 1, 0, 0.0, 0.0, M_PI_2, 0});
std::chrono::time_point<std::chrono::system_clock> start = kite_utils::get_time();
controller.computeControl(X0);
std::chrono::time_point<std::chrono::system_clock> stop = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
auto latency = static_cast<double>(duration.count());
DM opt_x = controller.getOptimalTrajetory();
std::cout << "Optimal Solution: \n" << opt_x << "\n";
std::cout << "COLLOCATION_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
/** initialization */
DM theta0 = controller.findClosestPointOnPath(X0(Slice(6,9)));
std::cout << "INIT: " << theta0 << "\n";
/** process real trajectory */
bool success = true;
//for(int i = 0; i < kite_states.size1(); ++i)
for(int i = 0; i < 1; ++i)
{
start = kite_utils::get_time();
X0 = kite_states(i, Slice(1, kite_states.size2()));
X0 = DM::horzcat({X0, 0, 0}).T();
controller.computeControl(X0);
stop = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
latency = static_cast<double>(duration.count());
std::cout << "COLLOCATION_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
}
BOOST_CHECK(success);
}
BOOST_AUTO_TEST_CASE( lqr_test )
{
/** lyapunov test */
Eigen::MatrixXd A(2,2), Q(2,2);
A << 1, 2, -3, -4;
Q << 3, 1, 1, 1;
kite_utils::time_point start = kite_utils::get_time();
Eigen::MatrixXd X = kmath::oc::lyapunov(A, Q);
kite_utils::time_point finish = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
auto latency = static_cast<double>(duration.count());
std::cout << "Lyapunov equation solution: \n" << X << "\n";
std::cout << (A * X + X * A.transpose()) << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
/** PINV test */
start = kite_utils::get_time();
Eigen::MatrixXd pinvA = kmath::oc::pinv(A);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "pinv: " << pinvA << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
/** INIT Newton test */
X = kmath::oc::init_newton_care(A, Q);
std::cout << "Init: \n" << X << "\n";
Eigen::MatrixXd A1(2,2), B1(2,2), C1(2,2);
A1 << -3, 2, 1, 1;
B1 << 0, 0, 0, 1;
C1 << 1, -1, -1, 1;
start = kite_utils::get_time();
Eigen::MatrixXd S = kmath::oc::care(A1, B1, C1);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "CARE solved: \n" << S << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
std::cout <<"-----------------------Controllability test------------------------\n";
Eigen::MatrixXd Ac(2,2), Bc(2,2), Qc(2,2), R(2,2), M(2,2);
Ac << 1,1,4,-2;
Bc << Eigen::MatrixXd::Identity(2,2);
Qc = Eigen::MatrixXd::Identity(2,2);
R = Eigen::MatrixXd::Identity(2,2);
M = Eigen::MatrixXd::Zero(2,2); //0.25 * Eigen::MatrixXd::Identity(2,2);
kmath::LinearSystem sys(Ac, Bc, Eigen::MatrixXd());
std::cout << "Controllable: " << sys.is_controllable() << "\n";
std::cout <<"-----------------------LQR test------------------------\n";
start = kite_utils::get_time();
Eigen::MatrixXd K = kmath::oc::lqr(sys, Qc, R, M);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "Feedback matrix: \n" << K << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( pseudo_test )
{
- const int poly_order = 1;
- const int num_segments = 3;
- const int dimx = 2;
- Chebyshev<SX, poly_order, num_segments, dimx> cheb;
+ const int poly_order = 3;
+ const int num_segments = 2;
+ const int dimx = 1;
+ const int dimu = 1;
+ const int dimp = 1;
+ Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> cheb;
std::cout << "----------------------------------------------- \n";
std::cout << "Collocation Matrix \n" << cheb.CPoints() << " \n";
std::cout << "Differentiation Matrix \n" << cheb.D() << "\n";
std::cout << "Quadrature weights \n" << cheb.QWeights() << "\n";
std::cout << "Composite Diff matrix \n" << cheb.CompD() << "\n";
std::cout << "----------------------------------------------- \n";
+ SX x = SX::sym("x");
+ SX u = SX::sym("u");
+ SX f = x + u;
+ Function dynamics = Function("rhs", {x,u}, {f});
+ std::cout << "X var : " << cheb.VarX() << "\n";
+ std::cout << "U var : " << cheb.VarU() << "\n";
+ SX g = cheb.CollocateDynamics(dynamics, 0, 1);
+ std::cout << "Collocated Dynamics \n" << g << "\n";
+
+ std::cout << "Chebyshev Expansion : \n";
+ std::vector<double> coeff = {1, 2, 3, 4};
+ double value = -1;
+ double expansion = kmath::chebyshev_expansion<double>(coeff, value);
+ double expansion2 = kmath::chebyshev_expansion2<double>(coeff, value);
+ std::cout << expansion << "\n";
+ std::cout << expansion2 << "\n";
+
DM cp, D;
kmath::cheb(cp, D, poly_order, std::make_pair<double>(-1,1));
std::cout << "ref collocation points: \n" << cp << "\n";
std::cout << "ref diff matrix: \n" << D << "\n";
- //Chebyshev<Eigen::MatrixXd, 3, 1> abel;
- //std::cout << "Diff Matrix \n" << abel.D() << "\n Eigen \n";
-
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_SUITE_END()
diff --git a/src/kite_math/kitemath.cpp b/src/kite_math/kitemath.cpp
index 56d07f2..daa2f36 100644
--- a/src/kite_math/kitemath.cpp
+++ b/src/kite_math/kitemath.cpp
@@ -1,328 +1,337 @@
#include "kitemath.h"
using namespace casadi;
namespace kmath
{
SX quat_multiply(const SX &q1, const SX &q2)
{
SX s1 = q1[0];
SX v1 = q1(Slice(1,4),0);
SX s2 = q2[0];
SX v2 = q2(Slice(1,4),0);
SX s = (s1 * s2) - SX::dot(v1, v2);
SX v = SX::cross(v1, v2) + (s1 * v2) + (s2 * v1);
SXVector tmp{s,v};
return SX::vertcat(tmp);
}
SX quat_inverse(const SX &q)
{
SXVector tmp{q[0], -q[1], -q[2], -q[3]};
return SX::vertcat(tmp);
}
SX heaviside(const SX &x, const double K)
{
return K / (1 + exp(-4 * x));
}
SX rk4_symbolic(const SX &x,
const SX &u,
Function &func,
const SX &h)
{
SXVector res = func(SXVector{x, u});
SX k1 = res[0];
res = func(SXVector{x + 0.5 * h * k1, u});
SX k2 = res[0];
res = func(SXVector{x + 0.5 * h * k2, u});
SX k3 = res[0];
res = func(SXVector{x + h * k3, u});
SX k4 = res[0];
return x + (h/6) * (k1 + 2*k2 + 2*k3 + k4);
}
void cheb(DM &CollocPoints, DM &DiffMatrix, const unsigned &N,
const std::pair<double, double> interval = std::make_pair(0,1))
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto grid_int = casadi::range(0, N+1);
/** cast grid to Casadi type */
DMVector DMgrid(grid_int.begin(), grid_int.end());
DM grid = DM::vertcat(DMgrid);
DM X = cos(grid * (M_PI / N));
/** shift and scale points */
CollocPoints = (X + interval.first + 1) * ((interval.second - interval.first) / 2);
/** Compute Differentiation matrix */
DM c = DM::vertcat({2, DM::ones(N-1,1), 2});
c = SX::mtimes(SX::diag( pow(-1, grid)), c);
DM XM = DM::repmat(CollocPoints, 1, N+1);
DM dX = XM - XM.T();
DM Dn = DM::mtimes(c, (1 / c).T() ) / (dX + (DM::eye(N+1))); /** off-diagonal entries */
DiffMatrix = Dn - DM::diag( DM::sumRows(Dn.T() )); /** diagonal entries */
}
SX mat_func(const SX &matrix_in, Function &func)
{
SXVector columns = SX::horzsplit(matrix_in, 1);
/** evaluate fnuction for each column and stack in a single vector*/
SX output_vec;
for(auto it = columns.begin(); it != columns.end(); ++it)
{
SX res = SX::vertcat(func(*it));
/** append to resulting vector*/
output_vec = SX::vertcat(SXVector{output_vec, res});
}
return output_vec;
}
SX mat_dynamics(const SX &arg_x, const SX &arg_u, Function &func)
{
SXVector xdot;
SXVector x = SX::horzsplit(arg_x, 1);
SXVector u = SX::horzsplit(arg_u, 1);
for(uint i = 0; i < u.size(); ++i)
{
SXVector eval = func(SXVector{x[i], u[i]});
xdot.push_back(eval[0]);
}
/** discard the initial state */
xdot.push_back(x.back());
return SX::vertcat(xdot);
}
/** Linear system implementation */
bool LinearSystem::is_controllable()
{
/** compute controllability matrix */
int n = F.rows();
int m = G.cols();
Eigen::MatrixXd ctrb = Eigen::MatrixXd::Zero(n, n * m);
for(int k = 0; k < n; ++k)
{
ctrb.block(0, k*m, n, m) = F.pow(k) * G;
}
Eigen::FullPivLU<Eigen::MatrixXd> chol(ctrb);
if (chol.rank() == n)
return true;
else
return false;
}
+uint factorial(const uint &n)
+{
+ uint fact = 1;
+ for (int i = 1; i <= n; ++i)
+ fact *= i;
+
+ return fact;
+}
+
namespace oc {
/** Lyapunov equation */
Eigen::MatrixXd lyapunov(const Eigen::MatrixXd &A, const Eigen::MatrixXd &Q)
{
int m = Q.rows();
/** compute Schur decomposition of A */
Eigen::RealSchur<Eigen::MatrixXd> schur(A);
Eigen::MatrixXd T = schur.matrixT();
Eigen::MatrixXd U = schur.matrixU();
Eigen::MatrixXd Q1 = (U.transpose() * Q) * U;
Eigen::MatrixXd X = Eigen::MatrixXd::Zero(m, m);
Eigen::MatrixXd E = Eigen::MatrixXd::Identity(m,m);
X.col(m-1) = (T + T(m-1,m-1) * E).partialPivLu().solve(Q1.col(m-1));
for(int i = m-2; i >= 0; --i)
{
Eigen::VectorXd v = Q1.col(i) - X.block(0, i+1, m, m-(i+1)) * T.block(i, i+1, 1, m-(i+1)).transpose();
X.col(i) = (T + T(i,i) * E).partialPivLu().solve(v);
}
X = (U * X) * U.transpose();
return X;
}
/** CARE Newton iteration */
Eigen::MatrixXd newton_ls_care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
const Eigen::MatrixXd &C, const Eigen::MatrixXd &X0)
{
/** initial guess */
Eigen::EigenSolver<Eigen::MatrixXd> eig(A - B * X0);
std::cout << "INIT X0: \n" << eig.eigenvalues() << "\n";
double tol = 1e-5;
int kmax = 20;
Eigen::MatrixXd X = X0;
double err = std::numeric_limits<double>::max();
int k = 0;
Eigen::MatrixXd RX, H, V;
/** temporary */
double tk = 1;
while( (err > tol) && (k < kmax) )
{
RX = C + X * A + A.transpose() * X - (X * B) * X;
/** newton update */
H = lyapunov((A - B * X).transpose(), -RX);
/** exact line search */
V = H * B * H;
double a = (RX * RX).trace();
double b = (RX * V).trace();
double c = (V * V).trace();
tk = line_search_care(a,b,c);
/** inner loop to accept step */
X = X + tk * H;
//err = tk * (H.lpNorm<1>() / X.lpNorm<1>());
err = RX.norm();
std::cout << "err " << err << " step " << tk << "\n";
k++;
}
/** may be defect correction algorithm? */
std::cout << "CARE solve took " << k << " iterations. \n";
if(k == kmax)
std::cerr << "CARE cannot be solved to specified precision :" << err << " max number of iteration exceeded! \n ";
return X;
}
Eigen::MatrixXd init_newton_care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
{
int n = A.rows();
double tolerance = 1e-12;
/** compute Schur decomposition of A */
Eigen::RealSchur<Eigen::MatrixXd> schur(A);
Eigen::MatrixXd TA = schur.matrixT();
Eigen::MatrixXd U = schur.matrixU();
Eigen::MatrixXd TD = U.transpose() * B;
Eigen::EigenSolver<Eigen::MatrixXd> es;
es.compute(TA, false);
Eigen::VectorXd eig_r = es.eigenvalues().real();
double b = -eig_r.minCoeff();
b = std::fmax(b, 0.0) + 0.5;
Eigen::MatrixXd E = Eigen::MatrixXd::Identity(n, n);
Eigen::MatrixXd Z = lyapunov(TA + b * E, 2 * TD * TD.transpose());
Eigen::MatrixXd X = (TD.transpose() * pinv(Z)) * U.transpose();
if( (X - X.transpose()).norm() > tolerance)
{
Eigen::MatrixXd M = (X.transpose() * B) * X + 0.5 * Eigen::MatrixXd::Identity(n ,n);
X = lyapunov((A - B*X).transpose(), -M);
}
return X;
}
/** Moore-Penrose pseudo-inverse */
Eigen::MatrixXd pinv(const Eigen::MatrixXd &mat)
{
/** compute SVD */
Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
double pinvtol = 1e-6;
Eigen::VectorXd singular_values = svd.singularValues();
/** make a copy */
Eigen::VectorXd singular_values_inv = singular_values;
for ( int i = 0; i < mat.cols(); ++i)
{
if ( singular_values(i) > pinvtol )
singular_values_inv(i) = 1.0 / singular_values(i);
else singular_values_inv(i) = 0;
}
return (svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose());
}
Eigen::MatrixXd care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C)
{
Eigen::MatrixXd X0 = init_newton_care(A, B);
return newton_ls_care(A, B, C, X0);
}
double line_search_care(const double &a, const double &b, const double &c)
{
Eigen::Matrix<double, 5, 1> poly;
Eigen::Matrix<double, 4, 1> poly_derivative;
poly_derivative << -2*a, 2*(a-2*b), 6*b, 4*c;
poly << a, -2*a, a-2*b, 2*b, c;
poly_derivative = (1.0/(4*c)) * poly_derivative;
poly = (1.0/c) * poly;
/** find extremums */
Eigen::PolynomialSolver<double, 3> root_finder;
root_finder.compute(poly_derivative);
/** compute values on the bounds */
double lb_value = Eigen::poly_eval(poly, 1e-5);
double ub_value = Eigen::poly_eval(poly, 2);
double argmin = lb_value < ub_value ? 1e-5 : 2;
/** check critical points : redo with visitor! */
double minimum = Eigen::poly_eval(poly, argmin);
for (int i = 0; i < root_finder.roots().size(); ++i)
{
double root = root_finder.roots()(i).real();
if((root >= 1e-5) && (root <= 2))
{
double candidate = Eigen::poly_eval(poly, root);
if(candidate < minimum)
{
argmin = root;
minimum = Eigen::poly_eval(poly, argmin);
}
}
}
return argmin;
}
Eigen::MatrixXd lqr(const LinearSystem &sys, const Eigen::MatrixXd Q,
const Eigen::MatrixXd R, const Eigen::MatrixXd M, const bool &check)
{
/** check preliminary conditions */
//assume F,G to be stabilizable
if(check)
{
Eigen::MatrixXd QR = Q - M * pinv(R) * M.transpose();
Eigen::EigenSolver<Eigen::MatrixXd> solver(QR);
Eigen::VectorXd values = solver.eigenvalues().real();
if( (values.array() < 0).any() )
{
std::cerr << "Weight matrices did not pass positivity check! \n";
return Eigen::MatrixXd();
}
}
/** formulate Ricatti equations */
Eigen::MatrixXd invR = pinv(R);
Eigen::MatrixXd A = sys.F - M * invR * (sys.G).transpose();
Eigen::MatrixXd B = sys.G * invR * (sys.G).transpose();
Eigen::MatrixXd C = M * invR * M + Q;
std::cout << "A: \n" << A << "\n";
std::cout << "B: \n" << B << "\n";
std::cout << "C: \n" << C << "\n";
/** solve Ricatti equation */
Eigen::MatrixXd S = care(A, B, C);
std::cout << "CARE solution: \n" << (S*A) + (A.transpose() * S) - (S * B) * S.transpose() + C << "\n";
std::cout << "S: \n" << S << "\n";
/** compute gain matrix */
Eigen::MatrixXd K = invR * ( (sys.G).transpose() * S + M.transpose());
return K;
}
/**oc bracket */
}
/**kmath bracket */
}
diff --git a/src/kite_math/kitemath.h b/src/kite_math/kitemath.h
index 455492c..c1c47dd 100644
--- a/src/kite_math/kitemath.h
+++ b/src/kite_math/kitemath.h
@@ -1,95 +1,146 @@
#ifndef KITEMATH_H
#define KITEMATH_H
#include "casadi/casadi.hpp"
#include "eigen3/Eigen/Dense"
#include "eigen3/Eigen/Eigenvalues"
#include "eigen3/unsupported/Eigen/Polynomials"
#include "eigen3/unsupported/Eigen/MatrixFunctions"
enum IntType {RK4, CVODES, CHEBYCHEV};
namespace kmath
{
/** quaternion arithmetic */
casadi::SX quat_multiply(const casadi::SX &q1, const casadi::SX &q2);
casadi::SX quat_inverse(const casadi::SX &q);
/** collection of custom functions */
casadi::SX heaviside(const casadi::SX &x, const double K);
double deg2rad(const double °){return (M_PI / 180) * deg;}
/** integration */
casadi::SX rk4_symbolic(const casadi::SX &X,
const casadi::SX &U,
casadi::Function &func,
const casadi::SX &h);
/** @brief: compute Chebyshev collocation points for a given interval */
void cheb(casadi::DM &CollocPoints, casadi::DM &DiffMatrix,
const unsigned &N, const std::pair<double, double> interval);
casadi::SX mat_func(const casadi::SX &matrix_in, casadi::Function &func);
casadi::SX mat_dynamics(const casadi::SX &arg_x, const casadi::SX &arg_u, casadi::Function &func);
+ /** range of numbers between first and the last */
template<typename T>
std::vector<T> range(const T &first, const T &last, const T &step = 1)
{
std::vector<T> _range;
for (T value = first; value <= last; value += step)
_range.push_back(value);
return _range;
}
+ /** factorial computation 'n!' */
+ uint factorial(const uint &n);
+
+ /** Chebyshev exapnsion of a function u(x) = Sum_0^K uk * Tk(x) */
+ template<typename Scalar>
+ Scalar chebyshev_expansion(const std::vector<Scalar> &FuncValues, const Scalar &Value)
+ {
+ if(FuncValues.empty())
+ return std::numeric_limits<Scalar>::infinity();
+
+ /** initialize polynomial basis*/
+ std::vector<Scalar> T = {1, Value};
+ Scalar result = 0;
+
+ if(FuncValues.size() > 2)
+ {
+ for (int k = 2; k < FuncValues.size(); ++k)
+ {
+ Scalar Tk = 2 * Value * T[k-1] - T[k-2];
+ T.push_back(Tk);
+ }
+ }
+
+ for (int i = 0; i < FuncValues.size(); ++i)
+ {
+ result += T[i] * FuncValues[i];
+ }
+
+ return result;
+ }
+
+ /** Chebyshev exapnsion of a function u(x) = Sum_0^K uk * Tk(x) using series expansion on [-1,1]*/
+ template<typename Scalar>
+ Scalar chebyshev_expansion2(const std::vector<Scalar> &FuncValues, const Scalar &Value)
+ {
+ if(FuncValues.empty())
+ return std::numeric_limits<Scalar>::infinity();
+
+ Scalar result = 0;
+ for (int k = 0; k < FuncValues.size(); ++k)
+ {
+ Scalar Tk = cos(k * acos(Value));
+
+ /** compute the Chebyshev polynomial of order k */
+ result += Tk * FuncValues[k];
+ }
+ return result;
+ }
+
+ /** Linear Systems Control and Analysis routines */
struct LinearSystem
{
Eigen::MatrixXd F; // dynamics
Eigen::MatrixXd G; // control mapping matrix
Eigen::MatrixXd H; // output mapping
LinearSystem(){}
LinearSystem(const Eigen::MatrixXd &_F, const Eigen::MatrixXd &_G, const Eigen::MatrixXd &_H) :
F(_F), H(_H), G(_G) {}
virtual ~LinearSystem(){}
bool is_controllable();
bool is_observable();
/** involves stable/unstable modes decomposition */
bool is_stabilizable();
};
/** fast optimal control methods */
namespace oc
{
/** Solve Lyapunov equation: AX + XA' = Q */
Eigen::MatrixXd lyapunov(const Eigen::MatrixXd &A, const Eigen::MatrixXd &Q);
/** solve Continuous Riccati Equation using Newton iteration with line search */
/** C + XA + A'X - XBX = 0 */
Eigen::MatrixXd newton_ls_care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
const Eigen::MatrixXd &C, const Eigen::MatrixXd &X0);
/** Compute a stabilizing intial approximation for X */
Eigen::MatrixXd init_newton_care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B);
/** Moore-Penrose pseudo-inverse */
Eigen::MatrixXd pinv(const Eigen::MatrixXd &mat);
/** solve CARE : C + XA + A'X - XBX*/
Eigen::MatrixXd care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C);
/** Line search for CARE Newton iteration */
double line_search_care(const double &a, const double &b, const double &c);
/** Linear Quadratic Regulator:
* J(x) = INT { xQx + xMu + uRu }dt
* xdot = Fx + Gu
*/
Eigen::MatrixXd lqr(const LinearSystem &sys, const Eigen::MatrixXd Q,
const Eigen::MatrixXd R, const Eigen::MatrixXd M, const bool &check = false);
}
}
#endif // KITEMATH_H
diff --git a/src/kite_math/pseudospectral/chebyshev.hpp b/src/kite_math/pseudospectral/chebyshev.hpp
index bd66660..fff526a 100644
--- a/src/kite_math/pseudospectral/chebyshev.hpp
+++ b/src/kite_math/pseudospectral/chebyshev.hpp
@@ -1,200 +1,264 @@
#ifndef CHEBYSHEV_HPP
#define CHEBYSHEV_HPP
#include "kitemath.h"
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int dimX>
+ int NX,
+ int NU,
+ int NP>
class Chebyshev
{
public:
/** constructor */
Chebyshev();
virtual ~Chebyshev(){}
BaseClass D(){return _D;}
BaseClass CompD(){return _ComD;}
BaseClass CPoints(){return _Points;}
BaseClass QWeights(){return _QuadWeights;}
+ BaseClass VarX(){return _X;}
+ BaseClass VarU(){return _U;}
+
+ BaseClass CollocateDynamics(casadi::Function &dynamics, const double &t0, const double &tf);
+ BaseClass CollocateCost(const casadi::Function &MayerTerm, const casadi::Function &LangrangeTerm);
+
private:
/** generate Differentiation matrix */
BaseClass DiffMatrix();
/** generate Chebyshev collocation points */
BaseClass CollocPoints();
/** generate Clenshaw-Curtis quadrature weights */
BaseClass QuadWeights();
/** generate Composite Differentiation matrix */
BaseClass CompDiffMatrix();
/** Diff matrix */
BaseClass _D;
/** Composite diff matrix */
BaseClass _ComD;
/** Collocation points */
BaseClass _Points;
/** Quadrature weights */
BaseClass _QuadWeights;
/** helper functions */
BaseClass range(const uint &first, const uint &last, const uint &step);
+
+ /** state in terms of Chebyshev coefficients */
+ BaseClass _X;
+ /** control in terms of Chebyshev coefficients */
+ BaseClass _U;
+ /** vector of parameters */
+ BaseClass _P;
};
/** @brief constructor */
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::Chebyshev()
+ int NX,
+ int NU,
+ int NP>
+Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::Chebyshev()
{
/** initialize pseudopsectral scheme */
_Points = CollocPoints();
_D = DiffMatrix();
_QuadWeights = QuadWeights();
_ComD = CompDiffMatrix();
+
+ /** create discretized states and controls */
+ _X = casadi::SX::sym("X", NumSegments * (PolyOrder + 1) * NX - (NumSegments - 1));
+ _U = casadi::SX::sym("U", NumSegments * (PolyOrder + 1) * NU - (NumSegments - 1) - NU);
+ _P = casadi::SX::sym("P", NP);
}
/** @brief range */
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::range(const uint &first, const uint &last, const uint &step)
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::range(const uint &first, const uint &last, const uint &step)
{
int numel = std::floor((last - first) / step);
BaseClass _range;
_range.reserve(numel);
int idx = 0;
for (uint value = first; value <= last; ++value)
{
_range(idx) = 0;
}
return _range;
}
/** @brief compute collocation points */
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::CollocPoints()
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::CollocPoints()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto grid_int = kmath::range<double>(0, PolyOrder);
/** cast grid to Casadi type */
BaseClass grid(grid_int);
BaseClass X = cos(grid * (M_PI / PolyOrder));
return X;
}
/** @brief compute differentiation matrix / ref {L. Trefethen "Spectral Methods in Matlab"}*/
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::DiffMatrix()
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::DiffMatrix()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto grid_int = kmath::range<double>(0, PolyOrder);
/** cast grid to Casadi type */
BaseClass grid(grid_int);
BaseClass cpoints = cos(grid * (M_PI / PolyOrder));
/** Diff Matrix */
BaseClass c = BaseClass::vertcat({2, BaseClass::ones(PolyOrder - 1, 1), 2});
c = BaseClass::mtimes(BaseClass::diag( pow(-1, grid)), c);
BaseClass XM = BaseClass::repmat(cpoints, 1, PolyOrder + 1);
BaseClass dX = XM - XM.T();
BaseClass Dn = BaseClass::mtimes(c, (1 / c).T() ) / (dX + (BaseClass::eye(PolyOrder + 1))); /** off-diagonal entries */
return Dn - BaseClass::diag( BaseClass::sumRows(Dn.T() )); /** diagonal entries */
}
/** @brief compute weights for Clenshaw-Curtis quadrature / ref {L. Trefethen "Spectral Methods in Matlab"}*/
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::QuadWeights()
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::QuadWeights()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto grid_int = kmath::range<double>(0, PolyOrder);
/** cast grid to Casadi type */
BaseClass theta(grid_int);
theta = theta * (M_PI / PolyOrder);
BaseClass w = BaseClass::zeros(1, PolyOrder + 1);
BaseClass v = BaseClass::ones(PolyOrder - 1, 1);
if ( PolyOrder % 2 == 0 )
{
w[0] = 1 / (pow(PolyOrder, 2) - 1);
w[PolyOrder] = w[0];
for(int k = 1; k <= PolyOrder / 2 - 1; ++k)
{
v = v - 2 * cos(2 * k * theta(casadi::Slice(1, PolyOrder))) / (4 * pow(k, 2) - 1);
}
v = v - cos( PolyOrder * theta(casadi::Slice(1, PolyOrder))) / (pow(PolyOrder, 2) - 1);
}
else
{
w[0] = 1 / std::pow(PolyOrder, 2);
w[PolyOrder] = w[0];
for (int k = 1; k <= (PolyOrder - 1) / 2; ++k)
{
v = v - 2 * cos(2 * k * theta(casadi::Slice(1, PolyOrder))) / (4 * pow(k, 2) - 1);
}
}
w(casadi::Slice(1, PolyOrder)) = 2 * v / PolyOrder;
return w;
}
/** @brief compute composite differentiation matrix */
template<class BaseClass,
int PolyOrder,
int NumSegments,
- int DimX>
-BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, DimX>::CompDiffMatrix()
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::CompDiffMatrix()
{
- int comp_rows = NumSegments * (PolyOrder + 1) * DimX;
- int comp_cols = NumSegments * (PolyOrder + 1) * DimX - (NumSegments - 1);
+ int comp_rows = NumSegments * (PolyOrder + 1) - (NumSegments - 1);;
+ int comp_cols = NumSegments * (PolyOrder + 1) - (NumSegments - 1);
BaseClass CompDiff = BaseClass::zeros(comp_rows, comp_cols);
BaseClass D = DiffMatrix();
BaseClass D0 = D;
D0(D0.size1() - 1, casadi::Slice(0, D0.size2())) = BaseClass::zeros(PolyOrder + 1);
D0(D0.size1() - 1, D0.size2() - 1) = 1;
- BaseClass E = BaseClass::eye(DimX);
-
- BaseClass Dn = BaseClass::kron(D, E);
- BaseClass D0n = BaseClass::kron(D0, E);
+ BaseClass E = BaseClass::eye(NX);
- int dn_size = Dn.size1();
+ int dn_size = D.size1();
if(NumSegments < 2)
{
- CompDiff = D0n;
+ CompDiff = D0;
}
else
{
/** insert first matrix */
- CompDiff(casadi::Slice(CompDiff.size1() - D0n.size1(), CompDiff.size1()),
- casadi::Slice(CompDiff.size2() - D0n.size2(), CompDiff.size2())) = D0n;
+ CompDiff(casadi::Slice(CompDiff.size1() - D0.size1(), CompDiff.size1()),
+ casadi::Slice(CompDiff.size2() - D0.size2(), CompDiff.size2())) = D0;
/** fill in diagonal terms */
for(int k = 0; k < NumSegments - 1; ++k)
{
int shift = k > 0 ? -1 : 0;
int i = k * dn_size;
int j = k * dn_size + shift;
- CompDiff(casadi::Slice(i, i + dn_size), casadi::Slice(j, j + dn_size)) = Dn;
+ CompDiff(casadi::Slice(i, i + dn_size - 1), casadi::Slice(j, j + dn_size)) =
+ D(casadi::Slice(0, PolyOrder), casadi::Slice(0, D.size2()));
}
}
- return CompDiff;
+ return BaseClass::kron(CompDiff, E);
+}
+
+/** @brief collocate differential constraints */
+template<class BaseClass,
+ int PolyOrder,
+ int NumSegments,
+ int NX,
+ int NU,
+ int NP>
+BaseClass Chebyshev<BaseClass, PolyOrder, NumSegments, NX, NU, NP>::CollocateDynamics(casadi::Function &dynamics,
+ const double &t0, const double &tf)
+{
+ /** evaluate RHS at the collocation points */
+ int DIMX = _X.size1();
+ BaseClass F_XU = BaseClass::zeros(DIMX);
+ casadi::SXVector tmp;
+ int j = 0;
+
+ double t_scale = (tf - t0) / (2 * NumSegments);
+
+ for (int i = 0; i < DIMX - NX; i += NX)
+ {
+ //std::cout << "X in : " << _X(casadi::Slice(i, i + NX)) << "\n";
+ //std::cout << "U in : " << _U(casadi::Slice(j, j + NU)) << "\n";
+
+ tmp = dynamics(casadi::SXVector{_X(casadi::Slice(i, i + NX)),
+ _U(casadi::Slice(j, j + NU)) });
+
+ F_XU(casadi::Slice(i, i + NX)) = t_scale * tmp[0];
+ j += NU;
+ }
+
+ BaseClass G_XU = BaseClass::mtimes(_ComD, _X) - F_XU;
+ return G_XU;
}
#endif // CHEBYSHEV_HPP