math.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues> 
 
#include <cstdint>
 
namespace controlpp{
 
    template<class T>
    constexpr T product_over(T from, T to){
        unsigned long product = 1;
        for(; from < to; ++from){
            product *= from;
        }
        return product;
    }
 
    template<class T>
    constexpr T pow(const T& base, const int& exp){
        T result = static_cast<T>(1);
        if(exp >= 0){
            for(int i = 0; i < exp; ++i){
                result *= base;
            }
        }else{
            for(int i = 0; i < -exp; ++i){
                result /= base;
            }
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap(const Eigen::Vector<T, N>& y, const T& modulo){
        Eigen::Vector<T, N> result;
        if constexpr (N == Eigen::Dynamic) result.resize(y.size());
        T offset = 0;
        result(0) = y(0);
        for(int i = 1; i < y.size(); ++i){
            while(((y(i) + offset) - result(i-1)) > (modulo/2)){
                offset -= modulo;
            }
            while(((y(i) + offset) - result(i-1)) < (-modulo/2)){
                offset += modulo;
            }
            result(i) = y(i) + offset;
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap_rad(const Eigen::Vector<T, N>& phases){
        const T modulo = std::numbers::pi_v<T> * static_cast<T>(2);
        return unwrap(phases, modulo);
    }
 
    template<class T, int N>
    Eigen::Vector<T, N> unwrap_deg(const Eigen::Vector<T, N>& phases){
        const T modulo = 360;
        return unwrap(phases, modulo);
    }
 
    
    template<class T = double>
    constexpr T pade_param_reccursice(T P_k, std::int32_t m, std::int32_t n, std::int32_t k){
        const std::int64_t num = (m - k);
        const std::int64_t den = (k + 1) * (m + n - k);
        const double f = static_cast<T>(num) / static_cast<T>(den);
        return P_k * f;
    }
 
    template<class T = double>
    constexpr void pade_params(T* params, std::size_t size, std::uint32_t m, std::uint32_t n){
        params[0] = T(1.0);
        for(std::size_t k = 1; k < size; ++k){
            params[k] = pade_param_reccursice(params[k-1], m, n, k-1);
        }
    }
 
    template<class T = double>
    constexpr T pade_num_param(std::int32_t m, std::int32_t n, std::int32_t k){
        double P_k = 1.0;
        for(std::int32_t i = 0; i < k; ++i){
            P_k = pade_param_reccursice(P_k, m, n, i);
        }
        return P_k;
    }
 
    template<class T = double>
    constexpr T pade_den_param(unsigned long m, unsigned long n, unsigned long k){
        return pade_num_param(n, m, k);
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> identity_like([[maybe_unused]]const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& unused){
        if constexpr (Rows != Eigen::Dynamic && Cols != Eigen::Dynamic){
            return Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>::Identity();
        }else{
            Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> result;
            result.setIdentity();
            return result;
        }
    }
 
    template<class T, int LSize, int RSize>
    Eigen::Matrix<T, LSize + RSize, LSize + RSize> join_to_diagonal(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Matrix<T, LSize + RSize, LSize + RSize> result;
        result.diagonal().head(LSize) = l;
        result.diagonal().tail(RSize) = r;
        return result;
    }
 
    template<class T, int LSize, int RSize>
    requires(LSize != Eigen::Dynamic && RSize != Eigen::Dynamic)
    Eigen::Vector<T, LSize + RSize> join_to_vector(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Vector<T, LSize + RSize> result;
        result.head(LSize) = l;
        result.tail(RSize) = r;
        return result;
    }
 
    template<class T, int LSize, int RSize>
    requires(!(LSize != Eigen::Dynamic && RSize != Eigen::Dynamic))
    Eigen::Vector<T, Eigen::Dynamic> join_to_vector(const Eigen::Vector<T, LSize>& l, const Eigen::Vector<T, RSize>& r){
        Eigen::Vector<T, Eigen::Dynamic> result(l.size() + r.size());
        result.head(l.size()) = l;
        result.tail(r.size()) = r;
        return result;
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows-1, Cols-1> minor(const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& A, size_t ex_row, size_t ex_col){
        Eigen::Matrix<T, Rows-1, Cols-1> result;
        size_t A_row = 0;
        size_t A_col = 0;
        for(size_t result_row = 0; result_row < (Rows-1); ++result_row, (void)++A_row){
            for(size_t result_col = 0; result_col < (Cols-1); ++result_col, (void)++A_col){
                A_row += (A_row == ex_row) ? 1 : 0;
                A_col += (A_col == ex_col) ? 1 : 0;
                result(result_row, result_col) = A(A_row, A_col);
            }
        }
        return result;
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols> adj(const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& A){
        Eigen::Matrix<T, Rows, Cols> result;
        for(int row = 0; row < Rows; ++row){
            const bool row_sign = ((row & 1) == 1); // even are positive, odd are negative
            for(int col = 0; col < Cols; ++col){
                const bool col_sign = ((col & 1) == 1); // even are positive, odd are negative
                const bool value_sign = row_sign != col_sign; // sign of the value (both positive/negative: value is positive) (different signs: value is negative)
                const auto min = minor(A, row, col);
                const T det = min.determinant();
                if(value_sign){
                    // negative sign
                    result(col, row) = -det;
                }else{
                    // positive sign
                    result(col, row) = det;
                }
            }
        }
        return result;
    }
 
    template<class T, int N>
    Eigen::Matrix<T, N-1, N-1> companion(const Eigen::Vector<T, N>& v){
        Eigen::Matrix<T, N-1, N-1> result;
        result.template block<N-2, N-2>(0, 1) = Eigen::Matrix<T, N-2, N-2>::Identity();
        result.col(0).setZero();
        result.row(N-2) = -v.head(N-1) / v(N-1);
        return result;
    }
 
    template<class T, int N>
    requires(N % 2 == 0)
    Eigen::Matrix<T, N/2, N/2> hamilton_solver(
        const Eigen::Matrix<T, N, N> H
    ){
        // 2. Compute stable eigenvectors
        Eigen::ComplexEigenSolver<Eigen::Matrix<T, N, N>> ces;
        ces.compute(H);
        const auto& H_eigvals = ces.eigenvalues();
        const auto& H_eigvecs = ces.eigenvectors();
 
        // in a 2n hamilton matrix are exactly n stable ones
        Eigen::Matrix<std::complex<T>, N, N/2> StableEigenVecs;
        int si = 0; // stable eigenvalue iterator
        int ei = 0; // eigen value iterator
        for(; (si < N/2) && (ei < N); ++ei){
            if(H_eigvals(ei).real() <= 0){// only stable ones
                StableEigenVecs.col(si) = H_eigvecs.col(ei);
                ++si;
            }
        }
 
        // 3. Repartition
        const auto TopPartition = StableEigenVecs.template block<N/2, N/2>(0, 0);
        const auto BottomPartition = StableEigenVecs.template block<N/2, N/2>(N/2, 0);
 
        // 4. Recover Solution (BottomPartition * TopPartition^-1)
        const Eigen::Matrix<std::complex<T>, N/2, N/2> X = TopPartition.transpose().partialPivLu().solve(BottomPartition.transpose()).transpose();
 
        // The result is real, make sure it is real because it should be
        const Eigen::Matrix<T, N/2, N/2> realX = X.real();
 
        // force the result X to be symetric to combat small numerical errors
        const Eigen::Matrix<T, N/2, N/2> result = static_cast<T>(0.5) * (realX + realX.transpose());
        return result;
    }
 
    template<class T, int N>
    Eigen::Matrix<T, N/2, N/2> symplectic_solver(
        const Eigen::Matrix<T, N, N> S
    ){
        // 2. Compute stable eigenvectors
        Eigen::ComplexEigenSolver<Eigen::Matrix<T, N, N>> ces;
        ces.compute(S);
        const auto& eigvals = ces.eigenvalues();
        const auto& eigvecs = ces.eigenvectors();
 
        // in a 2n hamilton matrix are exactly n stable ones
        Eigen::Matrix<std::complex<T>, N, N/2> StableEigenVecs;
        StableEigenVecs.setZero();
        int si = 0; // stable eigenvalue iterator
        int ei = 0; // eigen value iterator
        for(; (si < N/2) && (ei < N); ++ei){
            if(std::abs(eigvals(ei)) <= 1){// only stable ones
                StableEigenVecs.col(si) = eigvecs.col(ei);
                ++si;
            }
        }
 
        // 3. Repartition
        const auto TopPartition = StableEigenVecs.template block<N/2, N/2>(0, 0);
        const auto BottomPartition = StableEigenVecs.template block<N/2, N/2>(N/2, 0);
 
        // 4. Recover Solution (BottomPartition * TopPartition^-1)
        const Eigen::Matrix<std::complex<T>, N/2, N/2> X = TopPartition.transpose().partialPivLu().solve(BottomPartition.transpose()).transpose();
 
        // The result is real, make sure it is real because it should be
        const Eigen::Matrix<T, N/2, N/2> realX = X.real();
 
        // force the result X to be symetric to combat small numerical errors
        const Eigen::Matrix<T, N/2, N/2> result = static_cast<T>(0.5) * (realX + realX.transpose());
 
        return result;
    }
 
    template<class T, int NStates,
            int AOpt, int AMaxR, int AMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> lyapunov_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q
    ){
        // 1. Build the hamilton matrix
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A;
        H.topRightCorner(NStates, NStates).setZero();
        H.bottomLeftCorner(NStates, NStates) = - Q;
        H.bottomRightCorner(NStates, NStates) = -A.transpose();
        return hamilton_solver(H);
    }
 
    template<class ValueType, int NStates, int NInputs, int NOutputs>
    std::tuple<Eigen::Matrix<ValueType, NStates, NStates>, Eigen::Matrix<ValueType, NStates, NStates>> energy_scaling_matrix(
        const Eigen::Matrix<ValueType, NStates, NStates> A,
        const Eigen::Matrix<ValueType, NStates, NInputs> B,
        const Eigen::Matrix<ValueType, NOutputs, NStates> C
    ){
        // Calculate the two gramians Wc and Wo
        const Eigen::Matrix<ValueType, NStates, NStates> Bsqr = B * B.transpose();
        const Eigen::Matrix<ValueType, NStates, NStates> Wc = lyapunov_solver(A.transpose(), Bsqr);
 
        const Eigen::Matrix<ValueType, NStates, NStates> Csqr = C.transpose() * C;
        const Eigen::Matrix<ValueType, NStates, NStates> Wo = lyapunov_solver(A, Csqr);
 
        // force symetry and add some to the diagonal for numerical stability
        const ValueType eps = std::numeric_limits<ValueType>::epsilon();
 
        Eigen::Matrix<ValueType, NStates, NStates> Wc_sym = (Wc + Wc.transpose()) * static_cast<ValueType>(0.5);
        {
            ValueType jitter = std::max(ValueType(1), Wc_sym.diagonal().cwiseAbs().maxCoeff());
            Wc_sym.diagonal().array() += jitter * eps;
        }
        Eigen::Matrix<ValueType, NStates, NStates> Wo_sym = (Wo + Wo.transpose()) * static_cast<ValueType>(0.5);
        {
            ValueType jitter = std::max(ValueType(1), Wo_sym.diagonal().cwiseAbs().maxCoeff());
            Wo_sym.diagonal().array() += jitter * eps;
        }
 
        // calculate the cholesky factors using LLT
        const Eigen::LLT<Eigen::Matrix<ValueType, NStates, NStates>> Wc_colesky(Wc_sym);
        const Eigen::LLT<Eigen::Matrix<ValueType, NStates, NStates>> Wo_colesky(Wo_sym);
 
        // partition into triangle forms
        const Eigen::Matrix<ValueType, NStates, NStates> Rc = Wc_colesky.matrixU();
        const Eigen::Matrix<ValueType, NStates, NStates> Ro = Wo_colesky.matrixL();
 
        // calculate the SVD
        const Eigen::Matrix<ValueType, NStates, NStates> M = Ro * Rc;
        const Eigen::JacobiSVD<Eigen::Matrix<ValueType, NStates, NStates>> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
        const Eigen::Matrix<ValueType, NStates, NStates> U  = svd.matrixU();
        const Eigen::Matrix<ValueType, NStates, 1> s  = svd.singularValues();
        const Eigen::Matrix<ValueType, NStates, NStates> V = svd.matrixV();
 
        
        const Eigen::Matrix<ValueType, NStates, 1> s_sqrt = (s.array().max(eps)).sqrt().matrix();
        const Eigen::Matrix<ValueType, NStates, NStates> S_sqrt = s_sqrt.asDiagonal();      
 
        // Finally!!! we can calculate the resulting transformation matrices
        const Eigen::Matrix<ValueType, NStates, NStates> T = Rc.transpose().template triangularView<Eigen::Lower>().solve((S_sqrt * V.transpose()).transpose()).transpose();
        const Eigen::Matrix<ValueType, NStates, NStates> T_inverse = Ro.template triangularView<Eigen::Lower>().solve(U * S_sqrt);
 
        return {T, T_inverse};
    }
 
    
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> care_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A;
        H.topRightCorner(NStates, NStates) =  -B * R.ldlt().solve(B.transpose());
        H.bottomLeftCorner(NStates, NStates) = -Q;
        H.bottomRightCorner(NStates, NStates) = -A;
        return hamilton_solver(H);
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC
    >
    Eigen::Matrix<T, NStates, NStates> dare_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R
    ){
        const Eigen::Matrix<T, NStates, NStates> brb = B * R.llt().solve(B.transpose());
        Eigen::ColPivHouseholderQR<Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>> At_qr(A.transpose());
        const Eigen::Matrix<T, NStates, NStates> aq = At_qr.solve(Q);
        Eigen::Matrix<T, 2*NStates, 2*NStates> S;
        S.topLeftCorner(NStates, NStates) = A + brb * aq;
        S.topRightCorner(NStates, NStates) = -A.colPivHouseholderQr().solve(brb.transpose()).transpose();
        S.bottomLeftCorner(NStates, NStates) = - aq;
        S.bottomRightCorner(NStates, NStates) = At_qr.solve(identity_like(A));
        return symplectic_solver(S);
    }
 
    template<class T, int NStates, int NInputs,
            int AOpt, int AMaxR, int AMaxC,
            int BOpt, int BMaxR, int BMaxC,
            int ROpt, int RMaxR, int RMaxC,
            int QOpt, int QMaxR, int QMaxC,
            int NOpt, int NMaxR, int NMaxC
    >
    Eigen::Matrix<T, NStates, NStates> care_solver(
        const Eigen::Matrix<T, NStates, NStates, AOpt, AMaxR, AMaxC>& A,
        const Eigen::Matrix<T, NStates, NInputs, BOpt, BMaxR, BMaxC>& B,
        const Eigen::Matrix<T, NStates, NStates, QOpt, QMaxR, QMaxC>& Q,
        const Eigen::Matrix<T, NInputs, NInputs, ROpt, RMaxR, RMaxC>& R,
        const Eigen::Matrix<T, NStates, NInputs, NOpt, NMaxR, NMaxC>& N
    ){
        // 1. Build the hamilton matrix
        const Eigen::Matrix<T, NInputs, NStates> R_inv_DT_C = R.ldlt().solve((N.transpose()));
        const Eigen::Matrix<T, NStates, NStates> A_ = A - B * R_inv_DT_C;
 
        Eigen::Matrix<T, 2*NStates, 2*NStates> H;
        H.topLeftCorner(NStates, NStates) = A_;
        H.topRightCorner(NStates, NStates) =  - B * R.ldlt().solve(B.transpose());
        H.bottomLeftCorner(NStates, NStates) = - (Q - N * R_inv_DT_C);
        H.bottomRightCorner(NStates, NStates) = -A_.transpose();
        return hamilton_solver(H);
    }
    
}