Control.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Diseño del controlador\n",
    "\n",
    "Como se indicó en el apartado anterior, las ecuaciones de nuestro sistema tienen la siguiente forma:\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "    \\dot{\\mathbf x} &= \\mathbf A\\mathbf x + \\mathbf Bu \\\\\n",
    "    \\mathbf y &= \\mathbf C\\mathbf x + \\mathbf Du\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "En la bibliografía sobre Control es frecuente referirse al sistema que queremos controlar, de modo general, como *planta*. Cuando no hay realimentación, el sistema se conoce como *planta en lazo abierto*. Si consideramos el sistema libre, sin introducir ningún *input*, resultará una ecuación diferencial de la forma:\n",
    "\n",
    "$$\n",
    "    \\dot{\\mathbf x} = \\mathbf A\\mathbf x\n",
    "$$\n",
    "\n",
    "Se dice que un sistema es *asintóticamente estable* cuando, para unas condiciones iniciales cualesquiera, el vector de estados $\\mathbf x$ tiende a cero cuando el tiempo tiende a infinito. Se puede demostrar que la solución de un sistema de este tipo es estable cuando todos los autovalores de $\\mathbf A$ tienen parte real negativa. Como se puede ver, nuestro sistema no es estable, ya que tiene un autovalor positivo:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# Cargamos las matrices A, B, C, y D del paso anterior\n",
    "# Aprovechamos para cargar las dimensiones del modelo, para las animaciones\n",
    "with np.load('./Data/Model.npz') as m:\n",
    "    A, B, C, D = (m['A'], m['B'], m['C'], m['D'])\n",
    "    L, R = (5*m['r'], m['r'])\n",
    "\n",
    "# Comprobamos los autovalores de 'A'\n",
    "display(np.real(np.linalg.eig(A)[0]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Estabilización mediante realimentación de estados\n",
    "\n",
    "La realimentación de estados se basa en definir la entrada del sistema $u$ como una realimentación negativa del estado, de la siguiente forma:\n",
    "\n",
    "$$\n",
    "    u = -\\mathbf K\\mathbf x\n",
    "$$\n",
    "\n",
    "Con este tipo de realimentación, se puede determinar de forma analítica una matriz de ganancias $\\mathbf K$ que estabilice el sistema, es decir, que haga que todos los autovalores sean negativos. Para verlo más claramente, se puede introducir la realimentación en las ecuaciones del sistema:\n",
    "\n",
    "$$\n",
    "    \\dot{\\mathbf x} = \\mathbf{Ax} - \\mathbf{BKx} = \\left(\\mathbf A - \\mathbf{BK}\\right)\\mathbf x\n",
    "$$\n",
    "\n",
    "Se observa que, ahora, la estabilidad del sistema la determinan los autovalores de $\\mathbf A - \\mathbf{BK}$. Esto significa que se puede alterar la dinámica del sistema a conveniencia, ajustando la matriz $\\mathbf K$ para colocar los autovalores donde nos interese. Este método de control se puede esquematizar de la forma siguiente:\n",
    "\n",
    "![Diagrama de bloques](./Images/figure-01.png)\n",
    "\n",
    "En la figura se muestra un sistema con una entrada y una salida, es decir, un sistema SISO (*Single Input, Single Output*), pero todo lo aquí desarrollado se puede generalizar fácilemente a sistemas con varias entradas y salidas. Por ahora no vamos a considerar la entrada de referencia $r$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Regulador cuadrático lineal (LQR)\n",
    "\n",
    "El esquema de control de realimentación de estados deja abierta una decisión complicada al diseñador: ¿dónde colocar los autovalores del sistema? Existen diversos métodos descritos en la bibliografía, dedicados a determinar dónde se deben colocar los autovalores para conseguir que la respuesta del sistema presente unas características determinadas. Estas técnicas se denominan *asignación de polos* o *pole placement*.\n",
    "\n",
    "En nuestro proyecto, vamos a utilizar un método un poco más elaborado, pero a la vez sencillo de utilizar: el LQR (*Linear Quadratic Regulator*). El LQR no es más que otro método para calcular la matriz $\\mathbf K$, pero en lugar de tener que decidir dónde colocamos los polos del sistema, lo que hacemos es buscar el valor único de $\\mathbf K$ que minimiza la siguiente función:\n",
    "\n",
    "$$\n",
    "    J = \\int_0^\\infty\\left(\\mathbf x^T\\mathbf Q\\mathbf x + \\mathbf u^T\\mathbf R\\mathbf u\\right)dt\n",
    "$$\n",
    "\n",
    "Esta función (expresada en forma general, para un sistema con múltiples *inupts*) combina los valores cuadráticos, integrados a lo largo del tiempo, de las dos magnitudes que queremos minimizar: el error en los estados y la magnitud de las entradas de control. Cada término va ponderado por una matriz de pesos, $\\mathbf Q$ para los estados y $\\mathbf R$ para los *inputs*, de modo que podemos decidir qué variables queremos penalizar más en el controlador. Por ejemplo, asignar un valor elevado al elemento $Q_{ii}$ de la diagonal de $\\mathbf Q$ hará que los errores en el estado $x_i$ sean fuertemente penalizados en la función objetivo $J$, así que el controlador se esforzará en manternerlos bajos. Por el contrario, si se hace lo mismo con el elemento $R_{jj}$ de $\\mathbf R$, se indica al controlador que debe utilizar lo menos posible el actuador correspondiente al *input* $u_j$.\n",
    "\n",
    "Modificando los elementos de las matrices $\\mathbf Q$ y $\\mathbf R$ se puede ajustar el comportamiento del controlador a nuestros requerimientos, dando mayor relevancia al error en los estados o al esfuerzo de control. Lo primero puede ser importante si en algún estado es crítico mantener errores bajos, y lo segundo, por ejemplo, si es prioritario obtener una alta eficiencia energética, aunque sea al coste de menor precisión en el control.\n",
    "\n",
    "Una vez determinadas las matrices de pesos, la función `lqr` del módulo de control de Python resuelve el problema de optimización y nos devuelve directamente la matriz de ganancias $\\mathbf K$ que minimiza la función $J$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from control import ss, lqr, ss2tf, minreal, forced_response\n",
    "\n",
    "# Construimos un sistema state-space con las matrices A, B, C y D\n",
    "sys = ss(A, B, C, D)\n",
    "\n",
    "Qc = np.eye(3) # Aproximación incial para 'Q'\n",
    "Qc[2, 2] = 200 # Ajustamos peso de la velociad 'xp'\n",
    "Rc = 1         # Peso del voltaje 'V'\n",
    "\n",
    "K = lqr(sys, Qc, Rc)[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para evaluar el comportamiento del regulador, se simula el comportamiento del sistema cuando partimos de una posición fuera de equilibrio, con un ángulo incial $\\theta_0$ de 30º. La entrada de referencia $r$ que se ve en la figura se dejará a cero (se explicará más adelante para qué sirve esta entrada). Como ejercicio, es interesante comprobar cómo varía la repuesta al modificar los pesos del LQR:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%%capture\n",
    "from matplotlib.animation import FuncAnimation\n",
    "import plotfunctions as pf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Ensamblamos el sistema en lazo cerrado\n",
    "clsys = ss(A - B*K, B, C, D)\n",
    "\n",
    "tf = 3.00         # Duración de la simulación en segundos\n",
    "dt = 0.01         # Paso de tiempo de simulación en segundos\n",
    "\n",
    "# Simulamos respuesta para ángulo incial de 30 grados\n",
    "t = np.arange(0, tf, dt)\n",
    "r = np.zeros(t.shape)\n",
    "\n",
    "T, Y, X = forced_response(clsys, t, r, X0=[np.radians(30), 0, 0], return_x=True)\n",
    "\n",
    "pf.plot_response(T, X)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Como se ve, el sistema es estable, convergiendo rápidamente tanto $\\theta$ como $\\dot x$ a cero.\n",
    "\n",
    "Podemos verificar que, al aplicar la realimentación, todos los autovalores del sistema tienen ahora parte real negativa:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(np.real(np.linalg.eig(A - B*K)[0]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A continuación se muestra una animación con el movimiento resultante:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(X[[0, 2]], dt, R, L), interval=dt*1000, blit=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Control de la velocidad\n",
    "\n",
    "Cuando se aplica realimentación de estados, el controlador resultante siempre tratará de llevarlos todos al equilibrio, y eso es lo que se conoce como *regulador*. Hasta ahora, esto nos ha servido para estabilizar el sistema, manteniéndolo equilibrado en vertical y con velocidad cero. Si lo que queremos es asignar a algún *output* un valor arbitrario, hay que convertir nuestro regulador en un *servomecanismo*. El objetivo ahora es conseguir que alguno de los *outputs* (en nuestro caso la velocidad $\\dot x$) siga a un valor de referencia $r$, en lugar de converger hacia cero.\n",
    "\n",
    "Antes de explicar cómo se consigue esto, es importante introducir un nuevo concepto: el *tipo* de sistema o planta. Por definición, una planta es de tipo $N$ cuando, al representarla como un sistema con realimentación unitaria (como el que se muestra en la figura), el término de grado más bajo del denominador de la función de transferencia $G$ es de grado $N$.\n",
    "\n",
    "![Realimentación unitaria](./Images/figure-04.png)\n",
    "\n",
    "Es decir, si la función de transferencia en lazo abierto $G$ se escribe en la forma:\n",
    "\n",
    "$$\n",
    "    G(s) = \\frac{K\\left(T_as + 1\\right)\\left(T_bs + 1\\right)\\cdots\\left(T_ms + 1\\right)}\n",
    "    {s^N\\left(T_1s + 1\\right)\\left(T_2s + 1\\right)\\cdots\\left(T_ps + 1\\right)}\n",
    "$$\n",
    "\n",
    "el tipo de sistema corresponderá al exponente $N$ que aparece en el denominador. El tipo de un sistema determina su error de seguimiento $e$ en régimen permanente para distintos tipos de entrada. Por ejemplo, en un sistema tipo 1, el error tenderá a cero para una entrada escalón, a un valor finito para una entrada rampa, y a infinito para una entrada cuadrática o de orden superior. En cambio, un sistema tipo 0 ya mostrará un error finito en régimen permanente para una entrada escalón, y será inestable frente a una entrada rampa o superior.\n",
    "\n",
    "Para saber el tipo de nuestro sistema, primero tenemos que representarlo según la estructura de la figura. La función de transferencia $H$ del sistema completo, entre la entrada $r$ y la salida $y$, se puede obtener sustituyendo el error de seguimiento $e$ por su valor $r - y$:\n",
    "\n",
    "$$\n",
    "    y = G\\left(r - y\\right) \\implies H(s) = \\frac{y}{r} = \\frac{G}{1 + G}\n",
    "$$\n",
    "\n",
    "Entonces, se puede hacer el cambio inverso para expresar cualquier sistema con función de transferencia $H$ como sistema con realimentación unitaria:\n",
    "\n",
    "$$\n",
    "    G(s) = \\frac{H}{1 - H}\n",
    "$$\n",
    "\n",
    "Comprobamos el tipo de nuestro sistema, incluyendo la realimentación, cuando consideramos que nuestro único *output* es la velocidad. La función `ss2tf` devuelve la función de transferencia $H$ de un sistema *state-space*, y `minreal` (*minimal realization*) cancela los factores comunes del numerador y el denominador:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Matrices de salida si el output es sólo la velocidad\n",
    "Cc = C[1:]\n",
    "Dc = D[1:]\n",
    "\n",
    "# Función de transferencia del sistema con realimentación\n",
    "H = ss2tf(ss(A - B*K, B, Cc, Dc))\n",
    "\n",
    "# Calculamos G para transformar en sistema con realimentación unitaria\n",
    "minreal(H/(1 - H))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Mirando el denominador, se ve claramente que nuestro sistema realimentado es de tipo 0. Eso significa que, para una entrada escalón, la salida $\\dot x$ nunca alcanzará el valor de referencia $r$ en régimen permanente. Si queremos controlar la velocidad correctamente, tendremos que convertir el sistema en uno de tipo 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Servomecanismo tipo 1 para controlar la velocidad\n",
    "\n",
    "Existen varios métodos para convertir un sistema de tipo 0 en uno de tipo 1. Lo más sencillo es escalar la entrada de referencia $r$ mediante una ganancia de precompensación $\\bar N$, que haga que el *output* que queremos controlar (en nuestro caso la velocidad $\\dot x$) alcance el valor solicitado en régimen permanente. En la figura se muestra un diagrama del sistema original, con la precompensación aplicada en la entrada:\n",
    "\n",
    "![Diagrama con precompensación](./Images/figure-02.png)\n",
    "\n",
    "Al añadir la precompensación, la entrada del sistema en lazo abierto pasa a ser:\n",
    "\n",
    "$$\n",
    "    u = -\\mathbf{Kx} + \\bar Nr\n",
    "$$\n",
    "\n",
    "Vamos a estudiar lo que ocurre con $\\mathbf x$ y $u$ cuando tenemos una entrada de referencia $r$ de tipo escalón unitario. Supondremos que, en régimen permanente, la salida $y$ alcanza efectivamente el valor de referencia (es decir, la unidad). Como en régimen estacionario las derivadas de los estados $\\dot{\\mathbf x}$ serán nulas, podemos escribir:\n",
    "\n",
    "$$\n",
    "    \\begin{bmatrix} \\mathbf 0 \\\\ 1 \\end{bmatrix} = \n",
    "    \\begin{bmatrix} \\mathbf A && \\mathbf B \\\\ \\mathbf C && D \\end{bmatrix}\n",
    "    \\begin{bmatrix} \\mathbf{x_\\infty} \\\\ u_\\infty \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Si la matriz de este sistema lineal es invertible, resolviéndolo obtendremos el valor de $\\mathbf x_\\infty$ y $u_\\infty$. Como la referencia $r$ es un escalón unitario, la entrada del sistema en lazo abierto alcanzará el siguiente valor:\n",
    "\n",
    "$$\n",
    "    u_\\infty = -\\mathbf{Kx}_\\infty + \\bar N\n",
    "$$\n",
    "\n",
    "de donde obtenemos $\\bar N$ como:\n",
    "\n",
    "$$\n",
    "    \\bar N = \\mathbf{Kx}_\\infty + u_\\infty\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Precompensación para salida 'xp' en régimen permanente\n",
    "xuinf = np.linalg.solve(np.block([[A, B], [Cc, Dc]]), [0, 0, 0, 1])\n",
    "Nb = K@xuinf[:-1] + xuinf[-1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Antes de simular la respuesta, podemos montar el sistema en lazo cerrado con precompensación en la entrada, y comprobar su tipo:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Sistema en lazo cerrado, incluyendo la precompensación\n",
    "clsys = ss(A - B*K, B*Nb, Cc, Dc)\n",
    "\n",
    "# Función de transferencia del sistema con realimentación\n",
    "H = ss2tf(clsys)\n",
    "\n",
    "# Calculamos G(s) para sistema con realimentación unitaria\n",
    "minreal(H/(1 - H))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Vemos que el sistema se puede considerar prácticamente de tipo 1, ya que el término de grado 0 del denominador es muy pequeño.\n",
    "\n",
    "Para evaluar el comportamiento del controlador, se simulará su respuesta a una entrada escalón de 0.3 m/s seguida de una parada:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simulamos respuesta a escalón de 0.3 m/s con parada\n",
    "t = np.arange(0, tf, dt)\n",
    "r = np.zeros(len(t))\n",
    "r[t < 1.5] = 0.3\n",
    "\n",
    "T, Y, X = forced_response(clsys, t, r, return_x=True)\n",
    "\n",
    "pf.plot_response(T, X)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Se observa que el sistema alcanza efectivamente la velocidad objetivo y luego se para, manteniendo siempre la barra en equilibrio.\n",
    "\n",
    "Si este controlador se va a implementar en el robot real, hay que verificar que el voltaje no supere los 6V:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.plot_voltage(T, (Nb*r - K@X).T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Vemos que el sistema puede realizar la maniobra solicitada sin saturar el motor. En caso contrario, habría que variar los pesos del LQR, o realizar una maniobra más suave. A continuación se muestra una animación del movimiento obtenido:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(X[[0, 2]], dt, R, L), interval=dt*1000, blit=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Los parámetros obtenidos ya sirven para controlar el robot en velocidad. Los valores mostrados más abajo se pueden copiar y pegar directamente en un *sketch* de Arduino, de forma que, para implementar el controlador, sólo habría que calcular el voltaje como $V = -\\mathbf{Kx} + \\bar Nr$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('const float K1 =%10.6f;  // Ganancia theta'    % K[0, 0])\n",
    "print('const float K2 =%10.6f;  // Ganancia thetap'   % K[0, 1])\n",
    "print('const float K3 =%10.6f;  // Ganancia xp'       % K[0, 2])\n",
    "print('const float Nb =%10.6f;  // Precompensación'   % Nb[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Una de las principales desventajas de los métodos de realimentación de estados es que, para calcular $u$, necesitamos conocer el valor de todos los estados. Esto es un problema, porque normalmente no todos los estados se pueden medir fácilmente con precisión. En estos casos, lo que se hace es utilizar un *observador*, que consiste en un modelo del sistema que se va integrando en el tiempo, corrigiendo los errores de integración a partir de los datos de los sensores.\n",
    "\n",
    "En este proyecto hay dos estados, $\\dot\\theta$ y $\\dot x$, que son muy fáciles de medir, ya que se obtienen a partir del giróscopo y los *encoders*, pero el ángulo $\\theta$ no se puede medir directamente con los sensores que tenemos. Como el uso de observadores embebidos en el controlador queda fuera del ámbito de este proyecto, estimaremos $\\theta$ utilizando un filtro de Kalman simplificado, como se explica en el [documento correspondiente](./Kalman.ipynb)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Servomecanismo tipo 1 con integrador en la entrada\n",
    "\n",
    "El problema de usar precompensación es que la respuesta es muy sensible a errores en los parámetros del sistema. Cualquier desviación en el valor de $\\bar N$ hará que el sistema vuelva a ser de tipo 0, y por lo tanto tenga error en régimen permanente. La manera de resolver esto de forma más robusta es añadir a la entrada un integrador, en lugar de una simple ganancia:\n",
    "\n",
    "![Diagrama con integral](./Images/figure-03.png)\n",
    "\n",
    "Para utilizar este esquema de control, hay que aumentar el sistema añadiéndole un estado adicional $\\xi$, que será la integral del error de seguimiento $r - y$. Por lo tanto, su derivada es:\n",
    "\n",
    "$$\n",
    "    \\dot\\xi = r - y = r - \\left(\\mathbf{Cx} + Du\\right)\n",
    "$$\n",
    "\n",
    "y la entrada $u$ pasa a ser ahora:\n",
    "\n",
    "$$\n",
    "    u = -\\mathbf K \\mathbf x + k_I\\xi\n",
    "$$\n",
    "\n",
    "La ecuación completa de la dinámica del sistema aumentado, sin incluir la realimentación, se puede escribir como:\n",
    "\n",
    "$$\n",
    "    \\begin{bmatrix} \\dot{\\mathbf x} \\\\ \\dot\\xi \\end{bmatrix} = \n",
    "    \\begin{bmatrix} \\mathbf A && \\mathbf 0 \\\\ -\\mathbf C && 0 \\end{bmatrix}\n",
    "    \\begin{bmatrix} \\mathbf{x} \\\\ \\xi\\end{bmatrix} + \n",
    "    \\begin{bmatrix} \\mathbf B \\\\ -D \\end{bmatrix}u + \n",
    "    \\begin{bmatrix} \\mathbf 0 \\\\ 1 \\end{bmatrix}r\n",
    "$$\n",
    "\n",
    "Estudiaremos la respuesta de este sistema a una entrada escalón, cuando el tiempo tiende a infinito. Para ello, definiremos las siguientes magnitudes:\n",
    "\n",
    "$$\n",
    "    \\mathbf e = \\begin{bmatrix} \\mathbf{x} - \\mathbf{x}_\\infty \\\\ \\xi - \\xi_\\infty \\end{bmatrix} \\qquad\n",
    "    u_e = u - u_\\infty\n",
    "$$\n",
    "\n",
    "que representan las desviaciones del estado y la entrada respecto sus propios valores en régimen permanente. Como $r$ es un escalón, su valor es siempre el mismo para $t>0$, de forma que podemos escribir la dinámica del error como:\n",
    "\n",
    "$$\n",
    "    \\dot{\\mathbf e} = \n",
    "    \\begin{bmatrix} \\mathbf A && \\mathbf 0 \\\\ -\\mathbf C && 0 \\end{bmatrix}\n",
    "    \\mathbf e + \n",
    "    \\begin{bmatrix} \\mathbf B \\\\ -D \\end{bmatrix}u_e = \\hat{\\mathbf A}\\mathbf e + \\hat{\\mathbf B}u_e\n",
    "$$\n",
    "\n",
    "Se puede observar que, de forma análoga a lo que ocurría al principio, tenemos un sistema en forma *state-space* cuyos estados tienen que converger asintóticamente a cero. Eso quiere decir que existirán unas ganancias de realimentación $\\hat{\\mathbf K}$ que estabilicen el sistema:\n",
    "\n",
    "$$\n",
    "    \\hat{\\mathbf K} = \\begin{bmatrix} \\mathbf K && -k_I \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Estas ganancias se pueden calcular, como antes, utilizando el algoritmo LQR, sabiendo que ahora tenemos cuatro estados, ya que el último corresponde a la integral del error de seguimiento en la velocidad:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Ecuaciones del sistema con los errores como estados\n",
    "# Se añade la integral del error en velocidad como cuarto estado\n",
    "Ah = np.block([[A, np.zeros((3, 1))], [-Cc, 0]])\n",
    "Bh = np.block([[B], [-Dc]])\n",
    "\n",
    "# Controlador LQR para la dinámica del error\n",
    "Qc = np.eye(4)   # Aproximación inicial para 'Q'\n",
    "Qc[0, 0] = 0.1   # Ajustamos peso del ángulo 'th'\n",
    "Qc[1, 1] = 0.01  # Ajustamos peso de la derivada del ángulo 'thp'\n",
    "Qc[2, 2] = 2     # Ajustamos peso de la velocidad 'xp'\n",
    "Qc[3, 3] = 500   # Peso de la integral del error en velocidad 'xi'\n",
    "Rc = 1           # Peso del voltaje 'V'\n",
    "\n",
    "# La función 'LQR' también admite usar sólo las matrices A y B\n",
    "Kh = lqr(Ah, Bh, Qc, Rc)[0]\n",
    "Kn = Kh[0, :-1]\n",
    "Ki = -Kh[0, -1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para simular el sistema, utilizaremos un modelo aumentado de la siguiente forma, que se obtiene al sustituir en la ecuación del sistema el valor de $u$:\n",
    "\n",
    "$$\n",
    "    \\begin{bmatrix} \\dot{\\mathbf x} \\\\ \\dot\\xi \\end{bmatrix} = \n",
    "    \\begin{bmatrix} \\mathbf A - \\mathbf{BK} && \\mathbf Bk_I \\\\ D\\mathbf K - \\mathbf C && -Dk_I \\end{bmatrix}\n",
    "    \\begin{bmatrix} \\mathbf{x} \\\\ \\xi\\end{bmatrix} + \n",
    "    \\begin{bmatrix} \\mathbf 0 \\\\ 1 \\end{bmatrix}r\n",
    "$$\n",
    "\n",
    "Antes de simular, verificamos que este sistema es de tipo 1:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Sistema en lazo cerrado con las ganancias obtenidas\n",
    "An = np.block([[A - B*Kn, B*Ki], [Dc*Kn - Cc, -Dc*Ki]])\n",
    "Bn = [[0], [0], [0], [1]]\n",
    "Cn = [0, 0, 1, 0]\n",
    "Dn = 0\n",
    "\n",
    "clisys = ss(An, Bn, Cn, Dn)\n",
    "\n",
    "# Función de transferencia del sistema con realimentación\n",
    "H = ss2tf(clisys)\n",
    "\n",
    "# Calculamos G(s) para sistema con realimentación unitaria\n",
    "minreal(H/(1 - H))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para comprobar que este método es mucho más robusto que la precompensación, se puede hacer una prueba sencilla. En el modelo con precompensación, si antes de ensamblar el sistema *state-space* se simula un error en los datos del modelo multiplicando $\\bar N$ por un factor, por ejemplo 0.9, se verá que el sistema pasa a ser tipo 0, incluso con valores del factor muy cercanos a la unidad. En cambio, si se hace lo mismo con $k_I$, se observa que el término de grado 0 del denominador se mantiene cercano a cero para cualquier valor de $k_I$.\n",
    "\n",
    "A continuación se muestra gráficamente la respuesta, comparándola con la obtenida usando precompensación (en línea de puntos). Igual que antes, es interesante probar diferentes pesos en el LQR, para ver cómo varía el comportamiento del sistema."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Respuesta con misma entrada que en el modelo con precompensación\n",
    "T, Y, Xn = forced_response(clisys, t, r, return_x=True)\n",
    "\n",
    "pf.plot_response(T, Xn, X)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Antes de implementar el controlador, hay que asegurarse de que no estamos saturando el actuador:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.plot_voltage(T, (Ki*Xn[3, :] - Kn@Xn[:3, :]).T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Vemos que los voltajes se mantienen por debajo de los 6V. Por último, visualizamos el movimiento:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FuncAnimation(pf.fg1, pf.anim, init_func=pf.init, frames=len(T), fargs=(Xn[[0, 2]], dt, R, L), interval=dt*1000, blit=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Como todo es correcto, ya podemos utilizar las ganancias para la implementación física del controlador. Se pueden copiar y pegar directamente en un sketch de Arduino, siendo ahora el voltaje $V= -\\mathbf{Kx} + k_I\\xi$. Esto significa que, además de los estados anteriores, habrá que ir calculando la integral del error en velocidad."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('const float K1 =%10.6f;  // Ganancia theta'    % Kn[0])\n",
    "print('const float K2 =%10.6f;  // Ganancia thetap'   % Kn[1])\n",
    "print('const float K3 =%10.6f;  // Ganancia xp'       % Kn[2])\n",
    "print('const float Ki =%10.6f;  // Ganancia integral' % Ki)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}