LTISystem.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wunused-value"

namespace ct {
namespace core {


template <size_t STATE_DIM, size_t CONTROL_DIM>
class LTISystem : public LinearSystem<STATE_DIM, CONTROL_DIM>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    LTISystem(const Eigen::Matrix<double, STATE_DIM, STATE_DIM>& A,
        const Eigen::Matrix<double, STATE_DIM, CONTROL_DIM>& B,
        const Eigen::Matrix<double, STATE_DIM, STATE_DIM>& C = Eigen::Matrix<double, STATE_DIM, STATE_DIM>::Identity(),
        const Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> D = Eigen::Matrix<double, STATE_DIM, CONTROL_DIM>::Zero())
        : A_(A), B_(B), C_(C), D_(D)
    {
    }

    LTISystem(const LTISystem& arg) : A_(arg.A_), B_(arg.B_), C_(arg.C_), D_(arg.D_) {}
    LTISystem<STATE_DIM, CONTROL_DIM>* clone() const override { return new LTISystem<STATE_DIM, CONTROL_DIM>(*this); }
    virtual ~LTISystem() {}
    virtual const Eigen::Matrix<double, STATE_DIM, STATE_DIM>& getDerivativeState(const StateVector<STATE_DIM>& x,
        const ControlVector<CONTROL_DIM>& u,
        const double t = 0.0) override
    {
        return A_;
    }

    virtual const Eigen::Matrix<double, STATE_DIM, CONTROL_DIM>& getDerivativeControl(const StateVector<STATE_DIM>& x,
        const ControlVector<CONTROL_DIM>& u,
        const double t = 0.0) override
    {
        return B_;
    }

    Eigen::Matrix<double, STATE_DIM, STATE_DIM>& A() { return A_; }
    Eigen::Matrix<double, STATE_DIM, CONTROL_DIM>& B() { return B_; }
    Eigen::Matrix<double, STATE_DIM, STATE_DIM>& C() { return C_; }
    Eigen::Matrix<double, STATE_DIM, CONTROL_DIM>& D() { return D_; }

    void computeControlledDynamics(const Eigen::Matrix<double, STATE_DIM, 1>& state,
        const Time& t,
        const Eigen::Matrix<double, CONTROL_DIM, 1>& control,
        Eigen::Matrix<double, STATE_DIM, 1>& derivative)
    {
        derivative = A_ * state + B_ * control;
    }


    void computeOutput(const Eigen::Matrix<double, STATE_DIM, 1>& state,
        const Time& t,
        const Eigen::Matrix<double, CONTROL_DIM, 1>& control,
        Eigen::Matrix<double, STATE_DIM, 1>& output)
    {
        output = C_ * state + D_ * control;
    }


    void computeControllabilityMatrix(Eigen::Matrix<double, STATE_DIM, STATE_DIM * CONTROL_DIM>& CO)
    {
        CO.block<STATE_DIM, CONTROL_DIM>(0, 0) = B_;

        for (size_t i = 1; i < STATE_DIM; i++)
        {
            CO.block<STATE_DIM, CONTROL_DIM>(0, i * CONTROL_DIM) =
                A_ * CO.block<STATE_DIM, CONTROL_DIM>(0, (i - 1) * CONTROL_DIM);
        }
    }


    bool isControllable()
    {
        Eigen::Matrix<double, STATE_DIM, STATE_DIM * CONTROL_DIM> CO;
        computeControllabilityMatrix(CO);

        Eigen::FullPivLU<Eigen::Matrix<double, STATE_DIM, STATE_DIM * CONTROL_DIM>> LUdecomposition(CO);
        return LUdecomposition.rank() == STATE_DIM;
    }


    void computeObservabilityMatrix(Eigen::Matrix<double, STATE_DIM, STATE_DIM * STATE_DIM>& O)
    {
        O.block<STATE_DIM, STATE_DIM>(0, 0) = C_;

        for (size_t i = 1; i < STATE_DIM; i++)
        {
            O.block<STATE_DIM, STATE_DIM>(i * STATE_DIM, 0) =
                O.block<STATE_DIM, STATE_DIM>(0, (i - 1) * STATE_DIM) * A_;
        }
    }


    bool isObservable()
    {
        Eigen::Matrix<double, STATE_DIM, STATE_DIM * STATE_DIM> O;
        computeObservabilityMatrix(O);

        Eigen::FullPivLU<Eigen::Matrix<double, STATE_DIM, STATE_DIM * STATE_DIM>> LUdecomposition(O);
        return LUdecomposition.rank() == STATE_DIM;
    }


private:
    Eigen::Matrix<double, STATE_DIM, STATE_DIM> A_;    
    Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> B_;  

    Eigen::Matrix<double, STATE_DIM, STATE_DIM> C_;    
    Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> D_;  
};

}  // namespace core
}  // namespace ct

#pragma GCC diagnostic pop