dsmc 0.1.2

Digitalservo Motion Control Library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

mod error;
pub use error::StateSpaceError;

use nalgebra::DMatrix;
use std::marker::PhantomData;

use crate::Continuous;

#[derive(Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
pub struct StateSpaceOrder {
    pub system: usize,
    pub input: usize,
    pub output: usize,
}

/// `D` tells whether this is a continuous-time system (`dx/dt = Ax + Bu`, `Continuous`, the default)
/// or a discrete-time one (`x[k+1] = Ax[k] + Bu[k]`, `Discrete`).
#[derive(Clone, Debug)]
pub struct StateSpace<T, D = Continuous> {
    pub a: DMatrix<T>,
    pub b: DMatrix<T>,
    pub c: DMatrix<T>,
    pub d: DMatrix<T>,
    pub order: StateSpaceOrder,
    _domain: PhantomData<D>,
}

impl<T, D> StateSpace<T, D> {
    pub fn new(a: DMatrix<T>, b: DMatrix<T>, c: DMatrix<T>, d: DMatrix<T>) -> Result<Self, StateSpaceError> {

        let state_order = a.nrows();
        let input_order = b.ncols();
        let output_order = c.nrows();

        if a.ncols() != state_order {
            return Err(StateSpaceError::SystemMatrix {
                rows: a.nrows(),
                cols: a.ncols(),
            })
        }

        if b.nrows() != state_order {
            return Err(StateSpaceError::InputMatrix {
                expected_row: state_order,
                expected_col: input_order,
                actual_rows: b.nrows(),
                actual_cols: b.ncols(),
            })
        }

        if c.ncols() != state_order {
            return Err(StateSpaceError::OutputMatrix {
                expected_row: output_order,
                expected_col: state_order,
                actual_rows: c.nrows(),
                actual_cols: c.ncols(),
            })
        }

        if (d.nrows(), d.ncols()) != (output_order, input_order) {
            return Err(StateSpaceError::FeedthroughMatrix {
                expected_row: output_order,
                expected_col: input_order,
                actual_rows: d.nrows(),
                actual_cols: d.ncols(),
            })
        }

        let order = StateSpaceOrder {
            system: a.nrows(),
            input: b.ncols(),
            output: c.nrows(),
        };

        Ok(Self {a, b, c, d, order, _domain: PhantomData})
    }
}