InvariantSequentialExtension.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <iostream>
#include <memory>
 
#include <CCC/StateSpaceModel.h>
 
namespace CCC
{
template<int StateDim, int InputDim, int OutputDim>
class InvariantSequentialExtension
{
public:
  using _StateSpaceModel = StateSpaceModel<StateDim, InputDim, OutputDim>;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  InvariantSequentialExtension(const std::shared_ptr<_StateSpaceModel> & model,
                               int seq_len,
                               bool extend_for_output = false)
  : model_(model), seq_len_(seq_len)
  {
    setup(extend_for_output);
  }
 
  int totalStateDim() const
  {
    return seq_len_ * StateDim;
  }
 
  int totalInputDim() const
  {
    return seq_len_ * InputDim;
  }
 
  int totalOutputDim() const
  {
    return seq_len_ * OutputDim;
  }
 
protected:
  void setup(bool extend_for_output)
  {
    // Check D matrix
    if(extend_for_output && model_->D_.norm() > 0)
    {
      std::cerr
          << "[InvariantSequentialExtension] Matrix D in state-space model must be zero when extending for output.\n"
          << "  Matrix D:\n"
          << model_->D_;
    }
 
    // Resize matrix
    A_seq_.setZero(seq_len_ * StateDim, StateDim);
    B_seq_.setZero(seq_len_ * StateDim, seq_len_ * InputDim);
    E_seq_.setZero(seq_len_ * StateDim);
 
    for(int i = 0; i < seq_len_; i++)
    {
      // Setq A_seq_
      if(i == 0)
      {
        A_seq_.template middleRows<StateDim>(i * StateDim) = model_->Ad_;
      }
      else
      {
        (A_seq_.template middleRows<StateDim>(i * StateDim)).noalias() =
            model_->Ad_ * A_seq_.template middleRows<StateDim>((i - 1) * StateDim);
      }
 
      // Setq B_seq_
      for(int j = 0; j < seq_len_ - i; j++)
      {
        if(j == 0)
        {
          if(i == 0)
          {
            B_seq_.template block<StateDim, InputDim>(i * StateDim, 0) = model_->Bd_;
          }
          else
          {
            B_seq_.template block<StateDim, InputDim>(i * StateDim, 0).noalias() =
                model_->Ad_ * B_seq_.template block<StateDim, InputDim>((i - 1) * StateDim, 0);
          }
        }
        else
        {
          B_seq_.template block<StateDim, InputDim>((i + j) * StateDim, j * InputDim) =
              B_seq_.template block<StateDim, InputDim>(i * StateDim, 0);
        }
      }
 
      // Setq E_seq_
      if(i == 0)
      {
        E_seq_.template segment<StateDim>(i * StateDim) = model_->Ed_;
      }
      else
      {
        (E_seq_.template segment<StateDim>(i * StateDim)).noalias() =
            model_->Ad_ * E_seq_.template segment<StateDim>((i - 1) * StateDim) + model_->Ed_;
      }
    }
 
    // Transform for output
    if(extend_for_output)
    {
      // Set C_seq
      Eigen::MatrixXd C_seq = Eigen::MatrixXd::Zero(seq_len_ * OutputDim, seq_len_ * StateDim);
      for(int i = 0; i < seq_len_; i++)
      {
        C_seq.template block<OutputDim, StateDim>(i * OutputDim, i * StateDim) = model_->C_;
      }
 
      // Apply C_seq
      A_seq_ = C_seq * A_seq_;
      B_seq_ = C_seq * B_seq_;
      E_seq_ = C_seq * E_seq_;
    }
  }
 
public:
  std::shared_ptr<_StateSpaceModel> model_;
 
  int seq_len_ = 0;
 
  Eigen::Matrix<double, Eigen::Dynamic, StateDim> A_seq_;
 
  Eigen::MatrixXd B_seq_;
 
  Eigen::VectorXd E_seq_;
};
} // namespace CCC