StateSpaceModel.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
 
namespace CCC
{
template<int StateDim, int InputDim, int OutputDim>
class StateSpaceModel
{
public:
  using StateDimVector = Eigen::Matrix<double, StateDim, 1>;
 
  using InputDimVector = Eigen::Matrix<double, InputDim, 1>;
 
  using OutputDimVector = Eigen::Matrix<double, OutputDim, 1>;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  StateSpaceModel(int state_dim = StateDim, int input_dim = InputDim, int output_dim = OutputDim)
  : state_dim_(state_dim), input_dim_(input_dim), output_dim_(output_dim)
  {
    // Check dimension is positive
    if(state_dim_ <= 0)
    {
      throw std::runtime_error("state_dim must be positive: " + std::to_string(state_dim_) + " <= 0");
    }
    if(input_dim_ < 0)
    {
      throw std::runtime_error("input_dim must be non-negative: " + std::to_string(input_dim_) + " < 0");
    }
    if(output_dim_ < 0)
    {
      throw std::runtime_error("output_dim must be non-negative: " + std::to_string(output_dim_) + " < 0");
    }
 
    // Check dimension consistency
    if constexpr(StateDim != Eigen::Dynamic)
    {
      if(state_dim_ != StateDim)
      {
        throw std::runtime_error("state_dim is inconsistent with template parameter: " + std::to_string(state_dim_)
                                 + " != " + std::to_string(StateDim));
      }
    }
    if constexpr(InputDim != Eigen::Dynamic)
    {
      if(input_dim_ != InputDim)
      {
        throw std::runtime_error("input_dim is inconsistent with template parameter: " + std::to_string(input_dim_)
                                 + " != " + std::to_string(InputDim));
      }
    }
    if constexpr(OutputDim != Eigen::Dynamic)
    {
      if(output_dim_ != OutputDim)
      {
        throw std::runtime_error("output_dim is inconsistent with template parameter: " + std::to_string(output_dim_)
                                 + " != " + std::to_string(OutputDim));
      }
    }
 
    // Initialize with zero matrix if size is fixed
    A_.setZero(state_dim_, state_dim_);
    B_.setZero(state_dim_, input_dim_);
    C_.setZero(output_dim_, state_dim_);
    D_.setZero(output_dim_, input_dim_);
    E_.setZero(state_dim_);
    F_.setZero(output_dim_);
  }
 
  virtual ~StateSpaceModel() = default;
 
  int stateDim() const
  {
    return state_dim_;
  }
 
  int inputDim() const
  {
    return input_dim_;
  }
 
  int outputDim() const
  {
    return output_dim_;
  }
 
  StateDimVector stateEq(const StateDimVector & x, const InputDimVector & u) const
  {
    return A_ * x + B_ * u + E_;
  }
 
  StateDimVector stateEqDisc(const StateDimVector & x, const InputDimVector & u) const
  {
    return Ad_ * x + Bd_ * u + Ed_;
  }
 
  OutputDimVector observEq(const StateDimVector & x) const
  {
    return C_ * x + F_;
  }
 
  OutputDimVector observEq(const StateDimVector & x, const InputDimVector & u) const
  {
    return C_ * x + D_ * u + F_;
  }
 
  void calcDiscMatrix(double dt)
  {
    dt_ = dt;
 
    // Zero-order hold discretization
    // See https://en.wikipedia.org/wiki/Discretization
    if constexpr(StateDim == Eigen::Dynamic || InputDim == Eigen::Dynamic)
    {
      // If dimension is dynamic
      if(E_.norm() == 0)
      {
        Eigen::MatrixXd ABZero(state_dim_ + input_dim_, state_dim_ + input_dim_);
        ABZero << dt_ * A_, dt_ * B_, Eigen::MatrixXd::Zero(input_dim_, state_dim_ + input_dim_);
        Eigen::MatrixXd ABZeroExp = ABZero.exp();
        Ad_ = ABZeroExp.block(0, 0, state_dim_, state_dim_);
        Bd_ = ABZeroExp.block(0, state_dim_, state_dim_, input_dim_);
        Ed_.setZero(state_dim_);
      }
      else
      {
        // There is no proof that this is correct.
        Eigen::MatrixXd ABEZero(state_dim_ + input_dim_ + 1, state_dim_ + input_dim_ + 1);
        ABEZero << dt_ * A_, dt_ * B_, dt_ * E_, Eigen::MatrixXd::Zero(input_dim_ + 1, state_dim_ + input_dim_ + 1);
        Eigen::MatrixXd ABEZeroExp = ABEZero.exp();
        Ad_ = ABEZeroExp.block(0, 0, state_dim_, state_dim_);
        Bd_ = ABEZeroExp.block(0, state_dim_, state_dim_, input_dim_);
        Ed_ = ABEZeroExp.block(0, state_dim_ + input_dim_, state_dim_, 1);
      }
    }
    else
    {
      // If dimension is fixed
      if(E_.norm() == 0)
      {
        Eigen::Matrix<double, StateDim + InputDim, StateDim + InputDim> ABZero;
        ABZero << dt_ * A_, dt_ * B_, Eigen::Matrix<double, InputDim, StateDim + InputDim>::Zero();
        Eigen::Matrix<double, StateDim + InputDim, StateDim + InputDim> ABZeroExp = ABZero.exp();
        Ad_ = ABZeroExp.template block<StateDim, StateDim>(0, 0);
        Bd_ = ABZeroExp.template block<StateDim, InputDim>(0, StateDim);
        Ed_.setZero();
      }
      else
      {
        // There is no proof that this is correct.
        Eigen::Matrix<double, StateDim + InputDim + 1, StateDim + InputDim + 1> ABEZero;
        ABEZero << dt_ * A_, dt_ * B_, dt_ * E_, Eigen::Matrix<double, InputDim + 1, StateDim + InputDim + 1>::Zero();
        Eigen::Matrix<double, StateDim + InputDim + 1, StateDim + InputDim + 1> ABEZeroExp = ABEZero.exp();
        Ad_ = ABEZeroExp.template block<StateDim, StateDim>(0, 0);
        Bd_ = ABEZeroExp.template block<StateDim, InputDim>(0, StateDim);
        Ed_ = ABEZeroExp.template block<StateDim, 1>(0, StateDim + InputDim);
      }
    }
  }
 
public:
  const int state_dim_ = 0;
 
  const int input_dim_ = 0;
 
  const int output_dim_ = 0;
 
  Eigen::Matrix<double, StateDim, StateDim> A_;
 
  Eigen::Matrix<double, StateDim, InputDim> B_;
 
  Eigen::Matrix<double, OutputDim, StateDim> C_;
 
  Eigen::Matrix<double, OutputDim, InputDim> D_;
 
  StateDimVector E_;
 
  OutputDimVector F_;
 
  double dt_ = 0;
 
  Eigen::Matrix<double, StateDim, StateDim> Ad_;
 
  Eigen::Matrix<double, StateDim, InputDim> Bd_;
 
  StateDimVector Ed_;
};
} // namespace CCC