Make Nyquist and Bode workHEADmaster

1 files changed, 52 insertions, 10 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;
 	}