automatica 1.0.0

//! # Continuous time linear system
//!
//! The time evolution of the system is performed through ODE (ordinary
//! differential equation) [solvers](../solver/index.html).

use std::ops::{Add, AddAssign, Div, Mul, Neg, Sub};

use crate::{
    enums::Continuous,
    linear_system::{
        solver::{Order, Radau, RadauConst, Rk, RkConst, Rkf45, Rkf45Const},
        Equilibrium, SsGen,
    },
    matrix::{lup, MatrixMul},
    units::Seconds,
    Abs, Max, NumCast, One, Pow, RelativeEq, Zero,
};

/// State-space representation of continuous time linear system
pub type Ss<T> = SsGen<T, Continuous>;

impl<T> Ss<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Div<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Sub<Output = T>
        + Zero,
{
    /// Calculate the equilibrium point for continuous time systems,
    /// given the input condition
    /// ```text
    /// x = - A^-1 * B * u
    /// y = - (C * A^-1 * B + D) * u
    /// ```
    ///
    /// # Arguments
    ///
    /// * `u` - Input vector
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ss;
    /// let a = [-1.0, 1., -1., 0.25];
    /// let b = [1., 0.25];
    /// let c = [0., 1., -1., 1.];
    /// let d = [0., 1.];
    ///
    /// let sys = Ss::new_from_slice(2, 1, 2, &a, &b, &c, &d);
    /// let u = 0.0;
    /// let eq = sys.equilibrium(&[u]).unwrap();
    /// assert_eq!((0., 0.), (eq.x()[0], eq.y()[0]));
    /// ```
    #[must_use]
    pub fn equilibrium(&self, u: &[T]) -> Option<Equilibrium<T>> {
        if u.len() != self.dim.inputs() {
            return None;
        }
        // 0 = A*x + B*u
        let bu = -self.b.mmul(u);
        let lu = lup(self.a.clone());
        // A*x = -B*u
        let x = lu?.solve(&bu);
        // y = C*x + D*u
        let y = self.c.mmul(&x) + self.d.mmul(u);
        Some(Equilibrium::new(
            x.iter().cloned().collect(),
            y.iter().cloned().collect(),
        ))
    }
}

/// Implementation of the methods for the state-space
impl Ss<f64> {
    /// System stability. Checks if all A matrix eigenvalues (poles) are negative.
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ss;
    /// let sys = Ss::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
    /// assert!(sys.is_stable());
    /// ```
    #[must_use]
    pub fn is_stable(&self) -> bool {
        self.poles().iter().all(|p| p.re.is_sign_negative())
    }
}

impl<T> Ss<T>
where
    T: Abs
        + Add<Output = T>
        + AddAssign
        + Clone
        + Div<Output = T>
        + Max
        + Mul<Output = T>
        + NumCast
        + PartialOrd
        + Pow<T>
        + RkConst
        + Rkf45Const
        + Sub<Output = T>
        + Zero,
{
    /// Time evolution for the given input, using Runge-Kutta second order method
    ///
    /// # Arguments
    ///
    /// * `u` - input function returning a vector (column mayor)
    /// * `x0` - initial state (column mayor)
    /// * `h` - integration time interval
    /// * `n` - integration steps
    pub fn rk2<F>(&self, u: F, x0: &[T], h: Seconds<T>, n: usize) -> Rk<T, F>
    where
        F: Fn(Seconds<T>) -> Vec<T>,
    {
        Rk::new(self, u, x0, h, n, Order::Rk2)
    }

    /// Time evolution for the given input, using Runge-Kutta fourth order method
    ///
    /// # Arguments
    ///
    /// * `u` - input function returning a vector (column mayor)
    /// * `x0` - initial state (column mayor)
    /// * `h` - integration time interval
    /// * `n` - integration steps
    pub fn rk4<F>(&self, u: F, x0: &[T], h: Seconds<T>, n: usize) -> Rk<T, F>
    where
        F: Fn(Seconds<T>) -> Vec<T>,
    {
        Rk::new(self, u, x0, h, n, Order::Rk4)
    }

    /// Runge-Kutta-Fehlberg 45 with adaptive step for time evolution.
    ///
    /// # Arguments
    ///
    /// * `u` - input function returning a vector (column vector)
    /// * `x0` - initial state (column vector)
    /// * `h` - integration time interval
    /// * `limit` - time evaluation limit
    /// * `tol` - error tolerance
    pub fn rkf45<F>(&self, u: F, x0: &[T], h: Seconds<T>, limit: Seconds<T>, tol: T) -> Rkf45<T, F>
    where
        F: Fn(Seconds<T>) -> Vec<T>,
    {
        Rkf45::new(self, u, x0, h, limit, tol)
    }
}

impl<T> Ss<T>
where
    T: Abs
        + Add<Output = T>
        + AddAssign
        + Clone
        + Div<Output = T>
        + Mul<Output = T>
        + NumCast
        + One
        + PartialOrd
        + RadauConst
        + RelativeEq
        + Sub<Output = T>
        + Zero,
{
    /// Radau of order 3 with 2 steps method for time evolution.
    ///
    /// # Arguments
    ///
    /// * `u` - input function returning a vector (column vector)
    /// * `x0` - initial state (column vector)
    /// * `h` - integration time interval
    /// * `n` - integration steps
    /// * `tol` - error tolerance
    pub fn radau<F>(&self, u: F, x0: &[T], h: Seconds<T>, n: usize, tol: T) -> Option<Radau<T, F>>
    where
        F: Fn(Seconds<T>) -> Vec<T>,
    {
        Radau::new(self, u, x0, h, n, tol)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[allow(clippy::many_single_char_names)]
    #[test]
    fn equilibrium() {
        let a = [-1.0_f64, 1., -1., 0.25];
        let b = [1., 0.25];
        let c = [0., 1., -1., 1.];
        let d = [0., 1.];

        let sys = Ss::new_from_slice(2, 1, 2, &a, &b, &c, &d);
        let u = 0.0;
        let eq = sys.equilibrium(&[u]).unwrap();
        assert_eq!((0., 0.), (eq.x()[0], eq.y()[0]));
        assert!(!format!("{}", eq).is_empty());

        // Test wrong number of inputs.
        assert!(sys.equilibrium(&[0., 0., 0.]).is_none());
    }

    #[test]
    fn stability() {
        let eig1 = -2.;
        let eig2 = -7.;
        let sys = Ss::new_from_slice(
            2,
            1,
            1,
            &[eig1, 0., 3., eig2],
            &[1., 3.],
            &[-1., 0.5],
            &[0.1],
        );
        assert!(sys.is_stable());
    }

    #[test]
    fn new_rk2() {
        let a = [-1., 1., -1., 0.25];
        let b = [1., 0.25];
        let c = [0., 1.];
        let d = [0.];
        let sys = Ss::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let iter = sys.rk2(|_| vec![1.], &[0., 0.], Seconds(0.1), 30);
        assert_eq!(31, iter.count());
    }

    #[test]
    fn new_rk4() {
        let a = [-1., 1., -1., 0.25];
        let b = [1., 0.25];
        let c = [0., 1.];
        let d = [0.];
        let sys = Ss::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let iter = sys.rk4(|_| vec![1.], &[0., 0.], Seconds(0.1), 30);
        assert_eq!(31, iter.count());
    }

    #[test]
    fn new_rkf45() {
        let a = [-1., 1., -1., 0.25];
        let b = [1., 0.25];
        let c = [0., 1.];
        let d = [0.];
        let sys = Ss::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let iter = sys.rkf45(|_| vec![1.], &[0., 0.], Seconds(0.1), Seconds(2.), 1e-5);
        assert_relative_eq!(2., iter.last().unwrap().time().0, max_relative = 0.01);
    }

    #[test]
    fn new_radau() {
        let a = [-1., 1., -1., 0.25];
        let b = [1., 0.25];
        let c = [0., 1.];
        let d = [0.];
        let sys = Ss::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let iter = sys
            .radau(|_| vec![1.], &[0., 0.], Seconds(0.1), 30, 1e-5)
            .unwrap();
        assert_eq!(31, iter.count());
    }
}