diff options
Diffstat (limited to '')
| -rw-r--r-- | src/control.cpp | 130 | ||||
| -rw-r--r-- | src/control.cpp.old | 46 |
2 files changed, 176 insertions, 0 deletions
diff --git a/src/control.cpp b/src/control.cpp new file mode 100644 index 0000000..f57baec --- /dev/null +++ b/src/control.cpp @@ -0,0 +1,130 @@ +#include "control.h" +#include <armadillo> + + +namespace ct +{ + TransferFn::TransferFn(complex gain) + : gain(gain) + , num(math::Poly<complex>{1}) + , den(math::Poly<complex>{1}) + { + num(0) = 1.; + den(0) = 1.; + } + + bool TransferFn::is_proper() const + { + return den.degree() >= num.degree(); + } + + bool TransferFn::is_strictly_proper() const + { + return den.degree() >= num.degree(); + } + + TransferFn feedback(const TransferFn& tf, double k) + { + // TransferFn tf_cl; + } + + LocusSeries::LocusSeries(double start, double end, size_t nsamples) + : n_samples(nsamples) + , start(start) + , end(end) + , in(arma::logspace(log10(start), log10(end), n_samples)) + // FIXME: do not hardcode one root locus + , out(1, n_samples, arma::fill::zeros) + {} + + void rlocus(const TransferFn& tf, LocusSeries& ls) + { + using namespace arma; + + CT_ASSERT(tf.is_proper()); + CT_ASSERT(tf.den.degree() > 0); + + // prepare output + ls.out = cx_mat(tf.den.degree(), ls.n_samples, fill::zeros); + + // compute roots + for (int i = 0; i < ls.n_samples; i++) + ls.out.col(i) = (tf.den + ls.in(i) * tf.num).roots(); + + // sort the roots + } + + SSModel::SSModel(size_t n_in, size_t n_out, size_t n_states) + : n_in(n_in) + , n_out(n_out) + , n_states(n_states) + , A(n_states, n_states) + , B(n_states, n_in) + , C(n_out, n_states) + , D(n_out, n_in) + {} + + SSModel ctrb_form(const TransferFn& tf) + { + // TODO: change to proper by implementing D + CT_ASSERT(tf.is_strictly_proper()); + + int ord = tf.den.degree(); + SSModel ss(1, 1, ord); + + ss.B(ord - 1) = 1; + for (int i = 0; i < ord; i++) + { + ss.A(ord - 1, i) = tf.den(ord - i); + if (i < tf.num.coeffs.n_elem) + ss.C(i) = tf.num(i) * tf.gain; + } + + return ss; + } + + TimeSeries::TimeSeries(double start, double end, size_t n_samples) + : n_samples(n_samples) + , start(start) + , end(end) + , dt((start - end) / (n_samples - 1)) + , time(arma::linspace(start, end, n_samples)) + // FIXME: do not hardcode SISO + , in(1, n_samples, arma::fill::zeros) + , out(1, n_samples, arma::fill::zeros) + {} + + void response(const SSModel& ss, TimeSeries& ts) + { + using namespace arma; + + CT_ASSERT(ts.in.n_rows == ss.n_in); + CT_ASSERT(ts.in.n_cols == ts.n_samples); + + ts.out = cx_mat(ss.n_out, ts.n_samples, fill::zeros); + ts.state = cx_mat(ss.n_states, ts.n_samples, fill::zeros); + // FIXME: non-homogeneous initial condition + + // For the current application we want this to be faster rather than + // accurate. Hence we use a simulation with ZOH input is cheap and probably + // good enough. + // FIXME: do not invert matrix like that + const cx_mat Ad = expmat(ss.A * ts.dt); + const cx_mat Bd = (Ad - eye(size(Ad))) * pinv(ss.A) * ss.B; + + for (int k = 0; k < ts.n_samples - 1; k++) + ts.state.col(k + 1) = Ad * ts.state.col(k) + Bd * ts.in.col(k); + + ts.out = ss.C * ts.state + ss.D * ts.in; + } + + + void step(const SSModel& ss, TimeSeries& ts) + { + ts.in = arma::cx_mat(ss.n_in, ts.n_samples); + ts.in.fill(1.); + response(ss, ts); + } +} + +// vim: ts=2 sw=2 noet: 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: |