LinearSystemInverse.hpp

//
// Created by Hamza El-Kebir on 4/17/21.
//
 
#ifndef LODESTAR_LINEARSYSTEMINVERSE_HPP
#define LODESTAR_LINEARSYSTEMINVERSE_HPP
 
#include "Lodestar/systems/StateSpace.hpp"
 
namespace ls {
    namespace analysis {
        class LinearSystemInverse {
        public:
            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            inverse(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                    systems::StateSpace<TScalar, TStateDim, TOutputDim, TInputDim> *out);
 
            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            inverse(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> &ss,
                    systems::StateSpace<TScalar, TStateDim, TOutputDim, TInputDim> *out);
 
            static systems::StateSpace<>
            inverse(const systems::StateSpace<> *ss);
 
            static systems::StateSpace<>
            inverse(const systems::StateSpace<> &ss);
        };
    }
}
 
// TODO: Return status in case system is not invertible or is in discrete-time
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::LinearSystemInverse::inverse(
        const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
        systems::StateSpace<TScalar, TStateDim, TOutputDim, TInputDim> *out)
{
    out->setA((ss->getA()) -
              (ss->getB()) * (ss->getD()).inverse() * (ss->getC()));
    out->setB(-(ss->getB()) * (ss->getD()).inverse());
    out->setC((ss->getD()).inverse() * (ss->getC()));
    out->setD((ss->getD()).inverse());
}
 
template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::LinearSystemInverse::inverse(
        const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> &ss,
        ls::systems::StateSpace<TScalar, TStateDim, TOutputDim, TInputDim> *out)
{
    inverse(&ss, out);
}
 
#endif //LODESTAR_LINEARSYSTEMINVERSE_HPP