1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
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:
|
