dsmc 0.16.0

//! Multirate perfect tracking control (PTC) on the lifted model.

use std::borrow::Borrow;

use nalgebra::{ComplexField, DMatrix, DVector, RealField};
use num_traits::Float;
use thiserror::Error;

use super::ReferenceSignal;
use crate::discretize::Zoh;
use crate::laplace_transform::StableInverseError;
use crate::{Continuous, Discrete, StateSpace, StateSpaceError};

/// Zero-order hold discretization of a continuous-time plant lifted over a frame of `order`
/// samples, for multirate perfect tracking control. Built with `LiftedDiscretizedSystem::new`.
///
/// The discretized plant `x[k+1] = A x[k] + B u[k]` is lifted over a frame of `n` samples
/// (`n` = number of states): `x[(i+1)n] = A^n x[in] + B_lifted [u[in], ..., u[in+n-1]]`, so the
/// `n` inputs of a frame that bring the state exactly onto a reference are
/// `B_lifted^-1 (x_d[(i+1)n] - A^n x_d[in])`.
pub struct LiftedDiscretizedSystem<T> {
    /// Continuous-time model the system was discretized from.
    pub continuous: StateSpace<T, Continuous>,
    /// Lifted model `x[(i+1)n] = A^n x[in] + B_lifted [u[in], ..., u[in+n-1]]`.
    pub ssr: StateSpace<T, Discrete>,
    /// Sampling period of the input (not of the frame).
    pub ts: T,
    /// Samples per frame `n` (= number of states).
    pub order: u32,
    inv_b: DMatrix<T>,
}

impl<T: Float + ComplexField + RealField> LiftedDiscretizedSystem<T> {
    /// Discretize `continuous` with sampling period `ts` (`Zoh`) and lift it over a
    /// frame of `n` samples (`n` = number of states). For an output reference, `continuous` is
    /// usually `StateSpace::normalized_controllable_canonical` of the plant (the state coordinates
    /// of `ReferenceSignal::to_state_reference`); any realization works.
    pub fn new<S: Borrow<StateSpace<T, Continuous>>>(continuous: S, ts: T) -> Result<Self, StateSpaceError> {
        let continuous = continuous.borrow().clone();
        let discrete = continuous.discretize(Zoh, ts)?;

        let n = discrete.order.system;
        let m = discrete.order.input;

        let order = n as u32;

        let a = &discrete.a;
        let b = &discrete.b;

        let a_lifted = a.pow(order);

        // b_lifted = [a^(order-1)*b, a^(order-2)*b, ..., a*b, b]
        let mut b_lifted = DMatrix::<T>::zeros(n, m * order as usize);
        let mut a_power = DMatrix::<T>::identity(n, n);
        for k in 0..order as usize {
            let col = (order as usize - 1 - k) * m;
            b_lifted.columns_mut(col, m).copy_from(&(&a_power * b));
            a_power = a * &a_power;
        }

        let d = DMatrix::<T>::zeros(discrete.order.output, order as usize);

        let inv_b = b_lifted.clone()
            .try_inverse()
            .ok_or(StateSpaceError::SingularMatrix)?;

        let ssr = StateSpace::new(a_lifted, b_lifted, discrete.c.clone(), d)?;

        Ok(Self { continuous, ssr, ts, order, inv_b })
    }
}


/// Errors of perfect tracking control.
#[derive(Clone, Debug, Error, PartialEq)]
pub enum PtcError {
    /// Output references are supported for SISO systems only.
    #[error("Perfect tracking from an output reference supports SISO systems only, got {inputs} inputs and {outputs} outputs")]
    NotSiso {
        /// Number of inputs.
        inputs: usize,
        /// Number of outputs.
        outputs: usize,
    },

    /// The system has a direct feedthrough term.
    #[error("The system has a direct feedthrough term (D != 0)")]
    Feedthrough,

    /// `(A, B)` is not controllable.
    #[error("(A, B) is not controllable")]
    Uncontrollable,

    /// The state reference could not be computed by stable inversion.
    #[error(transparent)]
    StableInverse(#[from] StableInverseError),
}

impl<T: Float + ComplexField + RealField> LiftedDiscretizedSystem<T> {
    /// `B_lifted^-1 (x[k+1] - A^n x[k])` for consecutive frame states.
    fn inputs_from_frame_states(&self, frames: &[DVector<T>]) -> Vec<T> {
        let mut u = Vec::<T>::with_capacity(frames.len() * self.order as usize);
        for points in frames.windows(2) {
            let u_effort = &points[1] - &self.ssr.a * &points[0];
            let u_local = &self.inv_b * u_effort;
            u.extend(u_local.iter());
        }
        u
    }

    /// Perfect tracking control input that makes the output follow `y_d`, for `samples` samples
    /// (rounded up to whole frames of `n` samples). `u[k]` is applied at `t = k ts`, on the same
    /// time axis as `y_d`: place the move on it with the trajectory's start time (rest time).
    ///
    /// The state reference is obtained by stable inversion (bilateral Laplace transform) of the
    /// continuous-time plant `G(s) = C (sI - A)^-1 B`, see `TransferFunction::state_reference_from_output`.
    /// Unstable zeros of `G` make it non-causal: the input starts before the trajectory does
    /// (pre-actuation), so leave enough rest time before the move.
    pub fn calculate_ptc_input_for_reference_output<S: ReferenceSignal<T>>(
        &self,
        y_d: &S,
        samples: usize,
    ) -> Result<Vec<T>, PtcError> {
        let reference = self.state_reference_from_output(y_d)?;
        let n = self.order as usize;
        let frame_time = self.ts * T::from(n).unwrap();
        let frames: Vec<DVector<T>> = (0..=samples.div_ceil(n))
            .map(|k| reference(frame_time * T::from(k).unwrap()))
            .collect();
        Ok(self.inputs_from_frame_states(&frames))
    }

    /// State reference `x_d(t)` (in this system's state coordinates) that makes the output follow `y_d`.
    pub fn state_reference_from_output<S: ReferenceSignal<T>>(&self, y_d: &S) -> Result<impl Fn(T) -> DVector<T> + use<T, S>, PtcError> {
        let sys = &self.continuous;
        if sys.order.input != 1 || sys.order.output != 1 {
            return Err(PtcError::NotSiso { inputs: sys.order.input, outputs: sys.order.output });
        }
        if sys.d.iter().any(|d| !d.is_zero()) {
            return Err(PtcError::Feedthrough);
        }

        let plant = sys.transfer_function().map_err(|_| PtcError::NotSiso { inputs: sys.order.input, outputs: sys.order.output })?;
        let reference = y_d.to_state_reference(&plant)?;

        // x = T x_c with the controllable canonical realization (A_c, B_c) (in which `StateReference`
        // is expressed): T = W(A, B) W(A_c, B_c)^-1.
        let canonical = StateSpace::normalized_controllable_canonical(&plant).map_err(|_| PtcError::Uncontrollable)?;
        let w = controllability_matrix(&sys.a, &sys.b);
        let w_c = controllability_matrix(&canonical.a, &canonical.b);
        let w_c_inv = w_c.try_inverse().ok_or(PtcError::Uncontrollable)?;
        let transform = w * w_c_inv;
        if transform.clone().try_inverse().is_none() {
            return Err(PtcError::Uncontrollable);
        }

        Ok(move |t: T| &transform * DVector::from(reference.state(t)))
    }
}

/// `[B, AB, ..., A^(n-1) B]` (single input).
fn controllability_matrix<T: Float + ComplexField + RealField>(a: &DMatrix<T>, b: &DMatrix<T>) -> DMatrix<T> {
    let n = a.nrows();
    let mut w = DMatrix::<T>::zeros(n, n);
    let mut column = b.column(0).into_owned();
    for k in 0..n {
        w.set_column(k, &column);
        column = a * column;
    }
    w
}