feat: end assignment

MRAC.md

@@ -0,0 +1,124 @@
+# MRAC (Model Reference Adaptive Control)
+ Considering the LTI system
+
+$$
+\dot{x}=Ax+Bu\\
+y=Cx
+$$
+
+where:
+* $x\in	\mathbb{R}^n$: state vector
+* $u\in	\mathbb{R}^m$: control input vector 
+* $y\in	\mathbb{R}^p$: output vector
+* $A\in\mathbb{R}^{n\times n}$: state matrix (Unkonwn)
+* $B\in\mathbb{R}^{n\times m}$: input matrix (Partially known)
+* $C\in\mathbb{R}^{p\times n}$: output matrix (Known)
+
+
+
+Now, we define a reference model such that in open loop the interesting signal follows the reference:
+
+$$
+\dot{x}_m=A_mx_m+B_mr\\
+y=Cx_m
+$$
+
+* $r\in\mathbb{R}^p$: reference vector
+
+This model complies that:
+
+* $A_m=A+BK_x^*$
+* $B_m=BK_r^*$
+
+### Observer
+Since not all states are accessible, a Luenberger observer is created as:
+$$
+\dot{\hat{x}}=A\hat{x}+Bu+L(y-\hat{y})\\
+\hat{y}=C\hat{x}
+$$
+$$
+\dot{\hat{x}}=A\hat{x}+Bu+L(y-C\hat{x})\\
+\dot{\hat{x}}=A\hat{x}+Bu+Ly-LC\hat{x}\\
+\dot{\hat{x}}=(A-LC)\hat{x}+Bu+Ly
+$$
+
+* $BK_y^*=-L$
+Observer error:
+$$
+e_l=x-\hat{x}\\
+\dot{e}_l=\dot{x}-\dot{\hat{x}}\\
+\dot{e}_l=Ax+Bu-(A-LC)\hat{x}-Bu-Ly\\
+\dot{e}_l=Ax-(A-LC)\hat{x}-LCx\\
+\dot{e}_l=(A-LC)x-(A-LC)\hat{x}\\
+\dot{e}_l=(A-LC)e_l
+$$
+
+### Controller
+Using the next control law:
+$$
+u=K_x^*\hat{x}+K_r^*r+K_y^*(y-\hat{y})
+$$
+
+The state estimate is:
+$$
+\dot{\hat{x}}=(A_m-BK_x^*)\hat{x}+B(K_x\hat{x}+K_rr+K_y(y-\hat{y}))+L(y-\hat{y})
+$$
+
+
+### Error
+Defining the error as:
+$$
+e=x_m-\hat{x}\\
+\dot{e}=\dot{x}_m-\dot{\hat{x}}\\
+\dot{e}=A_mx_m+B_mr-A_m\hat{x}+BK_x^*\hat{x}-BK_x\hat{x}-BK_rr-BK_y(y-\hat{y})-L(y-\hat{y})\\
+\dot{e}=A_me+BK_r^*r+BK_x^*\hat{x}-BK_x\hat{x}-BK_rr-BK_y(y-\hat{y})+BK_y^*(y-\hat{y})
+$$
+
+Defining:
+* $\tilde{K}_x = K_x-K_x^*$
+* $\tilde{K}_r = K_r-K_r^*$
+* $\tilde{K}_y = K_y-K_y^*$
+
+$$
+\dot{e}=A_me-B\tilde{K}_rr-B\tilde{K}_x\hat{x}-B\tilde{K}_y(y-\hat{y})\\
+\dot{e}=A_me-B\tilde{K}_rr-B\tilde{K}_x\hat{x}-B\tilde{K}_yCe_l\\
+\dot{e}=A_me-B\left(\tilde{K}_rr+\tilde{K}_x\hat{x}+\tilde{K}_yCe_l\right)
+$$
+
+
+To find the $K$, we choice the following Lyapunov candidate:
+$$
+V = e^TPe + \text{trace}(\tilde{K}_x\Gamma_x^{-1}\tilde{K}^T_x) + \text{trace}(\tilde{K}_r\Gamma_r^{-1}\tilde{K}^T_r) + \text{trace}(\tilde{K}_y\Gamma_y^{-1}\tilde{K}^T_y) \\
+\dot{V} = e^TP\dot{e} + \dot{e}^TPe + 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y) \\
+\dot{V} = e^TP(A_me-BK)+(A_me-BK)^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\
+\dot{V} = e^TP(A_me-BK)+(e^TA_m^T-K^TB^T)Pe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\
+\dot{V} = e^TPA_me-e^TPBK+e^TA_m^TPe-K^TB^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\
+\dot{V} = e^T[PA_me+e^TA_m^TP]e-e^TPBK-K^TB^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\
+\dot{V} = e^T[PA_me+e^TA_m^TP]e-2e^TPB\left(\tilde{K}_rr+\tilde{K}_x\hat{x}+\tilde{K}_yCe_l\right)+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)
+$$
+
+* $e^TPB\tilde{K}_rr = \text{trace}(\tilde{K}_rre^TPB)$
+* $e^TPB\tilde{K}_x\hat{x} = \text{trace}(\tilde{K}_x\hat{x}e^TPB)$
+* $e^TPB\tilde{K}_yCe_l = \text{trace}(\tilde{K}_yCe_le^TPB)$
+
+$$
+\dot{V} = e^T[PA_me+e^TA_m^TP]e 
++ 2\text{trace}\left(\tilde{K}_x\left[-\hat{x}e^TPB+\Gamma_x^{-1}\dot{\tilde{K}}^T_x\right]\right)
++ 2\text{trace}\left(\tilde{K}_r\left[-re^TPB+\Gamma_r^{-1}\dot{\tilde{K}}^T_r\right]\right)
++ 2\text{trace}\left(\tilde{K}_y\left[-Ce_le^TPB+\Gamma_y^{-1}\dot{\tilde{K}}^T_y\right]\right)
+$$
+
+$$
+\tilde{K}_i = K_i-K_i^*\\
+\dot{\tilde{K}_i}=\dot{K}_i-\dot{K}_i^*\\
+\dot{\tilde{K}_i}=\dot{K}_i
+$$
+
+* $\dot{K}^T_x = \Gamma_x\hat{x}e^TPB$
+* $\dot{K}^T_r = \Gamma_rre^TPB$
+* $\dot{K}^T_y = \Gamma_yCe_le^TPB$
+
+
+$$ 
+
+$$

README.md

@@ -8,40 +8,33 @@ This repository contains the development of an aeropendulum and the performance
 
 $$
 \begin{gather*}
-    I\ddot{\theta}=W_cl_c\cos{\theta}+F_hl_m-W_bl_b\cos{\theta}-W_nl_n\cos{\theta}-B\dot{\theta}\\
-    \ddot{\theta}=\frac{1}{I}(W\cos{\theta}-B\dot{\theta}+F_hl_m)
+    I\ddot{\theta}=W_cl_c\cos{\theta}+F_hl_m-W_bl_b\cos{\theta}-W_nl_n\cos{\theta}-\beta\dot{\theta}\\
+    \ddot{\theta}=\frac{1}{I}(W\cos{\theta}-\beta\dot{\theta}+F_hl_m)
 \end{gather*}
 $$
 
 Where
-
 $$
 W=W_cl_c-W_bl_b-W_nl_n
 $$
 
 To simplify the inertia, we define the term $I_0$ as the system inertia without counterweight and the term $I$ as the system inertia with counterweight.
-
 $$
-I=I_0+\frac{W_c}{g}l_c^2\\
-I_0 = I_b+I_n
+I=I_0+m_cl_c^2
 $$
 
-To compute the bar inertia ($I_b$), we use the parallel axes theorem:
-
+Where $I_0 = I_b+I_n$, to compute the bar inertia ($I_b$), we use the parallel axes theorem:
 $$
 I_b = I_{b_{cm}} + m_bl_b^2\\
-I_b = \frac{1}{12}m_b(l_1+l_m)^2 + m_bl_b^2\\
-I_b = \frac{1}{12} \frac{W_b}{g} (l_1 + l_m)^2 + \frac{W_b}{g}l_b^2
+I_b = \frac{1}{12}m_b(l_1+l_m)^2 + m_bl_b^2
 $$
 
 And the motor+nuts inertia ($I_n$) is considered as a puntual load, i.e.
-
 $$
-I_n = m_nl_n^2\\
-I_n = \frac{W_n}{g} l_n^2
+I_n = m_nl_n^2
 $$
 
-#### State-space model
+## State-space model
 To represent the state-space model of the system, we define the next state equations:
 
 $$
@@ -53,7 +46,7 @@ Therefore:
 
 $$
 \dot{x}_1 = x_2\\
-\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-Bx_2+F_hl_m)
+\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-\beta x_2+F_hl_m)
 $$
 
 ### Linearization
@@ -77,7 +70,7 @@ $$
 $$
 \dot{Z}=\left.
 \begin{bmatrix}
-    \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\
+\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\
     \frac{\partial f_2}{\partial x_1} & \frac{\partial f_1}{\partial x_2} 
 \end{bmatrix}\right|_{X^*,F^*}
 Z+\left.
@@ -99,26 +92,53 @@ $$
 To $x_1$:
 $$
 f_2(x_1^*,x_2^*,F^*)=\dot{x}_2=0\\
-\frac{1}{I}(W\cos{x_1^*}-Bx_2^*+F^*l_m)=0\\
-W\cos{x_1^*}-Bx_2^*+F^*l_m=0\\
-F^*=\frac{Bx_2^*-W\cos{x_1^*}}{l_m}\\
+\frac{1}{I}(W\cos{x_1^*}-\beta x_2^*+F^*l_m)=0\\
+W\cos{x_1^*}-\beta x_2^*+F^*l_m=0\\
+F^*=\frac{\beta x_2^*-W\cos{x_1^*}}{l_m}\\
 \mathbf{F^*=-\frac{W\cos{x_1^*}}{l_m}}
 $$ 
 
-If you want $x_1^*=0$, the thrust force is $F^*=-\frac{W}{l_m}$.
+
 
 Finally:
 $$
 \dot{Z}=\left.
 \begin{bmatrix}
     0 & 1\\
-    -\frac{W}{I}\sin{x_1} & -\frac{B}{I}
+    -\frac{W}{I}\sin{x_1} & -\frac{\beta}{I}
 \end{bmatrix}\right|_{X^*,F^*}
 Z+\left.
 \begin{bmatrix}
     0\\
     \frac{l_m}{I} 
 \end{bmatrix}\right|_{X^*,F^*}
+u\\
+\dot{Z}=
+\begin{bmatrix}
+    0 & 1\\
+    -\frac{W}{I}\sin{x_1^*} & -\frac{\beta}{I}
+\end{bmatrix}
+Z+
+\begin{bmatrix}
+    0\\
+    \frac{l_m}{I} 
+\end{bmatrix}
+u
+$$
+
+If you want $x_1^*=0$, the thrust force is $F^*=-\frac{W}{l_m}$ and:
+
+$$
+\dot{Z}=
+\begin{bmatrix}
+    0 & 1\\
+    0 & -\frac{\beta}{I}
+\end{bmatrix}
+Z+
+\begin{bmatrix}
+    0\\
+    \frac{l_m}{I} 
+\end{bmatrix}
 u
 $$
 
@@ -140,7 +160,15 @@ To find the $B$ and $F_h$ parameters, we use the following methods:
 1. **Thrust force ($F_h$)** <br>
     The objective of this identification is to find the thrust force $F_h$ that depends of voltage. Therefore, we take measurements of the voltage locating the system in an angle $0$ with different counterweights. With this, we can calculate the thrust force $F_h$ that corresponds to the voltage $V$ in the system making a torque summation.
 
+    If we have:
+    $$
+    B=
+    \begin{bmatrix}
+    0\\
+    K_f\frac{l_m}{I}
+    \end{bmatrix}
+    $$
+
 
 
 
-## MRAC(Model Reference Adaptive Control)

SMC.md

@@ -0,0 +1,51 @@
+# SMC   
+DEfine the next states:
+$$
+\dot{x}_1 = x_2\\
+\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-\beta x_2+F_hl_m)
+$$
+
+Can be written as:
+$$
+\dot{x}_1 = x_2\\
+\dot{x}_2 = \frac{1}{I}[W\cos{x_1}-\beta x_2+(K_f\cdot v+b_f)l_m]
+$$
+
+* $h(x) = \frac{1}{I}(W\cos{x_1}-\beta x_2+b_fl_m)$
+* $f(x) = \frac{1}{I}(K_fl_m)$
+
+Now, we define an error:
+
+$$
+x_e=x_1-x_r\\
+\dot{x}_e=\dot{x}_1
+$$
+
+and the next sliding manifold:
+$$
+S = ax_e+x_2
+$$
+
+where its derivative is:
+$$
+\dot{S} = a\dot{x}_e+\dot{x}_2\\
+\dot{S} = a\dot{x}_1+h(x)+f(x)v
+$$
+
+and the signal control law is:
+$$
+v=-\rho\ \text{sign}(S)
+$$
+
+To the system stability, we need to define the following:
+$$
+\rho \geq \left|\frac{ax_2+h(x)}{f(x)}\right|\\
+\rho \geq \left|\frac{ax_2+\frac{1}{I}(W\cos{x_1}-\beta x_2+b_fl_m)}{\frac{1}{I}(K_fl_m)}\right|
+$$
+
+Taken as limits $x_2\leq\pi$ and, we get:
+
+$$
+\rho \geq \frac{a\max{x_2}+\frac{1}{\min{I}}(\max{W}+\max{\beta}\max{x_2}+\max{b_fl_m})}{\frac{1}{\max{I}}\min{K_fl_m}}\\
+\rho \geq \frac{a\pi+\frac{1}{I_0}(\max{W}-\max{\beta}\pi+\max{b_fl_m})}{\frac{1}{I}\min{K_fl_m}}
+$$

test.py

@@ -0,0 +1,15 @@
+from machine import Pin
+import time
+import numpy as np
+
+p2 = Pin(2, Pin.OUT)
+count = 0
+
+while True:
+    count+= 1
+    p2.on()
+    time.sleep(1)
+    p2.off()
+    time.sleep(1)
+
+    print(count)