automatica 1.0.0

//! # Discretization module for continuous time transfer functions
//!
//! Given a continuous time transfer function, the sampling period and the
//! discretization method the `TfDiscretization` returns the evaluation of
//! the equivalent discrete time transfer function.
//!
//! The available discretization methods are forward Euler, backward Euler
//! and Tustin (Trapezoidal).

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

use polynomen::Poly;

use crate::{
    enums::Discretization,
    transfer_function::{continuous::Tf, discrete::Tfz},
    units::{RadiansPerSecond, Seconds},
    wrappers::{CWrapper, PWrapper},
    Abs, Complex, Inv, One, Tan, Zero,
};

/// Discretization of a transfer function
#[derive(Debug)]
pub struct TfDiscretization<T> {
    /// Transfer function
    tf: Tf<T>,
    /// Sampling period
    ts: Seconds<T>,
    /// Discretization function
    conversion: fn(Complex<T>, Seconds<T>) -> Complex<T>,
}

/// Implementation of `TfDiscretization` struct.
impl<T> TfDiscretization<T>
where
    T: Clone + PartialEq,
{
    /// Create a new discretization transfer function from a continuous one.
    ///
    /// # Arguments
    /// * `tf` - Continuous transfer function
    /// * `ts` - Sampling period
    /// * `conversion` - Conversion function
    fn new_from_cont(
        tf: Tf<T>,
        ts: Seconds<T>,
        conversion: fn(Complex<T>, Seconds<T>) -> Complex<T>,
    ) -> Self {
        Self { tf, ts, conversion }
    }
}

/// Implementation of `TfDiscretization` struct.
impl<T> TfDiscretization<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialEq
        + PartialOrd
        + Sub<Output = T>
        + Zero,
{
    /// Discretize a transfer function.
    ///
    /// # Arguments
    /// * `tf` - Continuous transfer function
    /// * `ts` - Sampling period
    /// * `method` - Discretization method
    ///
    /// Example
    /// ```
    /// use automatica::{
    ///     transfer_function::discretization::TfDiscretization,
    ///     Complex, Discretization, Seconds, Tf
    /// };
    /// let tf = Tf::new([2., 20.], [1., 0.1]);
    /// let tfz = TfDiscretization::discretize(tf, Seconds(1.), Discretization::BackwardEuler);
    /// let gz = tfz.eval(Complex::new(0., 1.));
    /// ```
    pub fn discretize(tf: Tf<T>, ts: Seconds<T>, method: Discretization) -> Self {
        let conv = match method {
            Discretization::ForwardEuler => fe,
            Discretization::BackwardEuler => fb,
            Discretization::Tustin => tu,
        };
        Self::new_from_cont(tf, ts, conv)
    }
}

impl<T> Tf<T>
where
    T: Add<Output = T>
        + Clone
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialEq
        + Tan
        + Zero,
{
    /// Convert a continuous time transfer function into a discrete time
    /// transfer function using the given method.
    ///
    /// * `ts` - Sampling period in seconds
    /// * `method` - Discretization method
    ///
    /// Example
    /// ```
    /// use automatica::{Complex, Discretization, Seconds, Tf, Tfz};
    /// let tf = Tf::new([2.0_f64, 20.], [1., 0.1]);
    /// let tfz = tf.discretize(Seconds(1.), Discretization::BackwardEuler);
    /// assert_eq!(0.1 / 1.1, tfz.real_poles().unwrap()[0]);
    /// ```
    pub fn discretize(&self, ts: Seconds<T>, method: Discretization) -> Tfz<T> {
        use polynomen::{One, Zero};
        match method {
            Discretization::ForwardEuler => {
                let t = ts.0.inv();
                let s = Poly::new_from_coeffs(&[PWrapper(-t.clone()), PWrapper(t)]);
                let num = self.num_int().0.eval_by_val(s.clone());
                let den = self.den_int().0.eval_by_val(s);
                Tfz::new_from_poly(num, den)
            }
            Discretization::BackwardEuler => {
                let s_num = Poly::new_from_coeffs(&[-PWrapper::one(), PWrapper::one()]);
                let s_den = Poly::new_from_coeffs(&[PWrapper::zero(), PWrapper(ts.0)]);
                discr_impl(self, &s_num, &s_den)
            }
            Discretization::Tustin => {
                let k = (T::one() + T::one()) / ts.0;
                let s_num =
                    Poly::new_from_coeffs(&[-PWrapper::one(), PWrapper::one()]) * PWrapper(k);
                let s_den = Poly::new_from_coeffs(&[PWrapper::one(), PWrapper::one()]);
                discr_impl(self, &s_num, &s_den)
            }
        }
    }

    /// Convert a continuous time transfer function into a discrete time
    /// transfer function using Tustin method with frequency pre-warping.
    ///
    /// * `ts` - Sampling period in seconds
    /// * `warp_freq` - Pre-warping frequency in radians per second
    ///
    /// Example
    /// ```
    /// use automatica::{Complex, Discretization, RadiansPerSecond, Seconds, Tf, Tfz};
    /// let tf = Tf::new([2.0_f32, 20.], [1., 0.1]);
    /// let tfz = tf.discretize_with_warp(Seconds(1.), RadiansPerSecond(0.1));
    /// assert_eq!(-0.6668982, tfz.real_poles().unwrap()[0] as f32);
    /// ```
    pub fn discretize_with_warp(&self, ts: Seconds<T>, warp_freq: RadiansPerSecond<T>) -> Tfz<T> {
        use polynomen::One;
        let two = T::one() + T::one();
        let k = warp_freq.0.clone() / (warp_freq.0 * ts.0 / two).tan();
        let s_num = Poly::new_from_coeffs(&[-PWrapper::one(), PWrapper::one()]) * PWrapper(k);
        let s_den = Poly::new_from_coeffs(&[PWrapper::one(), PWrapper::one()]);
        discr_impl(self, &s_num, &s_den)
    }
}

/// Common operations for discretization
#[allow(clippy::cast_sign_loss)]
fn discr_impl<T>(tf: &Tf<T>, s_num: &Poly<PWrapper<T>>, s_den: &Poly<PWrapper<T>>) -> Tfz<T>
where
    T: Add<Output = T> + Clone + Mul<Output = T> + One + PartialEq + Zero,
{
    let s = Tf::new_from_poly(s_num.clone(), s_den.clone());
    let num = tf
        .num_int()
        .0
        .eval_by_val(PWrapper(s.clone()))
        .0
        .num_int()
        .0
        .clone();
    let den = tf
        .den_int()
        .0
        .eval_by_val(PWrapper(s))
        .0
        .num_int()
        .0
        .clone();
    match tf.relative_degree() {
        g if g > 0 => {
            let num = num * s_den.powi(g as u32);
            Tfz::new_from_poly(num, den)
        }
        g if g < 0 => {
            let den = den * s_den.powi(-g as u32);
            Tfz::new_from_poly(num, den)
        }
        _ => Tfz::new_from_poly(num, den),
    }
}

/// Forward Euler transformation
///
/// # Arguments
/// * `z` - Discrete evaluation point
/// * `ts` - Sampling period
fn fe<T>(z: Complex<T>, ts: Seconds<T>) -> Complex<T>
where
    T: Clone + Div<Output = T> + One + Sub<Output = T>,
{
    (z - T::one()) / ts.0
}

/// Backward Euler transformation
///
/// # Arguments
/// * `z` - Discrete evaluation point
/// * `ts` - Sampling period
fn fb<T>(z: Complex<T>, ts: Seconds<T>) -> Complex<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialOrd
        + Sub<Output = T>
        + Zero,
{
    (z.clone() - T::one()) / (z * ts.0)
}

/// Tustin transformation
///
/// # Arguments
/// * `z` - Discrete evaluation point
/// * `ts` - Sampling period
fn tu<T>(z: Complex<T>, ts: Seconds<T>) -> Complex<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialOrd
        + Sub<Output = T>
        + Zero,
{
    let float = (T::one() + T::one()) / ts.0;
    let num = z.clone() - T::one();
    let den = z + T::one();
    let (re, im) = complex_division::compdiv(
        CWrapper(num.re),
        CWrapper(num.im),
        CWrapper(den.re),
        CWrapper(den.im),
    );
    let complex = Complex::new(re.0, im.0);
    // Complex<T> * T is implemented, not T * Complex<T>
    complex * float
}

impl<T> TfDiscretization<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + PartialEq
        + PartialOrd
        + Sub<Output = T>
        + Zero,
{
    /// Evaluate the discretization of the transfer function
    ///
    /// # Arguments
    /// * `z` - Value at which the transfer function is evaluated.
    pub fn eval(&self, z: Complex<T>) -> Complex<T> {
        let s = (self.conversion)(z, self.ts.clone());
        self.tf.eval(&s)
    }
}

#[cfg(test)]
mod tests {
    use crate::{units::ToDecibel, Complex};

    use super::*;

    #[test]
    fn new_tfz() {
        let _t = TfDiscretization {
            tf: Tf::new([0., 1.], [1., 1., 1.]),
            ts: Seconds(0.1),
            conversion: |_z, _ts| Complex::new(0., 1.) * 1.,
        };
    }

    #[test]
    fn eval_forward_euler() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let z = Complex::<f64>::new(0., 1.) * 0.5;
        let ts = Seconds(1.);
        let tfz = TfDiscretization::discretize(tf, ts, Discretization::ForwardEuler);
        let s = (tfz.conversion)(z, ts);
        assert_relative_eq!(-1.0, s.re);
        assert_relative_eq!(0.5, s.im);

        let gz = tfz.eval(z);
        assert_relative_eq!(27.175, gz.norm().to_db(), max_relative = 1e-4);
        assert_relative_eq!(147.77, gz.arg().to_degrees(), max_relative = 1e-4);
    }

    #[test]
    fn eval_backward_euler() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let z = Complex::<f64>::new(0., 1.) * 0.5;
        let ts = Seconds(1.);
        let tfz = TfDiscretization::discretize(tf, ts, Discretization::BackwardEuler);
        let s = (tfz.conversion)(z, ts);
        assert_relative_eq!(1.0, s.re);
        assert_relative_eq!(2.0, s.im);

        let gz = tfz.eval(z);
        assert_relative_eq!(32.220, gz.norm().to_db(), max_relative = 1e-4);
        assert_relative_eq!(50.884, gz.arg().to_degrees(), max_relative = 1e-4);
    }

    #[test]
    fn eval_tustin() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let z = Complex::<f64>::new(0., 1.) * 0.5;
        let ts = Seconds(1.);
        let tfz = TfDiscretization::discretize(tf, ts, Discretization::Tustin);
        let s = (tfz.conversion)(z, ts);
        assert_relative_eq!(-1.2, s.re);
        assert_relative_eq!(1.6, s.im);

        let gz = tfz.eval(z);
        assert_relative_eq!(32.753, gz.norm().to_db(), max_relative = 1e-4);
        assert_relative_eq!(114.20, gz.arg().to_degrees(), max_relative = 1e-4);
    }

    #[test]
    fn discretization_forward_euler() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let tfz = tf
            .discretize(Seconds(1.), Discretization::ForwardEuler)
            .normalize();
        // (200z - 180)/(z + 9)
        let expected = Tfz::new([-180., 200.], [9., 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_forward_euler2() {
        let tf = Tf::new([1., 2., 3.], [4., 5.]);
        let tfz = tf
            .discretize(Seconds(0.1), Discretization::ForwardEuler)
            .normalize();
        let expected = Tfz::new([5.62, -11.6, 6.], [-0.92, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_forward_euler3() {
        let tf = Tf::new([4.0, 5.], [3., 2., 1.]);
        let tfz = tf
            .discretize(Seconds(0.1), Discretization::ForwardEuler)
            .normalize();
        let expected = Tfz::new([-0.46, 0.5], [0.83, -1.8, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_backward_euler() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let tfz = tf
            .discretize(Seconds(1.), Discretization::BackwardEuler)
            .normalize();
        // (20z - 18.18)/(z - 0.09)
        let expected = Tfz::new([-20. / 1.1, 20.], [-0.1 / 1.1, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_backward_euler2() {
        let tf = Tf::new([2.], [1., 0.1]);
        let tfz = tf
            .discretize(Seconds(1.), Discretization::BackwardEuler)
            .normalize();
        // 2z / (1.1z - 0.1)
        let expected = Tfz::new([0., 2. / 1.1], [-0.1 / 1.1, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_backward_euler3() {
        let tf = Tf::new([1., 2., 3.], [1., 0.1]);
        let tfz = tf
            .discretize(Seconds(1.), Discretization::BackwardEuler)
            .normalize();
        // (3 -8z +6z²) / (-0.1z +1.1z²)
        let expected = Tfz::new([3. / 1.1, -8. / 1.1, 6. / 1.1], [0., -0.1 / 1.1, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn discretization_tustin() {
        let tf = Tf::new([2., 20.], [1., 0.1]);
        let tfz = tf
            .discretize(Seconds(1.), Discretization::Tustin)
            .normalize();
        // (35z - 31.67)/(z + 0.67)
        let expected = Tfz::new([-38. / 1.2, 35.], [0.8 / 1.2, 1.]);
        assert_eq!(expected, tfz);
    }

    #[test]
    fn frequency_warping() {
        // in scilab ss2tf(cls2dls(tf2ss(sys), 1, 0.1/2/%pi))
        let tf = Tf::new([2.0_f32, 20.], [1., 0.1]);
        let tfz = tf
            .discretize_with_warp(Seconds(1.), RadiansPerSecond(0.1))
            .normalize();
        let expected = Tfz::new([-31.643_282, 34.977_077], [0.666_898_2, 1.]);
        assert_eq!(expected, tfz);
    }
}