Lodestar
An integrated real-time control package in C++
BilinearTransformation.hpp
1 //
2 // Created by Hamza El-Kebir on 4/17/21.
3 //
4 
5 #ifndef LODESTAR_BILINEARTRANSFORMATION_HPP
6 #define LODESTAR_BILINEARTRANSFORMATION_HPP
7 
8 #include <Eigen/Dense>
9 #include "Lodestar/systems/StateSpace.hpp"
10 #include "Lodestar/aux/CompileTimeQualifiers.hpp"
11 
12 namespace ls {
13  namespace analysis {
22  class BilinearTransformation {
23  public:
24  template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
25  struct mallocStructC2D {
26  Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH;
27  Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH2;
28  Eigen::Matrix<TScalar, TStateDim, TStateDim> I;
29  };
30 
31  template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
32  struct mallocStructD2C {
33  Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH;
34  Eigen::Matrix<TScalar, TStateDim, TStateDim> IMAC;
35  Eigen::Matrix<TScalar, TStateDim, TStateDim> I;
36  };
37 
52  static systems::StateSpace<>
53  c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
54  const Eigen::MatrixXd &C, const Eigen::MatrixXd &D,
55  double dt,
56  double alpha = 1);
57 
72  static systems::StateSpace<>
73  c2d(const Eigen::MatrixXd *A, const Eigen::MatrixXd *B, const Eigen::MatrixXd *C, const Eigen::MatrixXd *D,
74  double dt,
75  double alpha);
76 
88  static systems::StateSpace<>
89  c2d(const systems::StateSpace<> &ss, double dt, double alpha = 1);
90 
91  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
92  static void
93  c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
94  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
95  mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
96  LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
97 
98  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
99  static void
100  c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
101  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
102  mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
103  LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
104 
116  static systems::StateSpace<>
117  d2c(const systems::StateSpace<> &ss, double dt,
118  double alpha = 1);
119 
120  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
121  static void
122  d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
123  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
124  mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
125  LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
126 
127  template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
128  static void
129  d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
130  systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
131  mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
132  LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
133 
147  static systems::StateSpace<>
148  d2c(const systems::StateSpace<> &ss, double alpha = 1);
149 
164  static systems::StateSpace<>
165  d2c(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
166  const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt,
167  double alpha = 1);
168 
183  static systems::StateSpace<>
184  d2c(const Eigen::MatrixXd *A, const Eigen::MatrixXd *B, const Eigen::MatrixXd *C, const Eigen::MatrixXd *D,
185  double dt,
186  double alpha);
187 
197  static systems::StateSpace<>
198  c2dTustin(const systems::StateSpace<> &ss, double dt);
199 
209  static systems::StateSpace<>
210  d2cTustin(const systems::StateSpace<> &ss, double dt);
211 
221  static systems::StateSpace<>
222  c2dEuler(const systems::StateSpace<> &ss, double dt);
223 
233  static systems::StateSpace<>
234  d2cEuler(const systems::StateSpace<> &ss, double dt);
235 
245  static systems::StateSpace<>
246  c2dBwdDiff(const systems::StateSpace<> &ss, double dt);
247 
257  static systems::StateSpace<>
258  d2cBwdDiff(const systems::StateSpace<> &ss, double dt);
259  };
260  }
261 }
262 
263 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
264 void
265 ls::analysis::BilinearTransformation::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
266  double dt, double alpha,
267  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
268  mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
269  LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
270 {
271  if (alpha < 0 || alpha > 1) alpha = 0;
272  dt = abs(dt);
273 
274  memStruct->I.setIdentity(ss->stateDim(), ss->stateDim());
275  memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
276  memStruct->I - alpha * dt * (ss->getA()));
277  memStruct->HH2 = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
278  (memStruct->I - alpha * dt * (ss->getA())).transpose());
279 
280  out->setA(memStruct->HH.template solve(memStruct->I - (1 - alpha) * dt * (ss->getA())));
281  out->setB(memStruct->HH.template solve(dt * (ss->getB())));
282  out->setC(memStruct->HH2.template solve((ss->getC()).transpose()).transpose());
283  out->setD((ss->getD()) + alpha * (ss->getC()) * (out->getB()));
284  out->setDiscreteParams(dt, true);
285 }
286 
287 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
288 void
289 ls::analysis::BilinearTransformation::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
290  double dt, double alpha,
291  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
292  mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
293  LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
294 {
295  if (alpha < 0 || alpha > 1) alpha = 0;
296  dt = abs(dt);
297 
298  memStruct->I.setIdentity();
299  memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
300  memStruct->I - alpha * dt * (ss->getA()));
301  memStruct->HH2 = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
302  (memStruct->I - alpha * dt * (ss->getA())).transpose());
303 
304  out->setA(memStruct->HH.template solve(memStruct->I - (1 - alpha) * dt * (ss->getA())));
305  out->setB(memStruct->HH.template solve(dt * (ss->getB())));
306  out->setC(memStruct->HH2.template solve((ss->getC()).transpose()).transpose());
307  out->setD((ss->getD()) + alpha * (ss->getC()) * (*out->getB()));
308  out->setDiscreteParams(dt, true);
309 }
310 
311 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
312 void
313 ls::analysis::BilinearTransformation::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
314  double dt, double alpha,
315  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
316  mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
317  LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
318 {
319  if (alpha < 0 || alpha > 1) alpha = 0;
320  dt = abs(dt);
321 
322  memStruct->I.setIdentity(ss->stateDim(), ss->stateDim());
323  memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
324  alpha * dt * (ss->getA()).transpose() + (1 - alpha) * dt * memStruct->I);
325  out->setA(memStruct->HH.template solve((ss->getA()).transpose() - memStruct->I));
326  memStruct->IMAC = memStruct->I - alpha * dt * (out->getA());
327 
328  out->setB(memStruct->IMAC * (ss->getB()) / dt);
329  out->setC((ss->getC()) * memStruct->IMAC);
330  out->setD((ss->getD()) - alpha * (out->getC()) * (ss->getB()));
331  out->setDiscreteParams(-1, false);
332 }
333 
334 template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
335 void
336 ls::analysis::BilinearTransformation::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
337  double dt, double alpha,
338  ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
339  mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
340  LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
341 {
342  if (alpha < 0 || alpha > 1) alpha = 0;
343  dt = abs(dt);
344 
345  memStruct->I.setIdentity();
346  memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
347  alpha * dt * (ss->getA()).transpose() + (1 - alpha) * dt * memStruct->I);
348  out->setA(memStruct->HH.template solve((ss->getA()).transpose() - memStruct->I));
349  memStruct->IMAC = memStruct->I - alpha * dt * (out->getA());
350 
351  out->setB(memStruct->IMAC * (ss->getB()) / dt);
352  out->setC((ss->getC()) * memStruct->IMAC);
353  out->setD((ss->getD()) - alpha * (out->getC()) * (ss->getB()));
354  out->setDiscreteParams(-1, false);
355 }
356 
357 #endif //LODESTAR_BILINEARTRANSFORMATION_HPP
ls::analysis::BilinearTransformation
Routines for converting a state space system from continuous- to discrete-time and vice versa.
Definition: BilinearTransformation.hpp:22
ls::systems::StateSpace::getB
const TDInputMatrix & getB() const
Gets the input matrix.
Definition: StateSpace.hpp:488
ls::systems::StateSpace::getD
const TDFeedforwardMatrix & getD() const
Gets the feedforward matrix.
Definition: StateSpace.hpp:557
ls::systems::StateSpace::getA
const TDStateMatrix & getA() const
Gets the state matrix.
Definition: StateSpace.hpp:454
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::analysis::BilinearTransformation::d2c
static systems::StateSpace d2c(const systems::StateSpace<> &ss, double dt, double alpha=1)
Generates generalized bilinear transform of a discrete-time state space system.
Definition: BilinearTransformation.cpp:71
ls::analysis::BilinearTransformation::d2cBwdDiff
static systems::StateSpace d2cBwdDiff(const systems::StateSpace<> &ss, double dt)
Generates backward differencing transform of a discrete-time state space system.
Definition: BilinearTransformation.cpp:121
ls::systems::StateSpace::getC
const TDOutputMatrix & getC() const
Gets the output matrix.
Definition: StateSpace.hpp:522
ls::analysis::BilinearTransformation::d2cEuler
static systems::StateSpace d2cEuler(const systems::StateSpace<> &ss, double dt)
Generates Euler transform of a discrete-time state space system.
Definition: BilinearTransformation.cpp:107
ls::analysis::BilinearTransformation::c2dEuler
static systems::StateSpace c2dEuler(const systems::StateSpace<> &ss, double dt)
Generates Euler transform of a continuous-time state space system.
Definition: BilinearTransformation.cpp:100
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::analysis::BilinearTransformation::c2d
static systems::StateSpace c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt, double alpha=1)
Generates generalized bilinear transform of a continuous-time state space system.
Definition: BilinearTransformation.cpp:8
ls::systems::StateSpace
Definition: StateSpace.hpp:15
ls::analysis::BilinearTransformation::c2dTustin
static systems::StateSpace c2dTustin(const systems::StateSpace<> &ss, double dt)
Generates Tustin transform of a continuous-time state space system.
Definition: BilinearTransformation.cpp:86
ls::analysis::BilinearTransformation::d2cTustin
static systems::StateSpace d2cTustin(const systems::StateSpace<> &ss, double dt)
Generates Tustin transform of a discrete-time state space system.
Definition: BilinearTransformation.cpp:93
ls::analysis::BilinearTransformation::c2dBwdDiff
static systems::StateSpace c2dBwdDiff(const systems::StateSpace<> &ss, double dt)
Generates backward differencing transform of a continuous-time state space system.
Definition: BilinearTransformation.cpp:114
ls::systems::StateSpace::setC
void setC(TDOutputMatrix *C)
Sets the output matrix.
Definition: StateSpace.hpp:529
ls::analysis::BilinearTransformation::mallocStructD2C
Definition: BilinearTransformation.hpp:32
ls::systems::StateSpace::setD
void setD(TDFeedforwardMatrix *D)
Sets the feedforward matrix.
Definition: StateSpace.hpp:563
ls::analysis::BilinearTransformation::mallocStructC2D
Definition: BilinearTransformation.hpp:25
ls::systems::StateSpace::setB
void setB(TDInputMatrix *B)
Sets the input matrix.
Definition: StateSpace.hpp:494