diff options
Diffstat (limited to 'src/control.cpp.old')
| -rw-r--r-- | src/control.cpp.old | 46 |
1 files changed, 46 insertions, 0 deletions
diff --git a/src/control.cpp.old b/src/control.cpp.old new file mode 100644 index 0000000..0e0d641 --- /dev/null +++ b/src/control.cpp.old @@ -0,0 +1,46 @@ +#include "control.h" + +#include "EigenUnsupported/MatrixFunctions" + +namespace ct +{ + template<typename T> + template<int n, int m, int k> + TimeSeries<T>::TimeSeries(size_t nsamples, T start, T end, const SSModel<T, n, m, k>& ss) + : nsamples(nsamples) + , start(start) , end(end) + , t(nsamples) + , u(ss.u.rows(), nsamples) + , x(ss.x.rows(), nsamples) + , y(ss.y.rows(), nsamples) + {} + + template<typename T, int n, int m, int k> + void response(SSModel<T, n, m, k> ss, TimeSeries<T>& ts) + { + /* Numerically integrate solution with timestep dt and linear interpolation + * between input samples + */ + using namespace Eigen; + + T dt = (ts.start - ts.end) / ts.nsamples; + Matrix<T, n, n> expM = (ss.A * dt).exp(); + Index Ad = expM(seq(0,n), seq(0,n)); + Index Bd1 = expM(seq(0, n), seq(n + m, last)); + Matrix<T, n, m> Bd0 = expM(seq(0, n), seq(n, n + m)) - Bd1; + + for (int i = 1; i < ts.nsamples; i++) + ts.x(all, i) = Ad * ts.x(all, i - 1) + Bd0 * ts.u(all, i - 1) + Bd1 * ts.u(all, i); + + ts.y = ss.C * ts.x + ss.D * ts.u; + } + + template<typename T, int n, int m, int k> + void step(SSModel<T, n, m, k> ss, TimeSeries<T>& ts) + { + ts.u.fill(static_cast<T>(1)); + response(ss, ts); + } +} + +// vim: ts=2 sw=2 noet: |