dsmc 0.1.4

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

use std::ops::{AddAssign, MulAssign};
use nalgebra::{ComplexField, DMatrix, DVector};
use num_traits::Float;

use crate::{Discrete, TransferFunction};

//Use sequential data
pub struct DataBuffer<T> {
    pub u: DVector<T>,
    pub x: DVector<T>,
    psi_sum: DVector<T>,
    phi_sum: DMatrix<T>,
    input_order: usize,
    state_order: usize,
}

impl<T: Float + AddAssign + MulAssign + ComplexField> DataBuffer<T> {
    pub fn new(input_order: usize, state_order: usize) -> Self {
        Self {
            u: DVector::zeros(input_order + 1),
            x: DVector::zeros(state_order),
            psi_sum: DVector::zeros(input_order + state_order + 1),
            phi_sum: DMatrix::zeros(input_order + state_order + 1, input_order + state_order + 1),
            input_order,
            state_order,
        }
    }

    pub fn add(&mut self, u: T, x: T, y: T) {
        //FIFO for input u
        for i in (1..(self.input_order + 1)).rev() {
            self.u[i] = self.u[i - 1]
        }
        self.u[0] = u;

        //FIFO for state x
        for i in (1..self.state_order).rev() {
            self.x[i] = self.x[i - 1]
        }
        self.x[0] = x;

        let mut phi = DVector::zeros(self.input_order + self.state_order + 1);
        for i in 0..self.state_order {
            phi[i] = self.x[i]
        }
        for i in 0..(self.input_order + 1) {
            phi[i + self.state_order] = self.u[i]
        }

        self.psi_sum += &phi * y;
        self.phi_sum += &phi * &phi.transpose();
    }

    pub fn identify(&self) -> Option<TransferFunction<T, Discrete>> {
        match self.phi_sum.clone().try_inverse() {
            Some(res) => {
                let theta = &res * &self.psi_sum;

                let mut denom = Vec::<T>::with_capacity(self.state_order + 1);
                let mut numer = Vec::<T>::with_capacity(self.input_order + 1);

                denom.push(T::one());
                for i in 0..self.state_order {
                    denom.push(-theta[i]);
                }
                for i in 0..(self.input_order + 1) {
                    numer.push(theta[i + self.state_order]);
                }

                Some(TransferFunction::discrete(&numer, &denom))
            }
            None => None,
        }
    }
}