Balance the matrix for eigenvalue decomposition (cbae7276) · Commits · Daingun / automatica

automatica/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -9,6 +9,7 @@ examples = arma_channel \
		oscillation \
		polar \
		root_locus \
		springs \
		suspension \
		transfer_function

automatica/examples/springs.rs

0 → 100644

+72 −0

Original line number	Diff line number	Diff line
		use automatica::Ss;

		#[allow(clippy::many_single_char_names)]
		fn main() {
		// Three bodies with equal mass joined by four equal springs fixed at each side
		// \|
		// \|--/\/\/\/--o--/\/\/\/--o--/\/\/\/--o--/\/\/\/--\|
		// \| k m k m k m k
		//
		// Mass (m), spring (k)
		// external force (u)
		// position (x1, x2, x3), speed (x4, x5, x6)
		// position (y1=x1, y2=x2, y3=x3)
		let m = 1.;
		let k = 1.;
		let c = k / m;

		let a: Vec<f64> = [
		[0., 0., 0., 1., 0., 0.], // xdot1
		[0., 0., 0., 0., 1., 0.], // xdot2
		[0., 0., 0., 0., 0., 1.], // xdot3
		[-2. * c, 1. * c, 0., 0., 0., 0.], // xdot4
		[1. * c, -2. * c, 1. * c, 0., 0., 0.], // xdot5
		[0., 1. * c, -2. * c, 0., 0., 0.], // xdot6
		]
		.into_iter()
		.flatten()
		.collect();
		let b = [0., 0., 0., 0., 0., 0.];
		let c = [
		1., 0., 0., 0., 0., 0., // y1.
		0., 1., 0., 0., 0., 0., // y2
		0., 0., 1., 0., 0., 0., // y3
		];
		let d = [0., 0., 0.];

		let sys = Ss::new_from_slice(6, 1, 3, &a, &b, &c, &d);
		println!("{}", &sys);

		let poles = sys.poles();
		println!("\nPoles:");
		println!("\t{:.2}, {:.2}", poles[0], poles[1]);
		println!("\t{:.2}, {:.2}", poles[2], poles[3]);
		println!("\t{:.2}, {:.2}", poles[4], poles[5]);

		let v = sys.eigenvectors();
		println!("\nEigenvectors:");
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][0], v[1][0], v[2][0], v[3][0], v[4][0], v[5][0],
		);
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][1], v[1][1], v[2][1], v[3][1], v[4][1], v[5][1],
		);
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][2], v[1][2], v[2][2], v[3][2], v[4][2], v[5][2],
		);
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][3], v[1][3], v[2][3], v[3][3], v[4][3], v[5][3],
		);
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][4], v[1][4], v[2][4], v[3][4], v[4][4], v[5][4],
		);
		println!(
		"\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t{:.3}\t",
		v[0][5], v[1][5], v[2][5], v[3][5], v[4][5], v[5][5],
		);
		}

automatica/src/eigen.rs

+218 −6

Original line number	Diff line number	Diff line
		@@ -31,6 +31,12 @@ use crate::{Abs, Epsilon, Hypot, Max, One, Pow, Sqrt, Zero};

		use ndarray::{Array1, Array2};

		#[derive(Debug, Clone)]
		enum Scale<T> {
		Factor(T),
		Index(usize),
		}

		/// Eigenvalue decomposition structure.
		#[derive(Debug, Clone)]
		pub(crate) struct EigenvalueDecomposition<T> {
		@@ -48,6 +54,10 @@ pub(crate) struct EigenvalueDecomposition<T> {
		H: Array2<T>,
		/// Working storage for nonsymmetric algorithm.
		ort: Array1<T>,
		/// Scaling factors for balanc algorithm.
		scale: Array1<Scale<T>>,
		low: usize,
		high: usize,
		}

		impl<T> EigenvalueDecomposition<T>
		@@ -70,6 +80,176 @@ where
		+ Sqrt
		+ Zero,
		{
		/// Balance the elements of the matrix
		///
		/// This is derived from the Algol procedures balanc,
		/// Fortran subroutines in EISPACK.
		/// Radix is chosen as in https://github.com/scilab/scilab/blob/master/scilab/modules/elementary_functions/src/fortran/slatec/balanc.f
		fn balanc(&mut self) {
		let radix = T::one() + T::one();
		let b2 = radix * radix;
		let mut k: usize = 0;
		let mut l: usize = self.n;

		let mut sub_inline = \|j_int, m_int, l_int, k_int, H: &mut Array2<T>\| {
		self.scale[m_int] = Scale::Index(j_int);
		if j_int == m_int {
		return;
		}
		for i in 0..l_int {
		// let tmp = H[(i, j_int)];
		// H[(i, j_int)] = H[(i, m_int)];
		// H[(i, m_int)] = tmp;
		H.swap((i, j_int), (i, m_int));
		}
		for i in k_int..self.n {
		// let tmp = H[(j_int, i)];
		// j_int, i)] = H[(m_int, i)];
		// H[(m_int, i)] = tmp;
		H.swap((j_int, i), (m_int, i));
		}
		};

		'outer100: for jj in 0..l {
		let j = l - 1 - jj;
		for i in 0..l {
		if i == j {
		continue;
		};
		if !self.H[(j, i)].is_zero() {
		continue 'outer100;
		}
		}
		let m = l.saturating_sub(1);
		// go to 20
		sub_inline(j, m, l, k, &mut self.H);

		if l == 1 {
		self.low = k;
		self.high = l;
		return;
		}

		l = l - 1;
		}

		'outer140: for j in k..l {
		for i in k..l {
		if i == j {
		continue;
		};
		if !self.H[(i, j)].is_zero() {
		continue 'outer140;
		}
		}
		let m = k;
		// go to 20
		sub_inline(j, m, l, k, &mut self.H);

		k = k + 1;
		}

		for i in k..l {
		self.scale[i] = Scale::Factor(T::one());
		}

		let mut noconv = true;
		while noconv {
		noconv = false;
		for i in k..l {
		let mut c = T::zero();
		let mut r = T::zero();

		for j2 in k..l {
		if j2 == i {
		continue;
		}
		c = c + self.H[(j2, i)].abs();
		r = r + self.H[(i, j2)].abs();
		}

		if c.is_zero() \|\| r.is_zero() {
		continue;
		}

		let mut g = r / radix;
		let mut f = T::one();
		let s = c + r;
		while c < g {
		f = f * radix;
		c = c * b2;
		}
		g = r * radix;
		while c >= g {
		f = f / radix;
		c = c / b2;
		}

		if (c + r) / f >= T::_095() * s {
		continue;
		}

		g = T::one() / f;
		if let Scale::Factor(old) = self.scale[i] {
		self.scale[i] = Scale::Factor(old * f);
		}
		noconv = true;

		for j in k..self.n {
		self.H[(i, j)] = self.H[(i, j)] * g;
		}
		for j in 0..l {
		self.H[(j, i)] = self.H[(j, i)] * f;
		}
		}
		}

		self.low = k;
		self.high = l;
		return;
		}

		// This subroutine forms the eigenvectors of a real general
		// matrix by back transforming those of the corresponding
		// balanced matrix determined by balanc.
		fn balbak(&mut self) {
		let low = self.low;
		let high = self.high;
		let m = self.n;
		if m == 0 {
		return;
		}

		for i in low..high {
		if let Scale::Factor(s) = self.scale[i] {
		for j in 0..m {
		self.V[(i, j)] = self.V[(i, j)] * s;
		}
		}
		}

		for ii in 0..self.n {
		let mut i = ii;
		if i >= low && i <= high {
		continue;
		}
		if i < low {
		i = low - ii;
		}
		if let Scale::Index(k) = self.scale[i] {
		if k == i {
		continue;
		}
		for j in 0..m {
		// let s = self.V[(i, j)];
		// self.V[(i, j)] = self.V[(k, j)];
		// self.V[(k, j)] = s;
		self.V.swap((i, j), (k, j));
		}
		}
		}
		}

		/// Symmetric Householder reduction to tridiagonal form.
		///
		/// This is derived from the Algol procedures tred2 by
		@@ -300,10 +480,10 @@ where
		/// Vol.ii-Linear Algebra, and the corresponding
		/// Fortran subroutines in EISPACK.
		fn orthes(&mut self) {
		let low = 0;
		let high = self.n - 1;
		let low = self.low;
		let high = self.high - 1;

		for m in (low + 1)..=(high - 1) {
		for m in (low + 1)..high {
		// Scale column.
		let mut scale = T::zero();
		for i in m..=high {
		@@ -358,7 +538,7 @@ where
		}
		}

		for m in ((low + 1)..=(high - 1)).rev() {
		for m in ((low + 1)..high).rev() {
		if self.H[(m, m - 1)] != T::zero() {
		for i in (m + 1)..=high {
		self.ort[i] = self.H[(i, m - 1)];
		@@ -873,6 +1053,9 @@ where
		V: Array2::from_elem((n, n), T::zero()),
		H: Array2::<T>::from_elem((1, 1), T::zero()),
		ort: Array1::<T>::from_elem(0, T::zero()),
		scale: Array1::<Scale<T>>::from_elem(0, Scale::Index(0)),
		low: 0,
		high: n,
		};

		'outer: for j in 0..n {
		@@ -894,13 +1077,20 @@ where
		evd.tql2();
		} else {
		evd.ort = Array1::<T>::from_elem(n, T::zero());
		evd.scale = Array1::<Scale<T>>::from_elem(n, Scale::Index(0));
		evd.H = A;

		// Balance
		evd.balanc();

		// Reduce to Hessenberg form.
		evd.orthes();

		// Reduce Hessenberg to real Schur form.
		evd.hqr2();

		// Back transforming eigenvectors transformed by balanc.
		evd.balbak();
		}
		evd
		}
		@@ -953,6 +1143,8 @@ pub trait EigenConst {
		fn _m04375() -> Self;
		/// 0.964 constant
		fn _0964() -> Self;
		/// 0.95 constant
		fn _095() -> Self;
		}

		macro_rules! impl_eigen_const {
		@@ -967,6 +1159,9 @@ macro_rules! impl_eigen_const {
		fn _0964() -> Self {
		0.964
		}
		fn _095() -> Self {
		0.95
		}
		}
		};
		}
		@@ -1010,6 +1205,17 @@ mod tests {
		assert!(check_eq(&A.mmul(&V), &V.mmul(&D)));
		}

		#[test]
		fn non_symmetric_2x2_matrix() {
		let A = arr2(&[[-2., 0.], [3., -7.]]);
		let eig = EigenvalueDecomposition::new(&A);
		let D = eig.get_D();
		let V = eig.get_V();
		assert!(check_eq(&A.mmul(&V), &V.mmul(&D)));
		assert_eq!(D, arr2(&[[-7., 0.], [0., -2.]]));
		assert_eq!(V, arr2(&[[0., 1.], [1., 0.6]]));
		}

		#[test]
		fn bad_eigenvalues() {
		let bA = arr2(&[
		@@ -1037,10 +1243,16 @@ mod tests {
		let d = eig.get_real_eigenvalues();
		let e = eig.get_imag_eigenvalues();
		println!("{0:.5}", d);
		assert!(d.iter().all(\|re\| relative_eq!(*re, 0.)));
		println!("{0:.5}", e);
		for i in e.as_slice().unwrap().chunks(2) {
		assert_eq!(i[0], -i[1]);
		}
		let V = eig.get_V();
		println!("{0:.5}", V);
		assert!(true); // Move this test as an example.
		assert_relative_eq!(-2.0.sqrt(), V[(1, 0)] / V[(0, 0)], epsilon = 1.0e-15);
		assert_relative_eq!(-2.0.sqrt(), V[(1, 0)] / V[(2, 0)], epsilon = 1.0e-15);
		assert_relative_eq!(1., V[(0, 0)] / V[(0, 0)]);
		}

		#[test]
		@@ -1079,6 +1291,6 @@ mod tests {
		let eig = EigenvalueDecomposition::new(&A);
		let lambda = eig.get_eigenvalues();
		println!("{0:.5?}", lambda);
		assert_eq!(lambda[0], (0., 0.));
		assert_eq!(lambda[0], (-380.14774, 0.));
		}
		}

automatica/src/linear_system/mod.rs

+39 −44

Original line number	Diff line number	Diff line
		@@ -162,48 +162,6 @@ where
		}
		}

		impl<U> SsGen<f64, U>
		where
		U: Time,
		{
		/// Calculate the poles of the system
		///
		/// # Example
		///
		/// ```
		/// use automatica::Ss;
		/// let sys = Ss::<f64>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
		/// let poles = sys.poles();
		/// assert_eq!(-2., poles[0].re);
		/// assert_eq!(-7., poles[1].re);
		/// ```
		#[must_use]
		pub fn poles(&self) -> Vec<Complex<f64>> {
		self.poles_impl()
		}
		}

		impl<U> SsGen<f32, U>
		where
		U: Time,
		{
		/// Calculate the poles of the system
		///
		/// # Example
		///
		/// ```
		/// use automatica::Ss;
		/// let sys = Ss::<f32>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
		/// let poles = sys.poles();
		/// assert_eq!(-2., poles[0].re);
		/// assert_eq!(-7., poles[1].re);
		/// ```
		#[must_use]
		pub fn poles(&self) -> Vec<Complex<f32>> {
		self.poles_impl()
		}
		}

		impl<T, U> SsGen<T, U>
		where
		T: Abs
		@@ -227,10 +185,21 @@ where
		+ Zero,
		U: Time,
		{
		/// Calculate the poles of the system
		///
		/// # Example
		///
		/// ```
		/// use automatica::Ss;
		/// let sys = Ss::<f32>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
		/// let poles = sys.poles();
		/// assert_eq!(-2., poles[0].re);
		/// assert_eq!(-7., poles[1].re);
		/// ```
		#[must_use]
		fn poles_impl(&self) -> Vec<Complex<T>> {
		pub fn poles(&self) -> Vec<Complex<T>> {
		match self.dim().states() {
		1 => vec![Complex::new(self.a[(0, 0)], <T as Zero>::zero())],
		1 => vec![Complex::new(self.a[(0, 0)], T::zero())],
		2 => {
		let m00 = self.a[(0, 0)];
		let m01 = self.a[(0, 1)];
		@@ -255,6 +224,32 @@ where
		}
		}
		}

		/// Calculate the eigenvectors of the system as a result of
		/// the Schur decomposition.
		///
		/// # Example
		///
		/// ```
		/// use automatica::Ss;
		/// let sys = Ss::<f32>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
		/// let v = sys.eigenvectors();
		/// assert_eq!(0., v[0][0]);
		/// assert_eq!(1., v[0][1]);
		/// ```
		#[must_use]
		pub fn eigenvectors(&self) -> Vec<Vec<T>> {
		match self.dim().states() {
		1 => vec![vec![T::one()]],
		_ => {
		let evd = EigenvalueDecomposition::new(&self.a);
		evd.get_V()
		.axis_iter(ndarray::Axis(1))
		.map(\|x\| x.to_vec())
		.collect()
		}
		}
		}
		}

		/// Controllability matrix implementation.

automatica/tests/plots.rs

+7 −2

Original line number	Diff line number	Diff line
		use approx::{assert_relative_eq, relative_eq};
		use approx::assert_relative_eq;
		use polynomen::Poly;

		use automatica::{
		@@ -47,6 +47,7 @@ fn polar_plot() {
		#[test]
		fn root_locus_plot() {
		// Example 13.2.
		// Scilab: H = syslin('c',1/poly([0,-3,-5],'s')); kpure(H); evans(H);
		let tf = Tf::new([1.0_f64], Poly::new_from_roots(&[0., -3., -5.]));

		let loci = tf.root_locus_plot(1., 130., 1.);
		@@ -56,10 +57,14 @@ fn root_locus_plot() {
		assert!(out[0].re < 0.);
		assert!(out[1].re < 0.);
		assert!(out[2].re < 0.);
		} else if locus.k() == 120. {
		assert_relative_eq!(out[0].re, 0., epsilon = 1.0e-15);
		assert_relative_eq!(out[1].re, 0., epsilon = 1.0e-15);
		assert_relative_eq!(out[2].re, -8.);
		} else {
		assert!(out[0].re > 0.);
		assert!(out[1].re > 0.);
		assert!(relative_eq!(out[2].re, -8.) \|\| out[2].re <= -8.);
		assert!(out[2].re <= -8.);
		}
		// Test symmetry
		assert_relative_eq!(out[0].im.abs(), out[1].im.abs());