# State Space Class : Python



## Discrete Time



* Let's consider a time-invariant discret time state space representation as follows.



$$
\begin{align}
    \mathbf{x}_{k+1}&=\mathbf{Ax}_{k}+\mathbf{Bu}_{k} \\
    \mathbf{y}_{k}&=\mathbf{Cx}_{k}+\mathbf{Du}_{k}
\end{align}
$$



* If $\mathbf{D} = 0$ we can rewrite as follows.



$$
\begin{align}
    \mathbf{x}_{k+1}&=\mathbf{Ax}_{k}+\mathbf{Bu}_{k} \\
    \mathbf{y}_{k}&=\mathbf{Cx}_{k}
\end{align}
$$



* Here, we can think about following analogies.



|              |        State space        |      Class       |
|:------------:|:-------------------------:|:----------------:|
| $\mathbf{x}$ | vector of state variables | member variables |
| $\mathbf{B}$ |       input matrix        |  mutator method  |
| $\mathbf{u}$ | vector of input variables | mutator method argument |
| $\mathbf{C}$ |       output matrix       |  reader method  |
| $\mathbf{y}$ |    measurement vector     | return values of reader method |



* Let's see if the analogies above works.




```python
import numpy as np


```


```python
class LTI_DT(object):
    
    def __init__(self, A, B, C, D, x0=None,):

        # Set state space representation
        self.A = A
        self.B = B
        self.C = C
        self.D = D

        # is A matrix square?
        assert A.shape[0] == A.shape[1]
        
        # number of state variables
        self.n = A.shape[0]

        # check number of rows of B matrix
        assert B.shape[0] == self.n
        
        # expected size of input
        self.m = B.shape[1]

        # Initialize state column vector
        # https://stackoverflow.com/questions/12575421/convert-a-1d-array-to-a-2d-array-in-numpy
        self.x = np.array(x0).reshape((self.n, 1))

    def get_y(self, u_array=None):
        """
        y[k] = C x[k] + D u[k]
        """
        result = self.C @ self.x
        try:
            if u_array is not None:
                result += self.D @ u_array.reshape((self.m, 1))
        except ValueError as e:
            print(f'self.D = \n{self.D}')
            print(f'self.D.shape = \n{self.D.shape}')
            print(f'u_array = \n{u_array}')
            print(f'u_array.shape = \n{u_array.shape}')
            print(f'self.D @ u_array = \n{self.D @ u_array}')
            print(f'(self.D @ u_array).shape = \n{(self.D @ u_array).shape}')
            print(f'result = \n{result}')
            print(f'result.shape = \n{result.shape}')
            raise e
            
        return result

    def get_next_x(self, u_array=None):
        """
        x[k+1] = A x[k] + B u[k]
        """
        next_x = self.A @ self.x
        
        if u_array is not None:
            next_x += self.B @ u_array.reshape((self.m, 1))
        
        self.x = next_x

        
```

* Let's consider one example




```python
# Peng & Chiu, Design of Digital Control Systems, 2000.

A = np.array(
    [
        [0, 1],
        [-0.8, 1.8]
    ]
)

B = np.array([[0, 1]]).T

C = np.array(
    [
        [1, 0],
        [0, 1],
        [-0.5, 1],
    ]
)

D = np.zeros((C.shape[0], B.shape[1]))

u_array = np.array([0.15, 0.075, 0.0375, 0.01875])


```


```python
y_list = []
k_list = []
x0 = np.array([1, 0.5]).T
ss_dt = LTI_DT(A, B, C, D, x0)

for k, u in enumerate(u_array):
    k_list.append(k)
    y_now = ss_dt.get_y(u)
    y_list.append(y_now)
    ss_dt.get_next_x(u)

y_array = np.array(y_list)

y_array.squeeze().T


```

* Let's think about another example




```python
# https://ccrma.stanford.edu/~jos/fp/State_Space_Simulation_Matlab.html

A = np.array(
    [
        [0, 1],
        [-1, 0]
    ]
)

B = np.array([[0, 1]]).T

C = np.array(
    [
        [1, 0],
        [0, 1],
        [0, 1],
    ]
)

D = np.zeros((C.shape[0], B.shape[1]))

u_array = np.zeros((10)).T
u_array[0] = 1.0


```


```python
y_list = []
k_list = []
x0 = np.array([0, 0.0]).T
ss_dt = LTI_DT(A, B, C, D, x0)

for k, u in enumerate(u_array):
    k_list.append(k)
    y_now = ss_dt.get_y(u)
    y_list.append(y_now)
    ss_dt.get_next_x(u)

y_array = np.array(y_list)

y_array.squeeze().T


```

* For a possible C++ version please see if you can find in another file.

## Continuous Time

* A state space representation may calculate time derivatives of the state vector as follows.

$$
\begin{align}
    \frac{d}{dt}\mathbf{x}(t)&=\mathbf{Ax}(t)+\mathbf{Bu}(t) \\
    \mathbf{y}(t)&=\mathbf{Cx}(t)+\mathbf{Du}(t)
\end{align}
$$



* Assume that at time $t_i$ we know the state $\mathbf{x}(t_i)$ and want to find state $\mathbf{x}(t_{i+1})$ at time $t_{i+1} = t_i + \Delta t$

* For simplicity, we may apply Euler's method to find $\mathbf{x}(t_{i+1})$.

$$
    \mathbf{x}(t_{i+1}) =  \mathbf{x}(t_{i}) + \Delta t \frac{d}{dt}\mathbf{x}(t_i)
$$

* Let's implement by inheriting discrete time model.


```python
class LTI_CT(LTI_DT):
    def __init__(self, A, B, C, D, delta_t, x0=None, t0=0):
        super().__init__(A, B, C, D, x0)
        self.delta_t = delta_t
        self.t = t0

    def get_dx_dt(self, u_t=None):
        """
        dx_dt(t[i]) = A x(t[i]) + B u(t[i])
        """

        dx_dt_now = self.A @ self.x

        if u_t is not None:
            dx_dt_now += self.B @ u_t.reshape((self.m, 1))

        return dx_dt_now

    def get_next_x(self, u_t=None):
        """
        Euler method
        x(t[i+1]) = x(t[i]) + delta_t * dx_dt(t[i])
        """

        next_x = self.x + self.delta_t * self.get_dx_dt(u_t)

        self.x = next_x
        self.t += self.delta_t


```

* Let's think about following vibration system:

$$
    \begin{align}
        m \frac{d^2y(t)}{{dt}^2} &+ c \frac{dy(t)}{dt} + k y(t) = u(t) \\
        m \frac{d^2y(t)}{{dt}^2} &= - k y(t) - c \frac{dy(t)}{dt} + u(t) \\
        \mathbf{x}(t) &= \left(y(t), \frac{dy(t)}{dt} \right)^T \\
        \frac{d}{dt}\mathbf{x}(t) &= \left(\frac{dy(t)}{dt} , \frac{d^2y(t)}{{dt}^2} \right)^T \\
        &= \begin{bmatrix}
                0 & 1 \\
                -\frac{k}{m} & -\frac{c}{m} \\
            \end{bmatrix}  \mathbf{x} (t)
          + \begin{bmatrix}
                0\\
                \frac{1}{m}\\
            \end{bmatrix}  u (t)
    \end{align}
$$


```python
m = 1 # kg
c = 100 # N / m / s
k = 10000 # N / m

A = np.array(
    [
        [0, 1],
        [-k/m, -c/m]
    ]
)

B = np.array([[0, 1/m]]).T

C = np.array(
    [
        [1, 0],
        [0, 1],
    ]
)

D = np.zeros((C.shape[0], B.shape[1]))

w_hz = 1.0

t_end_sec = 5 / w_hz
delta_t_sec = t_end_sec * 1e-3

t_array = np.arange(0, t_end_sec+delta_t_sec*0.5, delta_t_sec)
u_array = np.sin((2 * np.pi * w_hz) * t_array)


```


```python
import matplotlib.pyplot as plt

y_list = []
t_list = []

x0 = np.array([0, 0.0]).T
delta_t_sec = 1e-3

ss_ct = LTI_CT(A, B, C, D, delta_t_sec, x0)

for u in u_array:
    t_list.append(ss_ct.t)
    y_now = ss_ct.get_y(u)
    y_list.append(y_now)
    ss_ct.get_next_x(u)

y_array = np.array(y_list).squeeze()

plt.subplot(2, 1, 1)
plt.plot(t_list, y_array[:, 0])
plt.grid(True)
plt.xlabel('t(sec)')
plt.ylabel('y(m)')

plt.subplot(2, 1, 2)
plt.plot(t_list, y_array[:, 1])
plt.grid(True)
plt.xlabel('t(sec)')
plt.ylabel('dy/dt(m/s)')

plt.savefig('lti_ct_ss.svg')

# If plot does not show up, please uncomment following lines
# import IPython.display as disp
# disp.display(disp.SVG('lti_ct_ss.svg'))
# https://stackoverflow.com/questions/30334385/display-svg-in-ipython-notebook-from-a-function


```

## Exercise

### 00 Continuous time version in C++

* Try to implement Continuous Time State Space model in C++
* Group assignment possible

### 01 State Space Simulation

* Implement a state space simulation and visualize using `matplotlib`.

* Following table suggests some of the possibilities.

|     model     | language |      visualization       |
|:-------------:|:--------:|:------------------------:|
| bicycle model |  python  |      `matplotlib`        |
| bicycle model |  cython  |      `matplotlib`        |
| bicycle model |   c++    | file i/o + `matplotlib`  |
| bicycle model |   c++    | `cython` + `matplotlib`  |
| path tracker  |  python  |      `matplotlib`        |
| path tracker  |  cython  |      `matplotlib`        |
| path tracker  |   c++    | file i/o + `matplotlib`  |
| path tracker  |   c++    | `cython` + `matplotlib`  |



```python

```

1ba5cd446256e00d7370abd091c2c4c198769670
ipynb
Jupyter Notebook
10.ipynb
2018pycpp/18pycpp-04
144a86e83bbc7ca45c03a894b923430c406a14fc
BSD-3-Clause
10.ipynb
2018pycpp/18pycpp-04
144a86e83bbc7ca45c03a894b923430c406a14fc
BSD-3-Clause
2018-10-26T12:38:02.000Z
2018-11-16T08:33:58.000Z
10.ipynb
kangwonlee/18pycpp-04
144a86e83bbc7ca45c03a894b923430c406a14fc
BSD-3-Clause
Qwen/Qwen-72B
1. YES
2. YES