use nalgebra::{ComplexField, DMatrix, DVector, RealField, Scalar};
use num_traits::Float;
use std::{
marker::PhantomData,
ops::{AddAssign, MulAssign},
};
use crate::{
linear_system::{continuous::Ss, Equilibrium, SsGen},
Discrete, Discretization,
};
pub type Ssd<T> = SsGen<T, Discrete>;
impl<T: ComplexField> Ssd<T> {
pub fn equilibrium(&self, u: &[T]) -> Option<Equilibrium<T>> {
if u.len() != self.dim.inputs() {
eprintln!("Wrong number of inputs.");
return None;
}
let u = DVector::from_row_slice(u);
let bu = &self.b * &u;
let lu = (DMatrix::identity(self.a.nrows(), self.a.ncols()) - &self.a.clone()).lu();
let x = lu.solve(&bu)?;
let y = &self.c * &x + &self.d * u;
Some(Equilibrium::new(x, y))
}
}
impl<T: Scalar> Ssd<T> {
pub fn evolution_fn<F>(&self, steps: usize, input: F, x0: &[T]) -> EvolutionFn<F, T>
where
F: Fn(usize) -> Vec<T>,
{
let state = DVector::from_column_slice(x0);
let next_state = DVector::from_column_slice(x0);
EvolutionFn {
sys: &self,
time: 0,
steps,
input,
state,
next_state,
}
}
pub fn evolution_iter<I, II>(&self, iter: II, x0: &[T]) -> EvolutionIter<I, T>
where
II: IntoIterator<Item = Vec<T>, IntoIter = I>,
I: Iterator<Item = Vec<T>>,
{
let state = DVector::from_column_slice(x0);
let next_state = DVector::from_column_slice(x0);
EvolutionIter {
sys: &self,
state,
next_state,
iter: iter.into_iter(),
}
}
}
impl<T: ComplexField + Float + RealField> Ssd<T> {
#[must_use]
pub fn is_stable(&self) -> bool {
self.poles().iter().all(|p| p.abs() < T::one())
}
}
pub trait DiscreteTime<T: Scalar> {
fn discretize(&self, st: T, method: Discretization) -> Option<Ssd<T>>;
}
impl<T: ComplexField + Float> DiscreteTime<T> for Ss<T> {
fn discretize(&self, st: T, method: Discretization) -> Option<Ssd<T>> {
match method {
Discretization::ForwardEuler => self.forward_euler(st),
Discretization::BackwardEuler => self.backward_euler(st),
Discretization::Tustin => self.tustin(st),
}
}
}
impl<T: ComplexField + Float> Ss<T> {
fn forward_euler(&self, st: T) -> Option<Ssd<T>> {
let states = self.dim.states;
let identity = DMatrix::identity(states, states);
Some(Ssd {
a: identity + &self.a * st,
b: &self.b * st,
c: self.c.clone(),
d: self.d.clone(),
dim: self.dim,
time: PhantomData,
})
}
fn backward_euler(&self, st: T) -> Option<Ssd<T>> {
let states = self.dim.states;
let identity = DMatrix::identity(states, states);
let a = (identity - &self.a * st).try_inverse()?;
Some(Ssd {
b: &a * &self.b * st,
c: &self.c * &a,
d: &self.d + &self.c * &a * &self.b * st,
a,
dim: self.dim,
time: PhantomData,
})
}
fn tustin(&self, st: T) -> Option<Ssd<T>> {
let states = self.dim.states;
let identity = DMatrix::identity(states, states);
let n_05 = T::from(0.5_f32).unwrap();
let a_05_st = &self.a * (n_05 * st);
let k = (&identity - &a_05_st).try_inverse()?;
let b = &k * &self.b * st;
Some(Ssd {
a: &k * (&identity + &a_05_st),
c: &self.c * &k,
d: &self.d + &self.c * &b * n_05,
b,
dim: self.dim,
time: PhantomData,
})
}
}
#[derive(Debug)]
pub struct EvolutionFn<'a, F, T>
where
F: Fn(usize) -> Vec<T>,
T: Scalar,
{
sys: &'a Ssd<T>,
time: usize,
steps: usize,
input: F,
state: DVector<T>,
next_state: DVector<T>,
}
impl<'a, F, T> Iterator for EvolutionFn<'a, F, T>
where
F: Fn(usize) -> Vec<T>,
T: AddAssign + Float + MulAssign + Scalar,
{
type Item = TimeEvolution<T>;
fn next(&mut self) -> Option<Self::Item> {
if self.time > self.steps {
None
} else {
let current_time = self.time;
let u = DVector::from_vec((self.input)(current_time));
std::mem::swap(&mut self.state, &mut self.next_state);
self.next_state = &self.sys.a * &self.state + &self.sys.b * &u;
let output = &self.sys.c * &self.state + &self.sys.d * &u;
self.time += 1;
Some(TimeEvolution {
time: current_time,
state: self.state.as_slice().to_vec(),
output: output.as_slice().to_vec(),
})
}
}
}
#[derive(Debug)]
pub struct EvolutionIter<'a, I, T>
where
I: Iterator<Item = Vec<T>>,
T: Scalar,
{
sys: &'a Ssd<T>,
state: DVector<T>,
next_state: DVector<T>,
iter: I,
}
impl<'a, I, T> Iterator for EvolutionIter<'a, I, T>
where
I: Iterator<Item = Vec<T>>,
T: AddAssign + Float + MulAssign + Scalar,
{
type Item = Vec<T>;
fn next(&mut self) -> Option<Self::Item> {
let u_vec = self.iter.next()?;
let u = DVector::from_vec(u_vec);
std::mem::swap(&mut self.state, &mut self.next_state);
self.next_state = &self.sys.a * &self.state + &self.sys.b * &u;
let output = &self.sys.c * &self.state + &self.sys.d * &u;
Some(output.as_slice().to_vec())
}
}
#[derive(Debug)]
pub struct TimeEvolution<T> {
time: usize,
state: Vec<T>,
output: Vec<T>,
}
impl<T> TimeEvolution<T> {
#[must_use]
pub fn time(&self) -> usize {
self.time
}
#[must_use]
pub fn state(&self) -> &Vec<T> {
&self.state
}
#[must_use]
pub fn output(&self) -> &Vec<T> {
&self.output
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn equilibrium() {
let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
let b = &[0., 1., 0., 0., -1., 0.];
let c = &[1., 1.8, 1.1];
let d = &[-1., 1.];
let u = &[170., 0.];
let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
let eq = sys.equilibrium(u).unwrap();
assert_relative_eq!(200.0, eq.x()[0]);
assert_relative_eq!(200.0, eq.x()[1]);
assert_relative_eq!(100.0, eq.x()[2], max_relative = 1e-10);
assert_relative_eq!(500.0, eq.y()[0]);
assert!(sys.equilibrium(&[0., 0., 0.]).is_none());
}
#[test]
fn stability() {
let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
let b = &[0., 1., 0., 0., -1., 0.];
let c = &[1., 1.8, 1.1];
let d = &[-1., 1.];
let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
assert!(!sys.is_stable());
}
#[test]
fn time_evolution() {
let disc_sys =
Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
let impulse = |t| if t == 0 { vec![1.] } else { vec![0.] };
let evo = disc_sys.evolution_fn(20, impulse, &[0., 0.]);
let last = evo.last().unwrap();
assert_eq!(20, last.time());
assert_abs_diff_eq!(0., last.state()[1], epsilon = 0.001);
assert_abs_diff_eq!(0., last.output()[0], epsilon = 0.001);
}
#[test]
fn time_evolution_iter() {
use std::iter;
let disc_sys =
Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
let impulse = iter::once(vec![1.]).chain(iter::repeat(vec![0.])).take(20);
let evo = disc_sys.evolution_iter(impulse, &[0., 0.]);
let last = evo.last().unwrap();
assert!(last[0] < 0.001);
}
#[test]
fn discretization_tustin() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::Tustin).unwrap();
let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
#[test]
fn discretization_tustin_fail() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(2., Discretization::Tustin);
assert!(disc_sys.is_none());
}
#[test]
fn discretization_euler_backward() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::BackwardEuler).unwrap();
let evo = disc_sys.evolution_fn(50, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
#[test]
fn discretization_euler_backward_fail() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(1., Discretization::BackwardEuler);
assert!(disc_sys.is_none());
}
#[test]
fn discretization_euler_forward() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::ForwardEuler).unwrap();
let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
}