summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/control.cpp62
-rw-r--r--src/control.h12
-rw-r--r--src/main.cpp215
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<cx_vec>::from(ls.in));
+ cx_mat den = polyval(tf.den.coeffs, j * conv_to<cx_vec>::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();
}
}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-21 13:04:58 +0100