Lodestar
An integrated real-time control package in C++
ZeroOrderHold.hpp
1 //
2 // Created by Hamza El-Kebir on 4/17/21.
3 //
4 
5 #ifndef LODESTAR_ZEROORDERHOLD_HPP
6 #define LODESTAR_ZEROORDERHOLD_HPP
7 
8 #include <Eigen/Dense>
9 #include <unsupported/Eigen/MatrixFunctions>
10 #include "Lodestar/systems/StateSpace.hpp"
11 #include "Lodestar/aux/CompileTimeQualifiers.hpp"
12 
13 namespace ls {
14  namespace analysis {
21  class ZeroOrderHold {
22  public:
23  template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
24  struct mallocStruct {
25  template<int TTStateDim = TStateDim, int TTInputDim = TInputDim, int TTOutputDim = TOutputDim, typename ::std::enable_if<
26  (TTStateDim >= 0) && (TTInputDim >= 0) && (TTOutputDim >= 0)>::type * = nullptr>
27  mallocStruct() : upperXM(decltype(upperXM)::Zero()), lowerXM(decltype(lowerXM)::Zero()),
28  XM(decltype(XM)::Zero()), XXM(
29  decltype(XXM)::Zero())
30  {}
31 
32  template<int TTStateDim = TStateDim, int TTInputDim = TInputDim, int TTOutputDim = TOutputDim, typename ::std::enable_if<
33  (TTStateDim < 0) && (TTInputDim < 0) && (TTOutputDim < 0)>::type * = nullptr>
34  mallocStruct()
35  {}
36 
37  Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> upperXM;
38  Eigen::Matrix<TScalar, TInputDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> lowerXM;
39  Eigen::Matrix<TScalar, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim), LS_STATIC_UNLESS_DYNAMIC(
40  TStateDim + TInputDim)> XM;
41  Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> XXM;
42  };
43 
56  static systems::StateSpace<>
57  c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
58  const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt);
59 
72  static systems::StateSpace<>
73  c2d(Eigen::MatrixXd *A, Eigen::MatrixXd *B, Eigen::MatrixXd *C, Eigen::MatrixXd *D, double dt);
74 
75 
76  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
77  static void c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
78  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
79  mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
80  LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
81 
82  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
83  static void c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
84  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
85  mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
86  LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
87 
97  static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
98  c2d(const systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> &ss, double dt);
99 
109  static systems::StateSpace<> c2d(const systems::StateSpace<> *ss, double dt);
110 
123  static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
124  d2c(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
125  const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt);
126 
139  static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
140  d2c(Eigen::MatrixXd *A, Eigen::MatrixXd *B,
141  Eigen::MatrixXd *C, Eigen::MatrixXd *D, double dt);
142 
152  static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
153  d2c(const systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> &ss, double dt);
154 
164  static systems::StateSpace<>
165  d2c(const systems::StateSpace<> *ss, double dt);
166 
179  static systems::StateSpace<>
180  d2c(const systems::StateSpace<> *ss);
181 
194  static systems::StateSpace<>
195  d2c(const systems::StateSpace<> &ss);
196 
206  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
207  static void d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
208  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
209  mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
210  LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
211 
221  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
222  static void d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
223  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
224  mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
225  LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
226  };
227  }
228 }
229 
230 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
231 void ls::analysis::ZeroOrderHold::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
232  double dt,
233  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
234  ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
235  LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
236 {
237  dt = abs(dt);
238 
239  const long n = ss->stateDim();
240  const long m = ss->inputDim();
241 
242  memStruct->upperXM.block(0, 0, n, n) << (ss->getA());
243  memStruct->upperXM.block(0, n, n, m) << (ss->getB());
244 
245  memStruct->lowerXM.setZero();
246 
247  memStruct->XM.block(0, 0, n, n + m) << memStruct->upperXM;
248  memStruct->XM.block(n, 0, m, n + m) << memStruct->lowerXM;
249 
250  memStruct->XXM = (memStruct->XM * dt).exp().block(0, 0, n, n + m);
251 
252  out->setA(memStruct->XXM.block(0, 0, n, n));
253  out->setB(memStruct->XXM.block(0, n, n, m));
254  out->setC(Eigen::MatrixXd::Identity(n, n));
255  out->setD(Eigen::MatrixXd::Zero(n, m));
256  out->setDiscreteParams(dt, true);
257 
258 }
259 
260 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
261 void ls::analysis::ZeroOrderHold::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
262  double dt,
263  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
264  ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
265  LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
266 {
267  dt = abs(dt);
268 
269  memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0);
270  memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0) << (ss->getA());
271  memStruct->upperXM.template block<TStateDim, TInputDim>(0, TStateDim) << (ss->getB());
272 
273  memStruct->lowerXM.setZero();
274 
275  memStruct->XM.template block<TStateDim, TStateDim + TInputDim>(0, 0) << memStruct->upperXM;
276  memStruct->XM.template block<TInputDim, TStateDim + TInputDim>(TStateDim, 0) << memStruct->lowerXM;
277 
278  memStruct->XXM = (memStruct->XM * dt).exp().template block<TStateDim, TStateDim + TInputDim>(0, 0);
279 
280  out->setA(memStruct->XXM.template block<TStateDim, TStateDim>(0, 0));
281  out->setB(memStruct->XXM.template block<TStateDim, TInputDim>(0, TStateDim));
282  out->setDiscreteParams(dt, true);
283 }
284 
285 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
286 void ls::analysis::ZeroOrderHold::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
287  double dt,
288  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
289  ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
290  LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
291 {
292  dt = abs(dt);
293 
294  const long n = ss->stateDim();
295  const long m = ss->inputDim();
296 
297  memStruct->upperXM.block(0, 0, n, n) << (ss->getA());
298  memStruct->upperXM.block(0, n, n, m) << (ss->getB());
299 
300  memStruct->lowerXM.setZero();
301  memStruct->lowerXM.block(0, n, m, m).setIdentity();
302 
303  memStruct->XM.block(0, 0, n, n + m) << memStruct->upperXM;
304  memStruct->XM.block(n, 0, m, n + m) << memStruct->lowerXM;
305 
306  memStruct->XXM = (memStruct->XM).log().block(0, 0, n, n + m) / dt;
307 
308  out->setA(memStruct->XXM.block(0, 0, n, n));
309  out->setB(memStruct->XXM.block(0, n, n, m));
310 }
311 
312 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
313 void ls::analysis::ZeroOrderHold::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
314  double dt,
315  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
316  ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
317  LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
318 {
319  dt = abs(dt);
320 
321  memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0) << (ss->getA());
322  memStruct->upperXM.template block<TStateDim, TInputDim>(0, TStateDim) << (ss->getB());
323 
324  memStruct->lowerXM.setZero();
325  memStruct->lowerXM.template block<TInputDim, TInputDim>(0, TStateDim)
326  << Eigen::Matrix<TScalar, TInputDim, TInputDim>::Identity();
327 
328  memStruct->XM.template block<TStateDim, TStateDim + TInputDim>(0, 0) << memStruct->upperXM;
329  memStruct->XM.template block<TInputDim, TStateDim + TInputDim>(TStateDim, 0) << memStruct->lowerXM;
330 
331  memStruct->XXM = (memStruct->XM).log().template block<TStateDim, TStateDim + TInputDim>(0, 0) / dt;
332 
333  out->setA(memStruct->XXM.template block<TStateDim, TStateDim>(0, 0));
334  out->setB(memStruct->XXM.template block<TStateDim, TInputDim>(0, TStateDim));
335 }
336 
337 #endif //LODESTAR_ZEROORDERHOLD_HPP
ls::systems::StateSpace::getB
const TDInputMatrix & getB() const
Gets the input matrix.
Definition: StateSpace.hpp:488
ls::systems::StateSpace::getA
const TDStateMatrix & getA() const
Gets the state matrix.
Definition: StateSpace.hpp:454
ls::analysis::ZeroOrderHold
Routines for computing zero-order hold transformation on state space systems.
Definition: ZeroOrderHold.hpp:21
ls::systems::StateSpace::setA
void setA(TDStateMatrix *A)
Sets the state matrix.
Definition: StateSpace.hpp:460
ls::systems::StateSpace::stateDim
stateDim() const
Returns the state dimension.
Definition: StateSpace.hpp:235
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::systems::StateSpace::setDiscreteParams
void setDiscreteParams(double dt)
Sets the discrete time system parameters.
Definition: StateSpace.hpp:609
ls::systems::StateSpace
Definition: StateSpace.hpp:15
ls::analysis::ZeroOrderHold::mallocStruct
Definition: ZeroOrderHold.hpp:24
ls::systems::StateSpace::setC
void setC(TDOutputMatrix *C)
Sets the output matrix.
Definition: StateSpace.hpp:529
ls::systems::StateSpace::inputDim
inputDim() const
Returns the input dimension.
Definition: StateSpace.hpp:262
ls::systems::StateSpace::setD
void setD(TDFeedforwardMatrix *D)
Sets the feedforward matrix.
Definition: StateSpace.hpp:563
ls::analysis::ZeroOrderHold::d2c
static systems::StateSpace< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic > d2c(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt)
Reverts a zero-order hold discretization on a discrete-time state space system.
Definition: ZeroOrderHold.cpp:62
ls::systems::StateSpace::setB
void setB(TDInputMatrix *B)
Sets the input matrix.
Definition: StateSpace.hpp:494
ls::analysis::ZeroOrderHold::c2d
static systems::StateSpace c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt)
Generates zero-order hold discretization from a continuous-time state space system.
Definition: ZeroOrderHold.cpp:8