2 files changed, 237 insertions, 0 deletions
diff --git a/src/control.h b/src/control.h
new file mode 100644
index 0000000..8ae9c5d
--- /dev/null
+++ b/src/control.h
@@ -0,0 +1,133 @@
+#pragma once
+
+#include <armadillo>
+#include <complex>
+#include <cassert>
+#include <cmath>
+
+#ifndef CONTROL_H
+#define CONTROL_H
+
+#define CT_ASSERT(x) (assert(x))
+#define CT_SMALL_TOL 1e-6
+#define CT_ASSERT_SMALL(x) (assert(std::abs(x) < CT_SMALL_TOL))
+
+namespace ct
+{
+	using complex = std::complex<double>;
+
+	namespace math
+	{
+		template<typename T>
+		struct Poly
+		{
+			arma::Col<T> coeffs;
+
+			Poly(size_t n_elem) : coeffs(n_elem) {};
+			Poly(arma::Col<T> c) : coeffs(c) {}
+
+			int degree() const { return coeffs.n_elem - 1; }
+			void add_root(T r) { (*this) = (*this) * Poly({1, -r}); }
+			arma::Col<T> roots() const { return arma::roots(coeffs); }
+
+			template<typename IndexT>
+			T& operator () (IndexT idx) { return coeffs(idx); };
+
+			template<typename IndexT>
+			T operator () (IndexT idx) const { return coeffs(idx); };
+		};
+		
+		template<typename T>
+		Poly<T> operator * (const Poly<T>& p, const Poly<T>& q)
+		{
+			arma::Col<T> coeffs = arma::conv(p.coeffs, q.coeffs);
+			return Poly<T>(coeffs);
+		}
+
+		template<typename T, typename R = T>
+		Poly<T> operator * (R scalar, const Poly<T>& p)
+		{
+			Poly<T> q(p.coeffs * scalar);
+			return q;
+		}
+
+		template<typename T>
+		Poly<T> operator + (const Poly<T>& p, const Poly<T>& q)
+		{
+			const Poly<T>& big = (p.degree() > q.degree()) ? p : q;
+			const Poly<T>& small = (p.degree() > q.degree()) ? q : p;
+
+			Poly<T> s(big.degree() + 1);
+
+			for (int i = 0; i <= small.degree(); i++)
+				s(i) = big(i) + small(i);
+
+			for (int i = small.degree() + 1; i <= big.degree(); i++)
+				s(i) = big(i);
+
+			return s;
+		}
+	}
+
+	struct TransferFn
+	{
+		complex gain;
+		math::Poly<complex> num;
+		math::Poly<complex> den;
+
+		TransferFn(complex gain);
+
+		inline void add_pole(complex p) { den.add_root(p); }
+		inline void add_zero(complex z) { num.add_root(z); };
+
+		bool is_proper() const;
+		bool is_strictly_proper() const;
+	};
+
+	TransferFn feedback(const TransferFn& tf, double k = -1);
+
+	struct LocusSeries
+	{
+		size_t n_samples;
+		double start, end;
+		arma::vec in;
+		arma::cx_mat out;
+
+		LocusSeries() = delete;
+		LocusSeries(double start, double end, size_t n_samples);
+	};
+
+	void rlocus(const TransferFn& tf, LocusSeries& ls);
+
+
+	struct SSModel
+	{
+		size_t n_in, n_out, n_states; 
+		arma::cx_mat A, B, C, D;
+
+		SSModel() = delete;
+		SSModel(size_t n_in, size_t n_out, size_t n_states);
+	};
+
+	SSModel ctrb_form(const TransferFn& tf);
+	// SSModel obsv_form(const TransferFn& tf);
+	// SSModel eigm_form(const TransferFn& tf);
+
+	struct TimeSeries
+	{
+		size_t n_samples;
+		double start, end, dt;
+		arma::vec time;
+		arma::cx_mat in, out;
+		arma::cx_mat state;
+
+		TimeSeries() = delete;
+		TimeSeries(double start, double end, size_t n_samples);
+	};
+
+	void response(const SSModel& ss, TimeSeries& ts);
+	void step(const SSModel& ss, TimeSeries& ts);
+}
+
+#endif // CONTROL_H
+// vim:ts=2 sw=2 noet:
diff --git a/src/control.h.old b/src/control.h.old
new file mode 100644
index 0000000..3a147fc
--- /dev/null
+++ b/src/control.h.old
@@ -0,0 +1,104 @@
+#pragma once
+
+#include "Eigen/Dense"
+#include <complex>
+
+#ifndef CONTROL_H
+#define CONTROL_H
+
+
+namespace ct
+{
+	// Fwd declarations
+	template<typename T, int n, int m, int k> struct SSModel;
+	template<typename T, int p, int z> struct TransferFn;
+
+	namespace math
+	{
+		template<typename T, int n, int m>
+		Eigen::Vector<T, n + m - 1> convolve1d(
+				const Eigen::Ref<Eigen::Vector<T, n>>& x
+				const Eigen::Ref<Eigen::Vector<T, m>>& y)
+		{
+			Eigen::Vector<T, n + m -1> out;
+
+			return out;
+		}
+
+		Eigen::VectorXd convolve1d(
+				const Eigen::Ref<Eigen::VectorXd>& x,
+				const Eigen::Ref<Eigen::VectorXd>& y)
+		{
+			
+		}
+	}
+
+	/* Transfer Functions for SISO and frequency domain analysis */
+	template<typename T, int p, int z>
+	struct TransferFn
+	{
+		Eigen::Vector<std::complex<T>, z> zeros;
+		Eigen::Vector<std::complex<T>, p> poles;
+
+		explicit operator SSModel<T, p, 1, 1>() const
+		{
+			
+		}
+	};
+
+	typedef TransferFn<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> TransferFnXd;
+
+	template<typename T, typename CT = std::complex<T>>
+	struct FreqSeries
+	{
+		size_t nsamples;
+		T start, end;
+		Eigen::VectorX<T> f;
+		Eigen::VectorX<CT> data;
+	};
+
+	typedef FreqSeries<double> FreqSeriesD;
+
+	/* State space modelling and time domain simulaiton */
+
+	template<typename T, int n, int m, int k>
+	struct SSModel
+	{
+		/* State space model with
+		 * - n dimensional state x
+		 * - m dimensional input u
+		 * - k dimensional output y
+		 */
+		Eigen::Matrix<T, n, n> A;
+		Eigen::Matrix<T, n, m> B;
+		Eigen::Matrix<T, k, m> C;
+		Eigen::Matrix<T, k, m> D;
+
+		// SSModel(Eigen::VectorX<T> zeros, Eigen::VectorX<T> poles);
+	};
+
+	typedef SSModel<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> SSModelXd;
+
+	template<typename T>
+	struct TimeSeries
+	{
+		size_t nsamples;
+		T start, end;
+		Eigen::VectorX<T> t;
+		Eigen::MatrixX<T> u, x, y;
+
+		template<int n, int m, int k>
+		explicit TimeSeries(size_t nsamples, T start, T end, const SSModel<T, n, m, k>& ss);
+	};
+
+	typedef TimeSeries<double> TimeSeriesD;
+
+	template<typename T, int n, int m, int k>
+	void response(SSModel<T, n, m, k> ss, TimeSeries<T>& ts);
+
+	template<typename T, int n, int m, int k>
+	void step(SSModel<T, n, m, k> ss, TimeSeries<T>& ts);
+}
+
+#endif // CONTROL_H
+// vim:ts=2 sw=2 noet: