From 10e5b87b396b7ac9f47c471de5d029609e4098fd Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Mon, 12 Feb 2024 20:12:21 +0100 Subject: Operators for TF --- src/control.cpp | 93 +++++++++++++++++++++++++++++++++++++++++++++++++++------ src/control.h | 71 +++++++++++++++++++++++++++++++++---------- src/main.cpp | 43 ++++++++++++++++---------- 3 files changed, 167 insertions(+), 40 deletions(-) diff --git a/src/control.cpp b/src/control.cpp index f57baec..4abc899 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -1,16 +1,30 @@ #include "control.h" #include +#include +#include namespace ct { - TransferFn::TransferFn(complex gain) - : gain(gain) - , num(math::Poly{1}) - , den(math::Poly{1}) + TransferFn::TransferFn() + : num(1) + , den(1) { - num(0) = 1.; - den(0) = 1.; + num(0) = 1; + den(0) = 1; + } + + TransferFn::TransferFn(math::PolyCx num, math::PolyCx den) + : num(num) + , den(den) + {} + + complex TransferFn::dc_gain() const + { + arma::cx_vec z(1, arma::fill::zeros); + arma::cx_vec n = arma::polyval(num.coeffs, z); + arma::cx_vec d = arma::polyval(den.coeffs, z); + return n(0) / d(0); } bool TransferFn::is_proper() const @@ -23,9 +37,70 @@ namespace ct return den.degree() >= num.degree(); } - TransferFn feedback(const TransferFn& tf, double k) + TransferFn operator + (const TransferFn& g, const TransferFn& h) + { + TransferFn tf(g.num * h.den + h.num * g.den, g.den * g.den); + tf.canonicalize(); + return tf; + } + + TransferFn operator - (const TransferFn& g, const TransferFn& h) { - // TransferFn tf_cl; + TransferFn tf = g + (-1. * h); + tf.canonicalize(); + return tf; + } + + TransferFn operator * (const TransferFn& g, const TransferFn& h) + { + TransferFn tf(g.num * h.num, g.den * h.den); + tf.canonicalize(); + return tf; + } + + TransferFn operator / (const TransferFn& g, const TransferFn& h) + { + TransferFn hinv = TransferFn(h.den, h.num); + hinv.canonicalize(); + TransferFn tf = g * hinv; + tf.canonicalize(); + return tf; + } + + TransferFn operator * (const complex k, const TransferFn& h) + { + TransferFn tf(k * h.num, h.den); + tf.canonicalize(); + return tf; + } + + TransferFn operator / (const TransferFn& h, const complex k) + { + TransferFn tf((1. / k) * h.num, h.den); + tf.canonicalize(); + return tf; + } + + + TransferFn feedback(const TransferFn& tf, complex k) + { + const TransferFn unit(math::PolyCx({1.}), math::PolyCx({1.})); + const TransferFn ol = k * tf; + const TransferFn bo = unit - ol; + const TransferFn fb = unit / bo; + const TransferFn tf_cl = ol * fb; + // const TransferFn tf_cl = (k * tf) / (unit - k * tf); + return tf_cl; + } + + void TransferFn::canonicalize() + { + complex an = den(0); + if (std::abs(an) > 0) + { + num.coeffs /= an; + den.coeffs /= an; + } } LocusSeries::LocusSeries(double start, double end, size_t nsamples) @@ -77,7 +152,7 @@ namespace ct { ss.A(ord - 1, i) = tf.den(ord - i); if (i < tf.num.coeffs.n_elem) - ss.C(i) = tf.num(i) * tf.gain; + ss.C(i) = tf.num(i); } return ss; diff --git a/src/control.h b/src/control.h index 8ae9c5d..587b80e 100644 --- a/src/control.h +++ b/src/control.h @@ -1,9 +1,12 @@ #pragma once -#include +#include "armadillo/include/armadillo" + +#include #include #include #include +#include #ifndef CONTROL_H #define CONTROL_H @@ -14,7 +17,8 @@ namespace ct { - using complex = std::complex; + typedef std::complex complex; + typedef std::size_t size_t; namespace math { @@ -23,11 +27,19 @@ namespace ct { arma::Col coeffs; - Poly(size_t n_elem) : coeffs(n_elem) {}; + Poly(size_t n_elem) : coeffs(n_elem) {} Poly(arma::Col c) : coeffs(c) {} + template + Poly(std::initializer_list l) : coeffs(l.size()) + { + int i = 0; + for (const U& u : l) + coeffs(i++) = T(u); + } + int degree() const { return coeffs.n_elem - 1; } - void add_root(T r) { (*this) = (*this) * Poly({1, -r}); } + void add_root(T r) { (*this) = (*this) * Poly({1., -r}); } arma::Col roots() const { return arma::roots(coeffs); } template @@ -41,6 +53,7 @@ namespace ct Poly operator * (const Poly& p, const Poly& q) { arma::Col coeffs = arma::conv(p.coeffs, q.coeffs); + CT_ASSERT(!coeffs.has_nan()); return Poly(coeffs); } @@ -48,6 +61,15 @@ namespace ct Poly operator * (R scalar, const Poly& p) { Poly q(p.coeffs * scalar); + CT_ASSERT(!q.coeffs.has_nan()); + return q; + } + + template + Poly operator / (R scalar, const Poly& p) + { + Poly q(p.coeffs / scalar); + CT_ASSERT(!q.coeffs.has_nan()); return q; } @@ -57,34 +79,53 @@ namespace ct const Poly& big = (p.degree() > q.degree()) ? p : q; const Poly& small = (p.degree() > q.degree()) ? q : p; - Poly s(big.degree() + 1); - - for (int i = 0; i <= small.degree(); i++) - s(i) = big(i) + small(i); + Poly s(small); + int rel = big.degree() - small.degree(); + s.coeffs.insert_rows(0, rel); - for (int i = small.degree() + 1; i <= big.degree(); i++) - s(i) = big(i); + CT_ASSERT(!s.coeffs.has_nan()); + return Poly(big.coeffs + s.coeffs); + } - return s; + template + Poly operator - (const Poly& p, const Poly& q) + { + return p + (-1. * q); } + + typedef Poly PolyCx; } struct TransferFn { complex gain; - math::Poly num; - math::Poly den; + math::PolyCx num; + math::PolyCx den; - TransferFn(complex gain); + TransferFn(void); + TransferFn(math::PolyCx num, math::PolyCx den); inline void add_pole(complex p) { den.add_root(p); } inline void add_zero(complex z) { num.add_root(z); }; + complex dc_gain() const; + bool is_proper() const; bool is_strictly_proper() const; + + void canonicalize(); }; - TransferFn feedback(const TransferFn& tf, double k = -1); + TransferFn operator + (const TransferFn& g, const TransferFn& h); + TransferFn operator - (const TransferFn& g, const TransferFn& h); + TransferFn operator * (const TransferFn& g, const TransferFn& h); + TransferFn operator / (const TransferFn& g, const TransferFn& h); + TransferFn operator * (const complex k, const TransferFn& h); + TransferFn operator / (const TransferFn& h, const complex k); + + // inline TransferFn operator / (const TransferFn& + + TransferFn feedback(const TransferFn& tf, complex k = -1); struct LocusSeries { diff --git a/src/main.cpp b/src/main.cpp index 2d442b7..f169982 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -34,7 +34,8 @@ struct GState int nyquist; int rlocus; } n_samples; - float gain; + float gain_re; + float gain_im; bool autogain; float time_start; float time_len; @@ -74,11 +75,7 @@ ImPlotPoint step_getter(int idx, void *data) ImPlotPoint rlocus_getter(int idx, void *data) { - ct::LocusSeries *ls = (ct::LocusSeries *) data; - // FIXME: this is a very ugly trick - idx = idx % ls->n_samples; - int row = (int) (idx / ls->n_samples); - return ImPlotPoint(ls->out(row, idx).real(), ls->out(row, idx).imag()); + // return ImPlotPoint(ls->out(row, idx).real(), ls->out(row, idx).imag()); } @@ -152,7 +149,8 @@ int main(int, char**) .nyquist = 500, .rlocus = 2000, }, - .gain = 1., + .gain_re = 1., + .gain_im = 0., .autogain = false, .time_start = 0, .time_len = 10, @@ -198,23 +196,33 @@ int main(int, char**) if (gstate.npoles || gstate.nzeros) { // Create new transfer function - ct::TransferFn tf(gstate.gain); + ct::TransferFn tf; + for (int k = 0; k < gstate.npoles; k++) tf.add_pole(ct::complex(gstate.poles[k].x, gstate.poles[k].y)); for (int k = 0; k < gstate.nzeros; k++) tf.add_zero(ct::complex(gstate.zeros[k].x, gstate.zeros[k].y)); + ct::TransferFn tf_cl = ct::feedback(tf); // Autogain if (gstate.autogain) { - gstate.gain = abs(tf.den.coeffs.back() / tf.num.coeffs.back()); + ct::complex gain = 1. / tf_cl.dc_gain(); + tf_cl = gain * tf_cl; + gstate.gain_re = gain.real(); + gstate.gain_im = gain.imag(); + } + else + { + ct::complex gain(gstate.gain_re, gstate.gain_im); + tf_cl = gain * tf_cl; } - if (tf.is_proper()) + if (tf_cl.is_proper()) { // Convert to state space - ct::SSModel ss = ct::ctrb_form(tf); + ct::SSModel ss = ct::ctrb_form(ct::feedback(tf)); // New time series cstate.time = ct::TimeSeries(gstate.time_start, @@ -253,9 +261,9 @@ int main(int, char**) ImGui::SeparatorText("Plant G(s)"); - ImGui::SliderFloat("Gain", &gstate.gain, -1000, 1000, NULL, ImGuiSliderFlags_Logarithmic); - ImGui::SameLine(); ImGui::Checkbox("Automatic", &gstate.autogain); + ImGui::SliderFloat("Gain (real)", &gstate.gain_re, -1000, 1000, NULL, ImGuiSliderFlags_Logarithmic); + ImGui::SliderFloat("Gain (imag)", &gstate.gain_im, -1000, 1000, NULL, ImGuiSliderFlags_Logarithmic); ImGui::PushItemWidth(140); if (ImGui::BeginListBox("Poles")) @@ -362,9 +370,12 @@ int main(int, char**) ImPlot::SetupAxesLimits(-10, 10, -10, 10); // Plot curren root locus - // FIXME: this is a very ugly trick - ImPlot::PlotLineG("Root Locus", rlocus_getter, &cstate.rlocus, - cstate.rlocus.n_samples * cstate.rlocus.out.n_rows); + for (int i = 0; i < cstate.rlocus.out.n_rows; i++) + { + ImGui::PushID(i); + // ImPlot::PlotLineG("Root Locus", rlocus_getter, &cstate.rlocus.out.row(i), cstate.rlocus.n_samples); + ImGui::PopID(); + } if (ImPlot::IsPlotHovered() && ImGui::GetIO().KeyCtrl) { -- cgit v1.2.1