Homec4science
Diffusion test_package (simple_kite) Browse

R1517/src/kite_math/kitemath.h
ea22a95d990asimple_kite

Last Change
Raw File

kitemath.h
View Options

#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 &deg){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);
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
c4science · Help