transformations.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
 
 
// controlpp
#include <controlpp/math.hpp>
#include <controlpp/expm.hpp>
 
#include <controlpp/ContinuousStateSpace.hpp>
#include <controlpp/ContinuousTransferFunction.hpp>
 
#include <controlpp/DiscreteStateSpace.hpp>
#include <controlpp/DiscreteTransferFunction.hpp>
 
 
namespace controlpp
{
 
    enum class EDiscretisation{
        zero_order_hold,
        tustin
    };
 
    template<class ValueType, int states>
    DiscreteStateSpace<ValueType, states, 1, 1> discretise_zoh(
            const ContinuousStateSpace<ValueType, states, 1, 1>& sys, 
            ValueType sample_time
    ){
        // allocation
        DiscreteStateSpace<ValueType, states, 1, 1> result;
        Eigen::Matrix<ValueType, states+1, states+1> M;
 
        // scale the energy of the states
 
        // preparation
        M.template block<states, states>(0, 0) = sys.A();
        M.col(states).head(states) = sys.B().col(0);
        M.row(states).setZero();
        M *= sample_time;
        
        // calculation
        Eigen::Matrix<ValueType, states+1, states+1> Md = controlpp::expm(M);
 
        // re-assignment
        result.A() = Md.template block<states, states>(0, 0);
        result.B().col(0) = Md.col(states).head(states);
        result.C() = sys.C();
        result.D() = sys.D();
 
        return result;
    }
 
    template<class T, int NStates, int NInputs, int NOutputs>
    DiscreteStateSpace<T, NStates, NInputs, NOutputs> discretise_tustin(
            const ContinuousStateSpace<T, NStates, NInputs, NOutputs>& sys, 
            const T& sample_time
    ){
        const Eigen::Matrix<T, NStates, NStates> I = Eigen::Matrix<T, NStates, NStates>::Identity();
        const Eigen::Matrix<T, NStates, NStates> M = (I - (sample_time / static_cast<T>(2)) * sys.A()).eval();
        const Eigen::Matrix<T, NStates, NStates> P = (I + (sample_time / static_cast<T>(2)) * sys.A()).eval();
 
        Eigen::PartialPivLU<Eigen::Matrix<T, NStates, NStates>> Mfactor(M);
 
        const Eigen::Matrix<T, NStates, NStates> A_d = Mfactor.solve(P);
        const Eigen::Matrix<T, NStates, NInputs> B_d = Mfactor.solve(sample_time * sys.B());
        const Eigen::Matrix<T, NOutputs, NStates> C_d = M.transpose().partialPivLu().solve(sys.C().transpose()).transpose();
        const Eigen::Matrix<T, NOutputs, NInputs> X = (sys.C() * Mfactor.solve(sys.B() * (sample_time / static_cast<T>(2))));
        const Eigen::Matrix<T, NOutputs, NInputs> D_d = (sys.D() + X);
 
        const DiscreteStateSpace<T, NStates, NInputs, NOutputs> result(A_d, B_d, C_d, D_d);
        return result;
    }
 
 
    template<class T, int NStates>
    DiscreteStateSpace<T, NStates, 1, 1> discretise(
        const ContinuousStateSpace<T, NStates, 1, 1>& sys, 
        const T& sample_time,
        EDiscretisation method
    ){
        if(method == EDiscretisation::zero_order_hold){
            return discretise_zoh(sys, sample_time);
        }else{
            return discretise_tustin(sys, sample_time);
        }
    }
    
 
    
} // namespace controlpp