From 6449c4d7bba3f45274b78a51871f64d429d82f8b Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Mon, 12 Feb 2024 23:58:46 +0100 Subject: Make Nyquist and Bode work --- src/control.cpp | 62 +++++++++++++--- src/control.h | 12 +++- src/main.cpp | 215 +++++++++++++++++++++++++++++++++++++++++--------------- 3 files changed, 219 insertions(+), 70 deletions(-) diff --git a/src/control.cpp b/src/control.cpp index 4abc899..69de3a8 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -81,6 +81,12 @@ namespace ct return tf; } + TransferFn cancel_zp(const TransferFn& tf, double tol) + { + math::PolyCx num(tf.num.coeffs.n_elem); + math::PolyCx den(tf.den.coeffs.n_elem); + // TODO: return new tf without poles & zeros that are closer to each other than tol + } TransferFn feedback(const TransferFn& tf, complex k) { @@ -88,8 +94,9 @@ namespace ct const TransferFn ol = k * tf; const TransferFn bo = unit - ol; const TransferFn fb = unit / bo; - const TransferFn tf_cl = ol * fb; + TransferFn tf_cl = ol * fb; // const TransferFn tf_cl = (k * tf) / (unit - k * tf); + tf_cl.canonicalize(); return tf_cl; } @@ -103,12 +110,21 @@ namespace ct } } + TransferFn& TransferFn::operator = (const TransferFn& other) + { + if (this == &other) + return *this; + + num = math::PolyCx(other.num); + den = math::PolyCx(other.den); + return *this; + } + 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) {} @@ -124,9 +140,27 @@ namespace ct // compute roots for (int i = 0; i < ls.n_samples; i++) + // FIXME: sort the roots ls.out.col(i) = (tf.den + ls.in(i) * tf.num).roots(); + } + + void nyquist(const TransferFn& tf, LocusSeries& ls) + { + using namespace arma; + complex j(0., 1.); - // sort the roots + // FIXME missing the countour at infinity and the small contours for poles + // on the imaginary axis + + // contour from 0 to i\infty + cx_mat num = polyval(tf.num.coeffs, j * conv_to::from(ls.in)); + cx_mat den = polyval(tf.den.coeffs, j * conv_to::from(ls.in)); + + if (ls.out.n_rows != ls.n_samples) + ls.out = cx_mat(1, ls.n_samples); + + for (int i = 0; i < ls.n_samples; i++) + ls.out(i) = num(i) / den(i); } SSModel::SSModel(size_t n_in, size_t n_out, size_t n_states) @@ -181,14 +215,22 @@ namespace ct // 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; - + // accurate. Hence we use a simulation with ZOH input since it is cheap and + // probably good enough. + // FIXME: do not invert matrix like that, but I am too lazy to implement + // regularization + const int n_steps = 3; + const cx_mat Ad = expmat(ss.A * ts.dt / n_steps); + const cx_mat Bd = (Ad - eye(size(Ad))) * inv(ss.A) * ss.B; + + cx_vec x(ss.n_states), xn; 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); + { + x = ts.state.col(k); + for (int l = 0; l < n_steps; l++) + xn = Ad * x + Bd * ts.in.col(k); + ts.state.col(k + 1) = xn; + } ts.out = ss.C * ts.state + ss.D * ts.in; } diff --git a/src/control.h b/src/control.h index 587b80e..61dcd0f 100644 --- a/src/control.h +++ b/src/control.h @@ -13,7 +13,8 @@ #define CT_ASSERT(x) (assert(x)) #define CT_SMALL_TOL 1e-6 -#define CT_ASSERT_SMALL(x) (assert(std::abs(x) < CT_SMALL_TOL)) +#define CT_SMALL(x) (std::abs(x) < CT_SMALL_TOL) +#define CT_ASSERT_SMALL(x) (assert(CT_SMALL)) namespace ct { @@ -98,12 +99,14 @@ namespace ct struct TransferFn { - complex gain; math::PolyCx num; math::PolyCx den; TransferFn(void); TransferFn(math::PolyCx num, math::PolyCx den); + TransferFn(const TransferFn& other) + : num(other.num) + , den(other.den) {} inline void add_pole(complex p) { den.add_root(p); } inline void add_zero(complex z) { num.add_root(z); }; @@ -114,6 +117,8 @@ namespace ct bool is_strictly_proper() const; void canonicalize(); + + TransferFn& operator = (const TransferFn& other); }; TransferFn operator + (const TransferFn& g, const TransferFn& h); @@ -125,6 +130,7 @@ namespace ct // inline TransferFn operator / (const TransferFn& + TransferFn cancel_zp(const TransferFn& tf, double tol = CT_SMALL_TOL); TransferFn feedback(const TransferFn& tf, complex k = -1); struct LocusSeries @@ -139,7 +145,7 @@ namespace ct }; void rlocus(const TransferFn& tf, LocusSeries& ls); - + void nyquist(const TransferFn& tf, LocusSeries& ls); struct SSModel { diff --git a/src/main.cpp b/src/main.cpp index f169982..e1f6671 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -37,12 +37,18 @@ struct GState float gain_re; float gain_im; bool autogain; - float time_start; - float time_len; - float bode_start; - float bode_len; - float rlocus_start; - float rlocus_len; + struct { + float time; + float bode; + float nyquist; + float rlocus; + } start; + struct { + float time; + float bode; + float nyquist; + float rlocus; + } len; size_t npoles, nzeros; ImPlotPoint poles[MAX_ZP], zeros[MAX_ZP]; }; @@ -51,7 +57,9 @@ struct CState { bool proper; ct::TimeSeries time; + ct::LocusSeries nyquist; ct::LocusSeries rlocus; + ct::TransferFn tf_cl; }; @@ -73,11 +81,38 @@ ImPlotPoint step_getter(int idx, void *data) return ImPlotPoint(ts->time(idx), idx != 0); } +// Global variable because std::function is trash +int rlocus_curr_row = 0; ImPlotPoint rlocus_getter(int idx, void *data) { - // return ImPlotPoint(ls->out(row, idx).real(), ls->out(row, idx).imag()); + ct::LocusSeries *ls = (ct::LocusSeries *) data; + return ImPlotPoint(ls->out(rlocus_curr_row, idx).real(), + ls->out(rlocus_curr_row, idx).imag()); } +ImPlotPoint nyquist_getter(int idx, void *data) +{ + ct::LocusSeries *ls = (ct::LocusSeries *) data; + return ImPlotPoint(ls->out(0, idx).real(), ls->out(0, idx).imag()); +} + +ImPlotPoint nyquist_getter_conj(int idx, void *data) +{ + ct::LocusSeries *ls = (ct::LocusSeries *) data; + return ImPlotPoint(ls->out(0, idx).real(), -ls->out(0, idx).imag()); +} + +ImPlotPoint bode_getter_ampl(int idx, void *data) +{ + ct::LocusSeries *ls = (ct::LocusSeries *) data; + return ImPlotPoint(ls->in(idx), std::abs(ls->out(0, idx))); +} + +ImPlotPoint bode_getter_phase(int idx, void *data) +{ + ct::LocusSeries *ls = (ct::LocusSeries *) data; + return ImPlotPoint(ls->in(idx), std::arg(ls->out(0, idx))); +} static void glfw_error_callback(int error, const char* description) { @@ -152,12 +187,18 @@ int main(int, char**) .gain_re = 1., .gain_im = 0., .autogain = false, - .time_start = 0, - .time_len = 10, - .bode_start = 0.01, - .bode_len = 10000, - .rlocus_start = 0.001, - .rlocus_len = 1000, + .start = { + .time = 0.0, + .bode = 0.001, + .nyquist = 0.001, + .rlocus = 0.001, + }, + .len = { + .time = 10., + .bode = 1000., + .nyquist = 1000., + .rlocus = 1000., + }, .npoles = 0, .nzeros = 0, }; @@ -167,10 +208,12 @@ int main(int, char**) // Computation state CState cstate = { .proper = false, - .time = ct::TimeSeries(gstate.time_start, - gstate.time_start + gstate.time_len, gstate.n_samples.time), - .rlocus = ct::LocusSeries(gstate.rlocus_start, - gstate.rlocus_start + gstate.rlocus_len, gstate.n_samples.rlocus), + .time = ct::TimeSeries(gstate.start.time, + gstate.start.time + gstate.len.time, gstate.n_samples.time), + .nyquist = ct::LocusSeries(gstate.start.nyquist, + gstate.start.nyquist + gstate.len.nyquist, gstate.n_samples.nyquist), + .rlocus = ct::LocusSeries(gstate.start.rlocus, + gstate.start.rlocus + gstate.len.rlocus, gstate.n_samples.rlocus), }; // Main loop @@ -204,38 +247,41 @@ int main(int, char**) 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); + + ct::complex gain(gstate.gain_re, gstate.gain_im); // Autogain if (gstate.autogain) { - ct::complex gain = 1. / tf_cl.dc_gain(); - tf_cl = gain * tf_cl; + gain = 1. / std::abs(tf.dc_gain()); 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; - } + + // tf = ct::cancel_zp(tf); + ct::TransferFn tf_cl = ct::feedback(tf, -gain); + cstate.tf_cl = tf_cl; if (tf_cl.is_proper()) { - // Convert to state space - ct::SSModel ss = ct::ctrb_form(ct::feedback(tf)); - // New time series - cstate.time = ct::TimeSeries(gstate.time_start, - gstate.time_start + gstate.time_len, gstate.n_samples.time); + cstate.time = ct::TimeSeries(gstate.start.time, + gstate.start.time + gstate.len.time, gstate.n_samples.time); - // Update time domain simulation - ct::step(ss, cstate.time); + // New root nyquist series + cstate.nyquist = ct::LocusSeries(gstate.start.nyquist, + gstate.start.nyquist + gstate.len.nyquist, gstate.n_samples.nyquist); // New root locus series - cstate.rlocus = ct::LocusSeries(gstate.rlocus_start, - gstate.rlocus_start + gstate.rlocus_len, gstate.n_samples.rlocus); + cstate.rlocus = ct::LocusSeries(gstate.start.rlocus, + gstate.start.rlocus + gstate.len.rlocus, gstate.n_samples.rlocus); + + // Convert to state space + ct::SSModel ss = ct::ctrb_form(ct::feedback(tf, -gain)); + // Update plots + ct::step(ss, cstate.time); ct::rlocus(tf, cstate.rlocus); + ct::nyquist(tf, cstate.nyquist); } } @@ -262,8 +308,8 @@ int main(int, char**) ImGui::SeparatorText("Plant G(s)"); 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::SliderFloat("Gain (real)", &gstate.gain_re, 0.0001, 10000, NULL, ImGuiSliderFlags_Logarithmic); + ImGui::SliderFloat("Gain (imag)", &gstate.gain_im, 0.0001, 10000, NULL, ImGuiSliderFlags_Logarithmic); ImGui::PushItemWidth(140); if (ImGui::BeginListBox("Poles")) @@ -301,12 +347,18 @@ int main(int, char**) ImGui::PopItemWidth(); ImGui::SeparatorText("Simulation Ranges"); - ImGui::InputFloat("Time Start", &gstate.time_start, 0, 100); - ImGui::SliderFloat("Time Length", &gstate.time_len, 0, 100, NULL, ImGuiSliderFlags_Logarithmic); - ImGui::InputFloat("Bode Start", &gstate.bode_start, 0, 100); - ImGui::SliderFloat("Bode Length", &gstate.bode_len, 0, 100, NULL, ImGuiSliderFlags_Logarithmic); - ImGui::InputFloat("Root Locus Start", &gstate.rlocus_start, 0, 100); - ImGui::SliderFloat("Root Locus Length", &gstate.rlocus_len, 0.001, 10000, NULL, ImGuiSliderFlags_Logarithmic); + + ImGui::InputFloat("Time Start", &gstate.start.time, 0, 100); + ImGui::SliderFloat("Time Length", &gstate.len.time, 0, 100, NULL, ImGuiSliderFlags_Logarithmic); + + ImGui::InputFloat("Bode Start", &gstate.start.bode, 0, 100); + ImGui::SliderFloat("Bode Length", &gstate.len.bode, 0, 100, NULL, ImGuiSliderFlags_Logarithmic); + + ImGui::InputFloat("Nyquist Start", &gstate.start.nyquist, 0, 100); + ImGui::SliderFloat("Nyquist Length", &gstate.len.nyquist, 0.001, 10000, NULL, ImGuiSliderFlags_Logarithmic); + + ImGui::InputFloat("Root Locus Start", &gstate.start.rlocus, 0, 100); + ImGui::SliderFloat("Root Locus Length", &gstate.len.rlocus, 0.001, 10000, NULL, ImGuiSliderFlags_Logarithmic); ImGui::SeparatorText("Number of Samples"); ImGui::SliderInt("Time", &gstate.n_samples.time, 50, 5000); @@ -322,18 +374,23 @@ int main(int, char**) { ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Log10); ImPlot::SetupAxisScale(ImAxis_Y1, ImPlotScale_Log10); - ImPlot::SetupAxesLimits(0.01, 10000, 0.001, 10); - ImPlot::SetupAxis(ImAxis_X1, "Amplitude |G(iw)| dB"); - ImPlot::SetupAxis(ImAxis_Y1, "Frequency w / (rad / s)"); + // ImPlot::SetupAxisLimits(ImAxis_X1, gstate.start.bode, + // gstate.start.bode + gstate.len.bode, ImPlotCond_Always); + ImPlot::SetupAxis(ImAxis_X1, "Amplitude |G(iw)| dB", ImPlotAxisFlags_AutoFit); + ImPlot::SetupAxis(ImAxis_Y1, "Frequency w / (rad / s)", ImPlotAxisFlags_AutoFit); + ImPlot::PlotLineG("|G(iw)|", bode_getter_ampl, &cstate.nyquist, cstate.nyquist.n_samples); ImPlot::EndPlot(); } if (ImPlot::BeginPlot("Bode Phase", ImVec2(-1, 0))) { - ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Linear); - ImPlot::SetupAxesLimits(0.1, 100, -180, 180); - ImPlot::SetupAxis(ImAxis_X1, "Phase arg G(iw) / deg"); - ImPlot::SetupAxis(ImAxis_Y1, "Frequency w / (rad / s)"); + ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Log10); + ImPlot::SetupAxisScale(ImAxis_Y1, ImPlotScale_Linear); + // ImPlot::SetupAxisLimits(ImAxis_X1, gstate.start.bode, + // gstate.start.bode + gstate.len.bode, ImPlotCond_Always); + ImPlot::SetupAxis(ImAxis_X1, "Phase arg G(iw) / deg", ImPlotAxisFlags_AutoFit); + ImPlot::SetupAxis(ImAxis_Y1, "Frequency w / (rad / s)", ImPlotAxisFlags_AutoFit); + ImPlot::PlotLineG("arg G(iw)", bode_getter_phase, &cstate.nyquist, cstate.nyquist.n_samples); ImPlot::EndPlot(); } } @@ -345,8 +402,8 @@ int main(int, char**) { ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Linear); ImPlot::SetupAxis(ImAxis_X1, "Time t / s"); - ImPlot::SetupAxisLimits(ImAxis_X1, gstate.time_start, - gstate.time_start + gstate.time_len, ImPlotCond_Always); + ImPlot::SetupAxisLimits(ImAxis_X1, gstate.start.time, + gstate.start.time + gstate.len.time, ImPlotCond_Always); ImPlot::SetupAxis(ImAxis_Y1, "Response y(t)", ImPlotAxisFlags_AutoFit); ImPlot::PlotLineG("Step", step_getter, &cstate.time, cstate.time.n_samples); @@ -360,8 +417,9 @@ int main(int, char**) if (ImGui::Begin("Stability")) { ImGui::Text("Use CTRL + R / L Click to add a pole / zero"); + ImGui::Text("Hold CTRL / SHIFT after dragging to keep it real / imaginary"); - int rlflags = ImPlotFlags_Equal | ImPlotFlags_NoBoxSelect; + int rlflags = ImPlotFlags_Equal | ImPlotFlags_NoBoxSelect | ImPlotFlags_NoLegend; if (ImGui::GetIO().KeyCtrl) rlflags |= ImPlotFlags_Crosshairs; @@ -373,7 +431,8 @@ int main(int, char**) 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); + rlocus_curr_row = i; // global variable! + ImPlot::PlotLineG("Root Locus", rlocus_getter, &cstate.rlocus, cstate.rlocus.n_samples); ImGui::PopID(); } @@ -404,16 +463,56 @@ int main(int, char**) // Plot zeros and poles const ImVec4 poleCol(0,0.9f,0,1); + const ImVec4 currPoleCol(0,0.5f,0,1); const ImVec4 zeroCol(1,0.5f,1,1); + const ImVec4 currZeroCol(1,0.2f,1,1); const float size = 4; const int flags = ImPlotDragToolFlags_Delayed; + auto zeros = cstate.tf_cl.num.roots(); + for (int i = 0; i < zeros.n_elem; i++) + { + const double x = zeros(i).real(); + const double y = zeros(i).imag(); + ImGui::PushID(i); + ImPlot::SetNextMarkerStyle(ImPlotMarker_Diamond, size, currZeroCol, 0); + ImPlot::PlotScatter("Zeros", &x, &y, 1); + ImGui::PopID(); + } + + auto roots = cstate.tf_cl.den.roots(); + for (int i = 0; i < roots.n_elem; i++) + { + const double x = roots(i).real(); + const double y = roots(i).imag(); + ImGui::PushID(i); + ImPlot::SetNextMarkerStyle(ImPlotMarker_Diamond, size, currPoleCol, 0); + ImPlot::PlotScatter("Roots", &x, &y, 1); + ImGui::PopID(); + } + for (int i = 0; i < gstate.nzeros; i++) - ImPlot::DragPoint(i, &gstate.zeros[i].x, &gstate.zeros[i].y, zeroCol, size, flags); + { + bool dragging = ImPlot::DragPoint(i, &gstate.zeros[i].x, &gstate.zeros[i].y, zeroCol, size, flags); + + if (dragging && ImGui::GetIO().KeyCtrl) + gstate.zeros[i].y = 0; + + if (dragging && ImGui::GetIO().KeyShift) + gstate.zeros[i].x = 0; + } for (int i = 0; i < gstate.npoles; i++) - ImPlot::DragPoint(gstate.nzeros + i, &gstate.poles[i].x, &gstate.poles[i].y, - poleCol, size, flags); + { + bool dragging = ImPlot::DragPoint(gstate.nzeros + i, &gstate.poles[i].x, + &gstate.poles[i].y, poleCol, size, flags); + + if (dragging && ImGui::GetIO().KeyCtrl) + gstate.poles[i].y = 0; + + if (dragging && ImGui::GetIO().KeyShift) + gstate.poles[i].x = 0; + } ImPlot::EndPlot(); } @@ -421,7 +520,9 @@ int main(int, char**) if (ImPlot::BeginPlot("Nyquist Diagram", ImVec2(-1, 0), ImPlotFlags_Equal)) { ImPlot::SetupAxesLimits(-1, 1, -1, 1); - ImPlot::SetNextMarkerStyle(ImPlotMarker_Cross); + // ImPlot::SetNextMarkerStyle(ImPlotMarker_Plus); + ImPlot::PlotLineG("Nyquist Curve", nyquist_getter, &cstate.nyquist, cstate.nyquist.n_samples); + ImPlot::PlotLineG("Nyquist Curve Conj", nyquist_getter_conj, &cstate.nyquist, cstate.nyquist.n_samples); ImPlot::EndPlot(); } } -- cgit v1.2.1