au/linear_system/
discrete.rs

1//! # Discrete linear system
2//!
3//! Time evolution of the system is performed through successive
4//! matrix multiplications.
5//!
6//! This module contains the algorithm for the discretization of
7//! [continuous](../continuous/index.html) systems:
8//! * forward Euler method
9//! * backward Euler method
10//! * Tustin (trapezoidal) method
11
12use nalgebra::{ComplexField, DMatrix, DVector, RealField, Scalar};
13use num_traits::Float;
14
15use std::{
16    marker::PhantomData,
17    ops::{AddAssign, MulAssign},
18};
19
20use crate::{
21    enums::{Discrete, Discretization},
22    linear_system::{continuous::Ss, Equilibrium, SsGen},
23};
24
25/// State-space representation of discrete time linear system
26pub type Ssd<T> = SsGen<T, Discrete>;
27
28/// Implementation of the methods for the state-space
29impl<T: ComplexField> Ssd<T> {
30    /// Calculate the equilibrium point for discrete time systems,
31    /// given the input condition.
32    /// Input vector must have the same number of inputs of the system.
33    /// ```text
34    /// x = (I-A)^-1 * B * u
35    /// y = (C * (I-A)^-1 * B + D) * u
36    /// ```
37    ///
38    /// # Arguments
39    ///
40    /// * `u` - Input vector
41    ///
42    /// # Example
43    ///
44    /// ```
45    /// use au::Ssd;
46    /// let a = [-1., 1., -1., 0.25];
47    /// let b = [1., 0.25];
48    /// let c = [0., 1., -1., 1.];
49    /// let d = [0., 1.];
50    ///
51    /// let sys = Ssd::new_from_slice(2, 1, 2, &a, &b, &c, &d);
52    /// let u = 0.0;
53    /// let eq = sys.equilibrium(&[u]).unwrap();
54    /// assert_eq!((0., 0.), (eq.x()[0], eq.y()[0]));
55    /// ```
56    pub fn equilibrium(&self, u: &[T]) -> Option<Equilibrium<T>> {
57        if u.len() != self.dim.inputs() {
58            eprintln!("Wrong number of inputs.");
59            return None;
60        }
61        let u = DVector::from_row_slice(u);
62        // x = A*x + B*u -> (I-A)*x = B*u
63        let bu = &self.b * &u;
64        let lu = (DMatrix::identity(self.a.nrows(), self.a.ncols()) - &self.a.clone()).lu();
65        // (I-A)*x = -B*u
66        let x = lu.solve(&bu)?;
67        // y = C*x + D*u
68        let y = &self.c * &x + &self.d * u;
69        Some(Equilibrium::new(x, y))
70    }
71}
72
73/// Trait for the set of methods on discrete linear systems.
74impl<T: Scalar> Ssd<T> {
75    /// Time evolution for a discrete linear system.
76    ///
77    /// # Arguments
78    ///
79    /// * `step` - simulation length
80    /// * `input` - input function
81    /// * `x0` - initial state
82    ///
83    /// # Example
84    /// ```
85    /// # #[macro_use] extern crate approx;
86    /// use au::{Discretization, Ssd};
87    /// let disc_sys = Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
88    /// let impulse = |t| if t == 0 { vec![1.] } else { vec![0.] };
89    /// let evo = disc_sys.evolution_fn(20, impulse, &[0., 0.]);
90    /// let last = evo.last().unwrap();
91    /// assert_abs_diff_eq!(0., last.state()[1], epsilon = 0.001);
92    /// ```
93    pub fn evolution_fn<F>(&self, steps: usize, input: F, x0: &[T]) -> EvolutionFn<F, T>
94    where
95        F: Fn(usize) -> Vec<T>,
96    {
97        let state = DVector::from_column_slice(x0);
98        let next_state = DVector::from_column_slice(x0);
99        EvolutionFn {
100            sys: &self,
101            time: 0,
102            steps,
103            input,
104            state,
105            next_state,
106        }
107    }
108
109    /// Time evolution for a discrete linear system.
110    ///
111    /// # Arguments
112    ///
113    /// * `iter` - input data
114    /// * `x0` - initial state
115    ///
116    /// # Example
117    /// ```
118    /// use std::iter;
119    /// use au::{Discretization, Ssd};
120    /// let disc_sys = Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
121    /// let impulse = iter::once(vec![1.]).chain(iter::repeat(vec![0.])).take(20);
122    /// let evo = disc_sys.evolution_iter(impulse, &[0., 0.]);
123    /// let last = evo.last().unwrap();
124    /// assert!(last[0] < 0.001);
125    /// ```
126    pub fn evolution_iter<I, II>(&self, iter: II, x0: &[T]) -> EvolutionIter<I, T>
127    where
128        II: IntoIterator<Item = Vec<T>, IntoIter = I>,
129        I: Iterator<Item = Vec<T>>,
130    {
131        let state = DVector::from_column_slice(x0);
132        let next_state = DVector::from_column_slice(x0);
133        EvolutionIter {
134            sys: &self,
135            state,
136            next_state,
137            iter: iter.into_iter(),
138        }
139    }
140}
141
142impl<T: ComplexField + Float + RealField> Ssd<T> {
143    /// System stability. Checks if all A matrix eigenvalues (poles) are inside
144    /// the unit circle.
145    ///
146    /// # Example
147    ///
148    /// ```
149    /// use au::Ssd;
150    /// let sys = Ssd::new_from_slice(2, 1, 1, &[-0.2, 0., 3., 0.1], &[1., 3.], &[-1., 0.5], &[0.1]);
151    /// assert!(sys.is_stable());
152    /// ```
153    #[must_use]
154    pub fn is_stable(&self) -> bool {
155        self.poles().iter().all(|p| p.norm() < T::one())
156    }
157}
158
159impl<T: ComplexField + Float> Ss<T> {
160    /// Convert a linear system into a discrete system.
161    ///
162    /// # Arguments
163    ///
164    /// * `st` - sample time
165    /// * `method` - discretization method
166    ///
167    /// # Example
168    /// ```
169    /// # #[macro_use] extern crate approx;
170    /// use au::{Discretization, Ss};
171    /// let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
172    /// let disc_sys = sys.discretize(0.1, Discretization::Tustin).unwrap();
173    /// let evo = disc_sys.evolution_fn(20, |t| vec![1.], &[0., 0.]);
174    /// let last = evo.last().unwrap();
175    /// assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
176    /// ```
177    pub fn discretize(&self, st: T, method: Discretization) -> Option<Ssd<T>> {
178        match method {
179            Discretization::ForwardEuler => self.forward_euler(st),
180            Discretization::BackwardEuler => self.backward_euler(st),
181            Discretization::Tustin => self.tustin(st),
182        }
183    }
184}
185
186impl<T: ComplexField + Float> Ss<T> {
187    /// Discretization using forward Euler Method.
188    ///
189    /// # Arguments
190    ///
191    /// * `st` - sample time
192    fn forward_euler(&self, st: T) -> Option<Ssd<T>> {
193        let states = self.dim.states;
194        let identity = DMatrix::identity(states, states);
195        Some(Ssd {
196            a: identity + &self.a * st,
197            b: &self.b * st,
198            c: self.c.clone(),
199            d: self.d.clone(),
200            dim: self.dim,
201            time: PhantomData,
202        })
203    }
204
205    /// Discretization using backward Euler Method.
206    ///
207    /// # Arguments
208    ///
209    /// * `st` - sample time
210    fn backward_euler(&self, st: T) -> Option<Ssd<T>> {
211        let states = self.dim.states;
212        let identity = DMatrix::identity(states, states);
213        let a = (identity - &self.a * st).try_inverse()?;
214        Some(Ssd {
215            b: &a * &self.b * st,
216            c: &self.c * &a,
217            d: &self.d + &self.c * &a * &self.b * st,
218            a,
219            dim: self.dim,
220            time: PhantomData,
221        })
222    }
223
224    /// Discretization using Tustin Method.
225    ///
226    /// # Arguments
227    ///
228    /// * `st` - sample time
229    fn tustin(&self, st: T) -> Option<Ssd<T>> {
230        let states = self.dim.states;
231        let identity = DMatrix::identity(states, states);
232        // Casting is safe for both f32 and f64, representation is exact.
233        let n_05 = T::from(0.5_f32).unwrap();
234        let a_05_st = &self.a * (n_05 * st);
235        let k = (&identity - &a_05_st).try_inverse()?;
236        let b = &k * &self.b * st;
237        Some(Ssd {
238            a: &k * (&identity + &a_05_st),
239            c: &self.c * &k,
240            d: &self.d + &self.c * &b * n_05,
241            b,
242            dim: self.dim,
243            time: PhantomData,
244        })
245    }
246}
247
248/// Struct to hold the iterator for the evolution of the discrete linear system.
249/// It uses function to supply inputs.
250#[derive(Debug)]
251pub struct EvolutionFn<'a, F, T>
252where
253    F: Fn(usize) -> Vec<T>,
254    T: Scalar,
255{
256    sys: &'a Ssd<T>,
257    time: usize,
258    steps: usize,
259    input: F,
260    state: DVector<T>,
261    next_state: DVector<T>,
262}
263
264impl<'a, F, T> Iterator for EvolutionFn<'a, F, T>
265where
266    F: Fn(usize) -> Vec<T>,
267    T: AddAssign + Float + MulAssign + Scalar,
268{
269    type Item = TimeEvolution<T>;
270
271    fn next(&mut self) -> Option<Self::Item> {
272        if self.time > self.steps {
273            None
274        } else {
275            let current_time = self.time;
276            let u = DVector::from_vec((self.input)(current_time));
277            // Copy `next_state` of the previous iteration into
278            // the current `state`.
279            std::mem::swap(&mut self.state, &mut self.next_state);
280            self.next_state = &self.sys.a * &self.state + &self.sys.b * &u;
281            let output = &self.sys.c * &self.state + &self.sys.d * &u;
282            self.time += 1;
283            Some(TimeEvolution {
284                time: current_time,
285                state: self.state.as_slice().to_vec(),
286                output: output.as_slice().to_vec(),
287            })
288        }
289    }
290}
291
292/// Struct to hold the iterator for the evolution of the discrete linear system.
293/// It uses iterators to supply inputs.
294#[derive(Debug)]
295pub struct EvolutionIter<'a, I, T>
296where
297    I: Iterator<Item = Vec<T>>,
298    T: Scalar,
299{
300    sys: &'a Ssd<T>,
301    state: DVector<T>,
302    next_state: DVector<T>,
303    iter: I,
304}
305
306impl<'a, I, T> Iterator for EvolutionIter<'a, I, T>
307where
308    I: Iterator<Item = Vec<T>>,
309    T: AddAssign + Float + MulAssign + Scalar,
310{
311    type Item = Vec<T>;
312
313    fn next(&mut self) -> Option<Self::Item> {
314        let u_vec = self.iter.next()?;
315        let u = DVector::from_vec(u_vec);
316        // Copy `next_state` of the previous iteration into
317        // the current `state`.
318        std::mem::swap(&mut self.state, &mut self.next_state);
319        self.next_state = &self.sys.a * &self.state + &self.sys.b * &u;
320        let output = &self.sys.c * &self.state + &self.sys.d * &u;
321        Some(output.as_slice().to_vec())
322    }
323}
324
325/// Struct to hold the result of the discrete linear system evolution.
326#[derive(Debug)]
327pub struct TimeEvolution<T> {
328    time: usize,
329    state: Vec<T>,
330    output: Vec<T>,
331}
332
333impl<T> TimeEvolution<T> {
334    /// Get the time of the current step
335    #[must_use]
336    pub fn time(&self) -> usize {
337        self.time
338    }
339
340    /// Get the current state of the system
341    #[must_use]
342    pub fn state(&self) -> &Vec<T> {
343        &self.state
344    }
345
346    /// Get the current output of the system
347    #[must_use]
348    pub fn output(&self) -> &Vec<T> {
349        &self.output
350    }
351}
352
353#[cfg(test)]
354mod tests {
355    use super::*;
356
357    #[allow(clippy::many_single_char_names)]
358    #[test]
359    fn equilibrium() {
360        let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
361        let b = &[0., 1., 0., 0., -1., 0.];
362        let c = &[1., 1.8, 1.1];
363        let d = &[-1., 1.];
364        let u = &[170., 0.];
365
366        let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
367        let eq = sys.equilibrium(u).unwrap();
368
369        assert_relative_eq!(200.0, eq.x()[0]);
370        assert_relative_eq!(200.0, eq.x()[1]);
371        assert_relative_eq!(100.0, eq.x()[2], max_relative = 1e-10);
372        assert_relative_eq!(500.0, eq.y()[0]);
373
374        // Test wrong number of inputs.
375        assert!(sys.equilibrium(&[0., 0., 0.]).is_none());
376    }
377
378    #[test]
379    fn stability() {
380        let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
381        let b = &[0., 1., 0., 0., -1., 0.];
382        let c = &[1., 1.8, 1.1];
383        let d = &[-1., 1.];
384
385        let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
386
387        assert!(!sys.is_stable());
388    }
389
390    #[test]
391    fn time_evolution() {
392        let disc_sys =
393            Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
394        let impulse = |t| if t == 0 { vec![1.] } else { vec![0.] };
395        let evo = disc_sys.evolution_fn(20, impulse, &[0., 0.]);
396        let last = evo.last().unwrap();
397        assert_eq!(20, last.time());
398        assert_abs_diff_eq!(0., last.state()[1], epsilon = 0.001);
399        assert_abs_diff_eq!(0., last.output()[0], epsilon = 0.001);
400    }
401
402    #[test]
403    fn time_evolution_iter() {
404        use std::iter;
405        let disc_sys =
406            Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
407        let impulse = iter::once(vec![1.]).chain(iter::repeat(vec![0.])).take(20);
408        let evo = disc_sys.evolution_iter(impulse, &[0., 0.]);
409        let last = evo.last().unwrap();
410        assert!(last[0] < 0.001);
411    }
412
413    #[test]
414    fn discretization_tustin() {
415        let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
416        let disc_sys = sys.discretize(0.1, Discretization::Tustin).unwrap();
417        let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
418        let last = evo.last().unwrap();
419        assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
420    }
421
422    #[test]
423    fn discretization_tustin_fail() {
424        let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
425        let disc_sys = sys.discretize(2., Discretization::Tustin);
426        assert!(disc_sys.is_none());
427    }
428
429    #[test]
430    fn discretization_euler_backward() {
431        let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
432        let disc_sys = sys.discretize(0.1, Discretization::BackwardEuler).unwrap();
433        //let evo = disc_sys.time_evolution(20, |_| vec![1.], &[0., 0.]);
434        let evo = disc_sys.evolution_fn(50, |_| vec![1.], &[0., 0.]);
435        let last = evo.last().unwrap();
436        assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
437    }
438
439    #[test]
440    fn discretization_euler_backward_fail() {
441        let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
442        let disc_sys = sys.discretize(1., Discretization::BackwardEuler);
443        assert!(disc_sys.is_none());
444    }
445
446    #[test]
447    fn discretization_euler_forward() {
448        let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
449        let disc_sys = sys.discretize(0.1, Discretization::ForwardEuler).unwrap();
450        let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
451        let last = evo.last().unwrap();
452        assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
453    }
454}
au/linear_system/discrete.rs

au/linear_system/
discrete.rs
