$\newcommand{\mb}[1]{\mathbf{ #1 }}$
$\newcommand{\bs}[1]{\boldsymbol{ #1 }}$
$\newcommand{\bb}[1]{\mathbb{ #1 }}$

$\newcommand{\R}{\bb{R}}$

$\newcommand{\ip}[2]{\left\langle #1, #2 \right\rangle}$
$\newcommand{\norm}[1]{\left\Vert #1 \right\Vert}$

$\newcommand{\der}[2]{\frac{\mathrm{d} #1 }{\mathrm{d} #2 }}$
$\newcommand{\derp}[2]{\frac{\partial #1 }{\partial #2 }}$

# Finite Dimensional Koopman Bilinear System

Consider a nonlinear dynamical system that allows an exact finite dimensional Koopman canonical transform such that the control-affine dynamics can be transformed to a bilinear system. Consider the dynamical system 

\begin{equation}
\mb{\dot{x}}=\mb{f}_0(\mb x) + \mb f_1 ( \mb x) u_1 + \mb f_2(\mb x) u_2, 
\end{equation}

where we for this example choose $\mb f_0, \mb f_1$ as follows:

\begin{equation}
\mb f_0(\mb x) = \begin{bmatrix} x_3 \\ x_4 \\ \lambda x_3 \\ \mu x_4 + (2 \lambda - \mu) c x_3^2 \end{bmatrix}, \qquad
\mb f_1(\mb x) = \begin{bmatrix} 0 \\ 0 \\ 1 \\ 0 \end{bmatrix}, \qquad
\mb f_2(\mb x) = \begin{bmatrix} 0 \\ 0 \\ 0 \\ x_1+1 \end{bmatrix}, 
\end{equation}   

and $\lambda, \mu, c \in \mathbb{R}$ are scalar parameters of the system. Setting $ \mb x = [q_1 \, q_2 \, \dot{q_1} \, \dot{q_2}]^T$, 
these equations of motion can be expressed as robotic dynamics of the form $\mb{D}(\mb{q})\ddot{\mb{q}} + \mb{C}(\mb{q}, \dot{\mb{q}})\dot{\mb{q}} + \mb{G}(\mb{q}) = \mb{B}\mb{u}$, where $\mb D$ is the inertia matrix, $\mb C$ is the matrix of Coriolis terms, $\mb G$ is the matrix of gravitational terms, and $\mb B$ is the static actuation matrix. Rewriting $\mb f_0, \mb f_1, \mb f_2$ in terms of $\mb D, \mb C, \mb G,$ and $\mb B$ yield


\begin{equation}
\mb D(\mb q) = \begin{bmatrix} 1 & 0\\ 0 &  \frac{1}{q_1+1} \end{bmatrix}, 
\qquad \mb C(\mb q, \mb{\dot{q}}) = -\begin{bmatrix} \lambda & 0 \\ \frac{1}{q_1 + 1}(2 \lambda - \mu) c \dot{q}_1 & \frac{1}{q_1 +1} \mu \end{bmatrix}, \qquad 
\mb G(\mb q) = \begin{bmatrix} 0 \\ 0 \end{bmatrix} 
\qquad \mb B = \begin{bmatrix}1 & 0 \\ 0 & 1 \end{bmatrix},
\end{equation}


```python
import numpy as np
from core.dynamics import RoboticDynamics

class FiniteDimKoopSys(RoboticDynamics):
    def __init__(self, lambd, mu, c):
        RoboticDynamics.__init__(self, 2, 2)
        self.params = lambd, mu, c
    
    def D(self, q):
        return np.array([[1, 0],[0, (q[0]+1)**(-1)]])
    
    def C(self, q, q_dot):
        labmd, mu, c = self.params
        return -np.array([[lambd, 0], [(q[0]+1)**(-1)*(2*lambd - mu)*c*q_dot[0], (q[0]+1)**(-1)*mu]])
    
    def G(self, q):
        return np.array([0, 0])
    
    def B(self, q):
        return np.array([[1, 0], [0, 1]])

n, m = 4, 2
lambd, mu, c = .3, .2, -.5
finite_dim_koop_sys = FiniteDimKoopSys(lambd, mu, c)
```

### Design trajectory using MPC based on the linearized model


```python
import scipy as sc
from koopman_core.controllers import MPCController
from koopman_core.learning.utils import differentiate_vec
from koopman_core.dynamics import LinearLiftedDynamics

A_lin = np.array([[0, 0, 1, 0],
                  [0, 0, 0, 1],
                  [0, 0, lambd, 0],
                  [0, 0, 0, mu]])
B_lin = np.array([[0, 0],
                  [0, 0],
                  [1, 0],
                  [0, 1]])
dt = 1e-2
linearized_sys = LinearLiftedDynamics(A_lin, B_lin, np.eye(n), lambda x: x)

# MPC parameters:
umax = np.array([100., 100.])
umin = -umax
xmax = np.array([10., 10., 10., 10.])
xmin = -xmax
q, r = 1e5, 1
Q_mpc = q*np.eye(n)
R_mpc = r*np.eye(m)
traj_length = 200

# Design trajectory:
x_0 = np.array([0., 0., 0., 0.])
set_pt = np.array([2., 5., 0., 0.])
mpc_controller_trajgen = MPCController(linearized_sys, traj_length,dt,umin,umax,xmin,xmax,np.zeros((n,n)),R_mpc,Q_mpc,set_pt)
mpc_controller_trajgen.eval(x_0, 0)
xr = mpc_controller_trajgen.parse_result()[:,:-1]
ur = mpc_controller_trajgen.get_control_prediction()

# Calculate outputs:
k = 2
xr_dot = linearized_sys.eval_dot(xr, ur, 0.)
xr_ddot = differentiate_vec(xr_dot.T, dt*np.arange(0,traj_length)).T
y_d, y_d_dot, y_d_ddot = xr[:k,:], xr[k:,:], xr_dot[k:,:]
```

### Feedback linearization of the control-affine dynamics

We attempt to stabilize the system to the origin, that is, drive $q$ to $0$. We'll use the normal form transformation:

\begin{equation}
    \bs{\Phi}(\mb{q}, \dot{\mb{q}}) = \bs{\eta}(\mb{q}, \dot{\mb{q}}) = \begin{bmatrix} \mb q \\ \mb{\dot{q}} \end{bmatrix}.
\end{equation}


```python
from core.dynamics import ConfigurationDynamics

class FiniteDimKoopSysOutput(ConfigurationDynamics):
    def __init__(self, finite_dim_koop_sys, y_d, y_d_dot, y_d_ddot, dt):
        ConfigurationDynamics.__init__(self, finite_dim_koop_sys, 2)
        self.finite_dim_koop_sys = finite_dim_koop_sys
        self.ref = y_d
        self.ref_dot = y_d_dot
        self.ref_ddot = y_d_ddot
        self.dt = dt

        self.t_d = self.dt*np.arange(0, self.ref.shape[1])
    
    def y(self, q):
        return q
        
    def dydq(self, q):
        return np.array([[1, 0], [0, 1]])
    
    def d2ydq2(self, q):
        return np.zeros((1, 2, 2))

    def y_d(self, t):
        return self.interpolate_ref_(self.ref, t)

    def y_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot, t)

    def y_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot, t)

    def interpolate_ref_(self, ref, t):
        return np.array([np.interp(t, self.t_d, ref[ii,:]) for ii in range(ref.shape[0])])

output = FiniteDimKoopSysOutput(finite_dim_koop_sys, y_d, y_d_dot, y_d_ddot, dt)
```


```python
from core.controllers import FBLinController, LQRController
q, r = 1e2, 1
Q_fb = q*np.identity(4)
R_fb = r*np.identity(2)
lqr = LQRController.build(output, Q_fb, R_fb)
fb_lin = FBLinController(output, lqr)
```


```python
x_0 = xr[:, 0] + np.random.uniform(-1,1,(n))
ts = dt*np.arange(0,traj_length)

xs_fb, us_fb = finite_dim_koop_sys.simulate(x_0, fb_lin, ts)
```

### Koopman Canonical Transform of the Control-Affine Dynamics

As a result of the careful construction of this system, there exists a Koopman canonical transform, $\mb z = T(\mb x)$ that exactly transforms the control-affine dynamics into a bilinear system. Consider the transformation:

\begin{equation}
    T(\mb q, \mb{\dot{q}}) = \begin{bmatrix} 
    \phi_1(\mb q, \mb{\dot{q}})\\
    \phi_2(\mb q, \mb{\dot{q}})\\
    \phi_3(\mb q, \mb{\dot{q}})\\
    \phi_4(\mb q, \mb{\dot{q}})\\
    \phi_5(\mb q, \mb{\dot{q}})\\
    \phi_6(\mb q, \mb{\dot{q}})\\
    \end{bmatrix}
    = \begin{bmatrix} 
    q_1 - \frac{1}{\lambda}\dot{q}_1\\
    q_2 - \frac{1}{\mu} \dot{q}_2 + \frac{(2 \lambda - \mu)c}{2\lambda \mu} \dot{q}_1^2\\
    \dot{q}_1\\
    \dot{q}_2 - c \dot{q}_1^2\\
    \dot{q}_1^2\\
    1
    \end{bmatrix},
\end{equation}

where $\phi_1, \phi_2, \phi_3, \phi_4, \phi_5, \phi_6$ are eigenfunctions of the Koopman operator associated with the drift 
vector field $\mb f_0$. The matrix with the eigenvalue associated with the i-th eigenfunction on the i-th diagonal 
element is $F=\text{diag}(0, 0, \lambda, \mu, 2 \lambda, 0)$. Then, to reformulate the dynamics we have:

\begin{equation} 
L_{\mb f_1} T(\mb q, \mb{\dot{q}}) = \begin{bmatrix} -\frac{1}{\lambda}\\ \frac{(2\lambda - \mu)c}{\lambda \mu}\dot{q}_1\\ 1 \\ -2c\dot{q}_1 \\ 2\dot{q_1} \\ 0 \end{bmatrix}, \qquad 
L_{\mb f_2} T(\mb q, \mb{\dot{q}}) = \begin{bmatrix} 0\\ -\frac{1}{\mu}(q_1 + 1)\\0 \\ q_1 + 1 \\ 0 \\ 0 \end{bmatrix} 
\end{equation}

and the dynamics can be equivalently transformed to a bilinear form $\mb{\dot{z}} = F \mb z + G_1\mb z u_1 + G_2\mb z u_2$ with

\begin{equation}
F = \begin{bmatrix} 
0 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & \lambda & 0 & 0 & 0\\
0 & 0 & 0 & \mu & 0 & 0\\
0 & 0 & 0 & 0 & 2 \lambda & 0\\
0 & 0 & 0 & 0 & 0 & 0\\  
\end{bmatrix}, \qquad 
G_1 = \begin{bmatrix} 
0 & 0 & 0 & 0 & 0 & -\frac{1}{\lambda}\\
0 & 0 & \frac{(2\lambda - \mu)c}{\lambda \mu} & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 1\\
0 & 0 & -2c & 0 & 0 & 0\\
0 & 0 & 2 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 0\\  
\end{bmatrix}
, \qquad 
G_2 = \begin{bmatrix} 
0 & 0 &  0 & 0 & 0 & 0\\
-\frac{1}{\mu} & 0 &  -\frac{1}{\lambda \mu} & 0 & 0 & -\frac{1}{\mu}\\
0 & 0 & 0 & 0 & 0 & 0\\
1 & 0 & \frac{1}{\lambda} & 0 & 0 & 1\\
0 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 0\\  
\end{bmatrix}
\end{equation}


```python
from koopman_core.dynamics import BilinearLiftedDynamics

F = np.array([[0, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, 0, 0],
              [0, 0, lambd, 0, 0, 0],
              [0, 0, 0, mu, 0, 0],
              [0, 0, 0, 0, 2*lambd, 0],
              [0, 0, 0, 0, 0, 0]])
G_1 = np.array([[0, 0, 0, 0, 0, -1/lambd],
                [0, 0, (2*lambd - mu)*c/(lambd*mu), 0, 0, 0],
                [0, 0, 0, 0, 0, 1], 
                [0, 0, -2*c, 0, 0, 0], 
                [0, 0, 2, 0, 0, 0], 
                [0, 0, 0, 0, 0, 0]])
G_2 = np.array([[0, 0, 0, 0, 0, 0],
                [-1/mu, 0, -1/(lambd*mu), 0, 0, -1/mu],
                [0, 0, 0, 0, 0, 0], 
                [1, 0, 1/lambd, 0, 0, 1], 
                [0, 0, 0, 0, 0, 0], 
                [0, 0, 0, 0, 0, 0]])
G = [G_1, G_2]
phi_fun = lambda x: np.array([x[0,0] - 1/lambd*x[0,2],
                            x[0,1] - 1/mu*x[0,3] + (2*lambd - mu)*c/(2*lambd*mu)*x[0,2]**2,
                            x[0,2],
                            x[0,3] - c*x[0,2]**2,
                            x[0,2]**2,
                            1])
phi_fun_grad = lambda x: np.array([[1., 0., -1/lambd, 0.],
                                   [0., 1., (2*lambd-mu)*c/(lambd*mu)*x[0,2], -1/mu],
                                   [0., 0., 1., 0.],
                                   [0., 0., -2*c*x[0,2], 1.],
                                   [0., 0., 2*x[0,2], 0.],
                                   [0., 0., 0., 0.]])

C_h = np.array([[1, 0, 1/lambd, 0, 0, 0],
                [0, 1, 0, 1/mu, c/mu - ((2*lambd - mu)*c)/(2*lambd*mu), 0]])
C_qdot = np.array([[0, 0, 1, 0, 0, 0],
                   [0, 0, 0, 1, c, 0]])
C_x = np.concatenate((C_h, C_qdot), axis=0)
n_koop = 6

koop_bilinear_sys = BilinearLiftedDynamics(n_koop, m, F, G, C_h, phi_fun)
```

With the same objective as before, we aim to design a feedback linearizing control law utilizing the bilinear dynamics. 
We assume that the outputs can be linearly described in the lifted state space such that $\mb y = C^{\mb h} \mb z$. 
The outputs encoding the objective (driving the system to the origin) can then be described as

\begin{equation}
    \bs{\eta}(\mb{q}, \dot{\mb{q}}) = C^{\mb h} \mb z - \mb y_d(t), 
    \qquad C^{\mb h} = \begin{bmatrix} 1 & 0 & \frac{1}{\lambda} & 0 & 0 & 0\\
    0 & 1 & 0 & \frac{1}{\mu} & \frac{c}{\mu} - \frac{(2\lambda - \mu)c}{2\lambda \mu} & 0
    \end{bmatrix}
\end{equation}

To verify that the transformation is correct, we compare the open loop simulation of the true system and the transformed 
system.


```python
from matplotlib.pyplot import subplots, tight_layout, show
from koopman_core.controllers.openloop_controller import OpenLoopController

ol_controller = OpenLoopController(koop_bilinear_sys, us_fb, ts[:-1])
z_0 = phi_fun(x_0.reshape((1,-1)))
zs_koop, us_koop = koop_bilinear_sys.simulate(z_0, ol_controller, ts)
xs_koop = np.dot(C_x, zs_koop.T)

_, axs = subplots(2, 2, figsize=(6, 8))
ylabels = ['$q_1$', '$q_2$', '$\\dot{q}_1$', '$\\dot{q}_2$']

for ax, data_fb, data_koop, ylabel in zip(axs.flatten(), xs_fb.T, xs_koop, ylabels):
    ax.plot(ts, data_fb, linewidth=3, label='True system')
    ax.plot(ts, data_koop, linewidth=3, label='KCT')
    ax.set_ylabel(ylabel, fontsize=16)
    ax.grid()
    ax.set_xlabel('$t$ (sec)', fontsize=16)
    ax.legend()
    
tight_layout()
show()
```

Differentiating the outputs we get and writing the actuated dynamics $G_1 \mb z u_1 + G_2 \mb z u_2$ as $\hat{G}(\mb z) \mb u$ we get:

\begin{equation}
    \bs{\dot{\eta}} = C^{\mb h}\mb{\dot{z}} - \mb{\dot{y}}_d(t) = C^{\mb h} \big (\mb{\dot{z}} - \mb{\dot{z}}_d(t) \big ) = C^{\mb h} \big ( F \mb z + \hat{G}(\mb z)\mb u - \mb{\dot{z}}_d \big ).
\end{equation}

Because of the relative degree assumption on the outputs, each output $y_i(\mb x)=h_i(\mb x), i=1,\dots,k$ has relative degree 2,
we have that $L_g h(x) = C^{\mb h} \hat{G}(\mb z) = 0$. As a result, $\bs{\dot{\eta}} = C^{\mb h} \big ( F \mb z - \mb{\dot{z}}_d \big )$.
Now, akin to classical feedback linearization we differentiate the outputs again to get:

\begin{equation}
    \bs{\ddot{\eta}} = C^{\mb h} \big ( F \mb{\dot{z}} - \mb{\ddot{z}}_d \big ) = C^{\mb h} \big ( F(F \mb z + \hat{G}(\mb z)\mb u)- \mb{\ddot{z}}_d \big )
    = C^{\mb h} \big ( F^2 \mb z + F\hat{G}(\mb z)\mb u - \mb{\ddot{z}}_d \big )
\end{equation}

Now, the output error dynamics can be written in normal form such that we get:

\begin{equation}
    \bs{\Phi}(\mb{q}, \dot{\mb{q}}) = \begin{bmatrix} \bs{\eta}(\mb{q}, \dot{\mb{q}})\\ \bs{\dot{\eta}}(\mb{q}, \dot{\mb{q}})\end{bmatrix}, \qquad
    \bs{\dot{\Phi}} = \begin{bmatrix} \bs{\dot{\eta}}\\ \bs{\ddot{\eta}}\end{bmatrix} =
    \begin{bmatrix} \bs{\dot{\eta}}\\
    C^{\mb h} \big ( F(F \mb z + \hat{G}(\mb z)\mb u)- \mb{\ddot{z}}_d \big ) \end{bmatrix}
\end{equation}


```python
from core.dynamics.configuration_dynamics import ConfigurationDynamics

class BilinearSysOutput(FiniteDimKoopSysOutput):
    def __init__(self, bilinear_dynamics, y_d, y_d_dot, y_d_ddot, dt, z_d, z_d_dot, z_d_ddot, C_h):
        FiniteDimKoopSysOutput.__init__(self, bilinear_dynamics, y_d, y_d_dot, y_d_ddot, dt)
        self.bilinear_dynamics = bilinear_dynamics
        self.C_h = C_h
        self.ref_z = z_d
        self.ref_dot_z = z_d_dot
        self.ref_ddot_z = z_d_ddot

    def z_d(self, t):
        return self.interpolate_ref_(self.ref_z, t)

    def z_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot_z, t)

    def z_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot_z, t)

phi_fun_grad_dot = lambda x_dot: np.array([[0., 0., 0., 0.],
                                   [0., 0., (2*lambd-mu)*c/(lambd*mu)*x_dot[0,2], 0.],
                                   [0., 0., 0., 0.],
                                   [0., 0., -2*c*x_dot[0,2], 0.],
                                   [0., 0., 2*x_dot[0,2], 0.],
                                   [0., 0., 0., 0.]])

z_d = np.array([phi_fun(x.reshape(1,-1)) for x in xr.T]).T
z_d_dot = np.array([phi_fun_grad(x.reshape(1,-1))@x_dot for (x,x_dot) in zip(xr.T, xr_dot.T)]).T
z_d_ddot = np.array([phi_fun_grad_dot(x_dot.reshape(1,-1))@x_dot + phi_fun_grad(x.reshape(1,-1))@x_ddot for (x,x_dot,x_ddot) in zip(xr.T, xr_dot.T, xr_ddot.T)]).T
output_bl_fb = BilinearSysOutput(koop_bilinear_sys, y_d, y_d_dot, y_d_ddot, dt, z_d, z_d_dot, z_d_ddot, C_h)
```

### Koopman Feedback Linearization


Since the unactuated dynamics are already linear in the lifted state space, we only need to cancel out the nonlinear actuated 
dynamics terms. Writing the actuated dynamics $G \mb z u$ as $\hat{G}(\mb z) u$ we get the control law:

\begin{equation}
    \mb u(\mb z, t) = \big(C^{\mb h}F\hat{G}(\mb z)\big )^\dagger (\mb{\ddot{z}}_d(t) - C^{\mb h}F^2 \mb z_d(t) + C^{\mb h}\bs{\nu}(\mb z, t)),
\end{equation}

where $\bs{\nu}(\mb z, t)$ is a controller designed with LQR to stabilize the system 

\begin{equation}
\dot{\Phi}_{\mb z} = \begin{bmatrix} 0 & I \\ F^2 & 0\end{bmatrix} \Phi_{\mb z} + 
\begin{bmatrix} 0 \\ I \end{bmatrix} \bs{\nu}
\end{equation}



```python
from scipy.linalg import solve_continuous_are
from koopman_core.controllers.bilinear_fb_lin_controller import BilinearFBLinController

f_eta = np.concatenate((np.zeros((n_koop,n_koop)), np.eye(n_koop)), axis=1)
f_eta_dot = np.concatenate((F@F, np.zeros((n_koop,n_koop))), axis=1)
F_lin = np.concatenate((f_eta, f_eta_dot), axis=0)
G_lin = np.concatenate((np.zeros((n_koop,n_koop)), np.eye(n_koop)), axis=0)
Q_bl_fb = q*np.eye(2*n_koop)
R_bl_fb = r*np.eye(n_koop)
P = solve_continuous_are(F_lin, G_lin, Q_bl_fb, R_bl_fb)
K = np.linalg.inv(R_bl_fb)@G_lin.T@P
bl_fb_lin = BilinearFBLinController(koop_bilinear_sys, output_bl_fb, K)
```


```python
xs_bl_fb, us_bl_fb = finite_dim_koop_sys.simulate(x_0, bl_fb_lin, ts)
```

### Performance comparison of the control-affine and Koopman bilinear controllers

To compare the relative performance of the feedback linearizing controller based on the control-affine dynamics
and the feedback linearizing controller based on the Koopman bilinear dynamics, consider the quadratic cost metric,
where $Q, R$ are the cost matrices used for the LRQ design of the linear auxillary controller:

\begin{equation}
    J(\mb x, \mb u, t) = \int_{0}^{T_f} \big [ (\mb y(t) - \mb y_d(t))^T Q (\mb y(t) - \mb y_d(t)) + \mb u(t)^T R \mb u(t) \big ] dt
\end{equation}

Recall that in this example the output is simply $y(t) = q$ that we aim to stabilize to the origin.


```python
import matplotlib.pyplot as plt
import matplotlib

cost_fb_lin = np.cumsum(np.diag((xs_fb[1:,:]-xr[:,1:].T) @ Q_fb @ (xs_fb[1:,:]-xr[:,1:].T).T) + np.diag(us_fb @ R_fb @ us_fb.T))
cost_bl_fb_lin = np.cumsum(np.diag((xs_bl_fb[1:,:]-xr[:,1:].T) @ Q_fb @ (xs_bl_fb[1:,:]-xr[:,1:].T).T) + np.diag(us_bl_fb @ R_fb @ us_bl_fb.T))

lw = 2
fs = 16
plt.figure(figsize=(12,4))
plt.subplot(2,3,1)
plt.plot(ts, xs_fb[:,0], linewidth=lw, label='Control-affine FL')
plt.plot(ts, xs_bl_fb[:,0], linewidth=lw, label='KCT FL')
plt.plot(ts, y_d[0,:], '--r', linewidth=lw, label='Reference')
plt.ylabel('$x_1$', fontsize=fs)
plt.title('Output states', fontsize=fs)
plt.grid()

plt.subplot(2,3,4)
plt.plot(ts, xs_fb[:,1], linewidth=lw, label='Control-affine FL')
plt.plot(ts, xs_bl_fb[:,1], linewidth=lw, label='KCT FL')
plt.plot(ts, y_d[1,:], '--r', linewidth=lw, label='Reference')
plt.xlabel('Time (sec)', fontsize=fs)
plt.ylabel('$x_2$', fontsize=fs)
plt.grid()

plt.subplot(2,3,2)
plt.plot(ts[:-1], us_fb[:,0], linewidth=lw, label='Control-affine FL')
plt.plot(ts[:-1], us_bl_fb[:,0], linewidth=lw, label='KCT FL')
plt.ylabel('$u_1$', fontsize=fs)
plt.title('Control action', fontsize=fs)
plt.grid()

plt.subplot(2,3,5)
plt.plot(ts[:-1], us_fb[:,1], linewidth=lw, label='Control-affine FL')
plt.plot(ts[:-1], us_bl_fb[:,1], linewidth=lw, label='KCT FL')
plt.xlabel('Time (sec)', fontsize=fs)
plt.ylabel('$u_2$', fontsize=fs)
plt.grid()

plt.subplot(1,3,3)
plt.plot(ts[:-1], cost_fb_lin/cost_fb_lin[-1], linewidth=lw, label='Control-affine FL')
plt.plot(ts[:-1], cost_bl_fb_lin/cost_fb_lin[-1], linewidth=lw, label='KCT FL')
plt.plot(0, 0, '--r', linewidth=lw, label='Reference')
plt.xlabel('Time (sec)', fontsize=fs)
plt.ylabel('J (normalized)', fontsize=fs)
plt.title('Normalized cost', fontsize=fs)
plt.grid()
plt.legend(loc='lower right', fontsize=fs-4)

matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.tight_layout()
plt.savefig('examples/figures/finite_koop_sys.pdf', format='pdf', dpi=2400)
plt.show()
```


```python
print('Cumulative improvement: ', str((1-cost_bl_fb_lin[-1]/cost_fb_lin[-1])*100), '%')


```

    Cumulative improvement:  4.805082367897395 %


f2e1ad9ad2edc949a9e8939e2ff63ec941b61738
ipynb
Jupyter Notebook
working_files/bilinearizable_sys_feedback_linearization.ipynb
Cafolkes/koopman_learning_and_control
0152a2bd5079da4d672dbaac404b6c084410297d
MIT
2021-11-06T11:32:40.000Z
2022-03-16T00:06:23.000Z
working_files/bilinearizable_sys_feedback_linearization.ipynb
Cafolkes/koopman-learning-and-control
0152a2bd5079da4d672dbaac404b6c084410297d
MIT
working_files/bilinearizable_sys_feedback_linearization.ipynb
Cafolkes/koopman-learning-and-control
0152a2bd5079da4d672dbaac404b6c084410297d
MIT
2022-03-04T09:34:58.000Z
2022-03-04T09:34:58.000Z
Qwen/Qwen-72B
1. YES
2. YES