expm.hpp

Go to the documentation of this file.

#pragma once
 
#include "math.hpp"
 
namespace controlpp{
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> expm_taylor(
            const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& x, 
            int n
    ){
        Eigen::Matrix<T, Rows, Cols> result;
        const auto I = Eigen::Matrix<T, Rows, Cols>::Identity();
        result.setZero();
        for(int i = n; i > 0; --i){
            Eigen::Matrix<T, Rows, Cols> new_result = (result + I) * x / static_cast<T>(i);
            result = new_result;
        }
        result += I;
        return result;
    }
 
    template<class T, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols> expm_taylor_scaled(
            const Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>& M, 
            int taylor_order = 8, 
            int scaling = 10
    ){
        using Matrix = Eigen::Matrix<T, Rows, Cols, Options, MaxRows, MaxCols>;
        T s = static_cast<T>(1 << scaling);
        Matrix scaled_M = M/s;
        Matrix t = expm_taylor(scaled_M, taylor_order);
        for(int i = 0; i < scaling; ++i){
            const Matrix temp = t * t;
            t = temp;
        };
        return t;
    }
 
 
    template<class T, int N, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, N, N> expm_pade(
            const Eigen::Matrix<T, N, N, Options, MaxRows, MaxCols>& A,
            int Order = 5
    ){
        if(Order >= 32){
            throw std::invalid_argument("expm_pade: Order must be less than 32");
        }
        const Eigen::Matrix<T, N, N> I = Eigen::Matrix<T, N, N>::Identity();
        const Eigen::Matrix<T, N, N> A2 = A * A;
 
        // even odd split of polynomials: allows to reuse results
        Eigen::Matrix<T, N, N> even;
        Eigen::Matrix<T, N, N> odd;
        even.setZero();
        odd.setZero();
 
        // calculate pade parameters
        T buffer[32];
        pade_params(buffer, Order+1, Order, Order);
 
        // horner chains to evaluate the polynomials
        for(int k = Order; k >= 0; --k){
            const bool is_first_iteration = (k == Order);
            const T pk = static_cast<T>(buffer[k]);
            if((k & 1) == 0){
                // even
                if(!is_first_iteration) even *= A2; // skipp on the first iteration
                even += I * pk;
            }else{
                // odd
                if(!is_first_iteration) odd *= A2; // skipp on the first iteration
                odd += I * pk;
            }
        }
        odd *= A;
 
        const Eigen::Matrix<T, N, N> num = even + odd;
        const Eigen::Matrix<T, N, N> den = even - odd;
 
        const Eigen::Matrix<T, N, N> result = den.partialPivLu().solve(num);
        
        return result;
    }
 
    template<class T, int N, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, N, N> expm_pade_scaled(
            const Eigen::Matrix<T, N, N, Options, MaxRows, MaxCols>& M, 
            int order,
            int scaling
    ){
        Eigen::Matrix<T, N, N> scaled_M = M * std::ldexp(T(1), -scaling);
        Eigen::Matrix<T, N, N> t = expm_pade(scaled_M, order);
        for(int i = 0; i < scaling; ++i){
            const Eigen::Matrix<T, N, N> temp = t * t;
            t = temp;
        };
        return t;
    }
 
    struct ExpmPadeParams{
        int order; 
        int scaling; 
    };
 
    template<class T, int N, int Options, int MaxRows, int MaxCols>
    ExpmPadeParams expm_pade_params(const Eigen::Matrix<T, N, N, Options, MaxRows, MaxCols>& M){
        const T norm = M.cwiseAbs().colwise().sum().maxCoeff();
 
        // Theta values from Higham
        constexpr T theta3  = T(1.495585217958292e-2);
        constexpr T theta5  = T(2.539398330063230e-1);
        constexpr T theta7  = T(9.504178996162932e-1);
        constexpr T theta9  = T(2.097847961257068);
        constexpr T theta13 = T(5.371920351148152);
 
        if(norm <= theta3){
            return {3, 0};
        }
        
        if(norm <= theta5){
            return {5, 0};
        }
        
        if(norm <= theta7){
            return {7, 0};
        }
        
        if(norm <= theta9){
            return {9, 0};
        }
        
        if(norm <= theta13){
            return {13, 0};
        }
        
        const double ratio = static_cast<double>(norm / theta13);
        int scaling = 0;
        if (ratio > 1.0) {
            scaling = static_cast<int>(std::ceil(std::log2(ratio)));
            if (scaling < 0) scaling = 0;
        }
        return {13, scaling};
    
    }
 
    template<class T, int N, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, N, N> expm_pade_scaled(
            const Eigen::Matrix<T, N, N, Options, MaxRows, MaxCols>& M
    ){
        const ExpmPadeParams params = expm_pade_params(M);
        return expm_pade_scaled(M, params.order, params.scaling);
    }
 
    template<class T, int N, int Options, int MaxRows, int MaxCols>
    Eigen::Matrix<T, N, N> expm(const Eigen::Matrix<T, N, N, Options, MaxRows, MaxCols>& M){
        // actual exponent calculation
        return expm_pade_scaled(M);
    }
}