DiscreteStateSpace.hpp

//
// Created by Hamza El-Kebir on 4/21/21.
//
 
#ifndef LODESTAR_DISCRETESTATESPACE_HPP
#define LODESTAR_DISCRETESTATESPACE_HPP
 
#include "StateSpace.hpp"
#include "Lodestar/analysis/ZeroOrderHold.hpp"
#include "Lodestar/analysis/BilinearTransformation.hpp"
 
namespace ls {
    namespace systems {
        template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
        class DiscreteStateSpace : public StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> {
        public:
            typedef StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> Base;
 
            DiscreteStateSpace() : StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>()
            {
                Base::setDiscreteParams(1);
            }
 
            DiscreteStateSpace(double dt) : StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>()
            {
                Base::setDiscreteParams(dt);
            }
 
            DiscreteStateSpace(const Eigen::MatrixXd &A,
                               const Eigen::MatrixXd &B,
                               const Eigen::MatrixXd &C,
                               const Eigen::MatrixXd &D)
                    : StateSpace<>(A, B, C, D)
            {
                Base::setDiscreteParams(1);
            }
 
            DiscreteStateSpace(const Eigen::MatrixXd &A,
                               const Eigen::MatrixXd &B,
                               const Eigen::MatrixXd &C,
                               const Eigen::MatrixXd &D,
                               double dt)
                    : StateSpace<>(A, B, C, D)
            {
                Base::setDiscreteParams(dt);
            }
 
            DiscreteStateSpace(const DiscreteStateSpace &other) :
                    StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>(other)
            {
                Base::setDiscreteParams(other.dt_, other.isDiscrete_);
            }
 
            void resample(double dt);
 
            void resampleFast(double dt);
 
            bool isStable(double tolerance = 0) const;
        };
    }
}
 
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteStateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::resample(const double dt)
{
    auto dss = ls::analysis::ZeroOrderHold::c2d(
            ls::analysis::ZeroOrderHold::d2c(*this, Base::getSamplingPeriod()), dt);
 
    copyMatrices(dss);
 
    Base::setSamplingPeriod(dt);
}
 
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteStateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::resampleFast(const double dt)
{
    auto dss = ls::analysis::BilinearTransformation::c2dBwdDiff(
            ls::analysis::BilinearTransformation::d2cBwdDiff(*this, Base::getSamplingPeriod()), dt);
 
    copyMatrices(dss);
 
    Base::setSamplingPeriod(dt);
}
 
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
bool ls::systems::DiscreteStateSpace<TScalar, TStateDim, TInputDim, TOutputDim>::isStable(const double tolerance) const
{
    auto eig = Base::A_->eigenvalues();
    double tol = (tolerance < 0 ? -tolerance * tolerance : tolerance *
                                                           tolerance);
 
    for (int i = 0; i < eig.size(); i++) {
        if (eig(i).real() * eig(i).real() + eig(i).imag() * eig(i).imag() > 1 + tol)
            return false;
    }
 
    return true;
}
 
#endif //LODESTAR_DISCRETESTATESPACE_HPP