# Pendulum Control Playground

###### tags: `control`

[GitHub Link for code implementation](https://github.com/shengwen-tw/pendulum-control-simulator)

## Pendulum System Dynamics

We consider the nonlinear dynamics of a simple pendulum with damping and control torque:

$$
\begin{aligned}
\dot{x}_1 &= x_2 \\
\dot{x}_2 &= -\frac{g}{l} \sin(x_1) - \frac{k}{m} x_2 + \frac{1}{ml^2} u
\end{aligned}
$$

Where:

- $x_1 = \theta$: Angle (rad)
- $x_2 = \dot{\theta}$: Angular velocity
- $u$: Control input (Torque)
- $g$: Gravitational acceleration
- $l$: Rod length
- $m$: Bob mass
- $k$: Damping

## PID Control

### 1. Linearization (Small-Angle Approximation)

For small angles, $\sin(\theta) \approx \theta$. The linearized model becomes:

$$\ddot{\theta} + \frac{k}{m} \dot{\theta} + \frac{g}{l} \theta = \frac{1}{ml^2} u$$

### 2. Transfer Function (Laplace Domain)

Taking Laplace transform (assuming zero initial conditions):

$$\frac{\Theta(s)}{U(s)} = \frac{1}{ml^2} \cdot \frac{1}{s^2 + \frac{k}{m}s + \frac{g}{l}}$$

With parameters:

- $m = 0.5$, $l = 1$, $g = 9.8$, $k = 1$

The transfer function becomes:

$$G(s) = \frac{2}{s^2 + 2s + 9.8}$$

### 3. PID Controller Design (S-Domain)

The standard PID controller is:

$$C(s) = K_P + \frac{K_I}{s} + K_D s$$

Example gains:

- $K_P = 30$, $K_I = 10$, $K_D = 5$

Closed-loop system:

$$T(s) = \frac{C(s)G(s)}{1 + C(s)G(s)}$$

### 4. Performance Metrics (via `stepinfo`)

| Metric | Description |
|------------------|-----------------------------------------------------------------------------|
| **Overshoot** | Maximum output above the target percentage. |
| **Rise Time** | Time taken to rise from 10% to 90% of the target value |
| **Settling Time** | Time after which response stays within ±2% of target |
| **Stability** | All closed-loop poles in the Left Half Plane (LHP) ⇒ Stable |

### 5. MATLAB Code

s = tf('s');
G = 2 / (s^2 + 2*s + 9.8);
C = 30 + 10/s + 5*s;
T = feedback(C*G, 1);
step(T);
stepinfo(T);
pzmap(T);

## Infinite Horizon Linear Quadratic Regulator (LQR)

### 1. System Linearization

$x=\pmatrix{x_1 \\ x_2}$

$A=\pmatrix{
\dfrac{\partial \dot{x_1}}{\partial x_1} &
\dfrac{\partial \dot{x_1}}{\partial x_2} \\
\dfrac{\partial \dot{x_2}}{\partial x_1} &
\dfrac{\partial \dot{x_2}}{\partial x_2}
}=
\pmatrix{0 & 1 \\
\dfrac{g}{l}cos(x_1) & -\dfrac{k}{m}
}$

$B=\pmatrix{
\dfrac{\partial \dot{x_1}}{\partial u} \\
\dfrac{\partial \dot{x_2}}{\partial u}
} = \pmatrix{0 \\ \dfrac{1}{ml^2}}$

$\dot{x} = Ax + Bu$

### 2. Linear Quadratic Regulator Problem

Design a **state-feedback control law** $u = -K(x - x_d)$ to minimize the following infinite-horizon quadratic cost:

$$
J(x, u) = \int_0^\infty \left( \tilde{x}^T Q \tilde{x} + u^T R u \right) dt
$$

Where:
- $\tilde{x} = x - x_d$: state error (actual state minus desired state)
- $Q \succeq 0$: positive semi-definite state weighting matrix
- $R \succ 0$: positive definite input weighting matrix

### 3. LQR Control Law

The optimal control law is:

$$
u = -K \tilde{x}
$$

The feedback gain $K$ is computed as:

$$
K = R^{-1} B^T X
$$

Here, $K$ is the optimal state feedback gain matrix that minimizes the cost function $J(x, u)$.

### 4. Continuous Algebraic Riccati Equation (CARE)

To find $K$, we must solve the **Continuous Algebraic Riccati Equation**:

$$
A^T X + X A - X B R^{-1} B^T X + Q = 0
$$

Once $X$ is solved, the gain is computed via:

$$
K = R^{-1} B^T X
$$

### 5. Solving CARE in MATLAB

In MATLAB, you can solve the CARE using the built-in function:

[X, ~, K] = care(A, B, Q, R);

## Sliding Mode Control

### 1. Sliding Surface Design

Define the sliding surface:

$$
s(x) = c e_1 + e_2
$$

Where:

- $e_1 = x_1 - x_{1d}$: angle tracking error
- $e_2 = x_2 - x_{2d}$: angular velocity error
- $c > 0$: sliding gain
- For regulation to rest: $x_{1d} = 0$, $x_{2d} = 0$

Thus:

$$
s = c x_1 + x_2
$$

### 2. Control Law (SMC)

Design the control input as:

$$
u = ml^2 \left[ \frac{g}{l} \sin(x_1) + \frac{k}{m} x_2 - c x_2 - \eta \cdot \text{sign}(s) \right]
$$

Where:

- $\eta > 0$: control gain to overcome uncertainty/disturbance
- This forces $\dot{s} = -\eta \cdot \text{sign}(s)$ → reaching condition

### 3. Stability Proof with Lyapunov Method

Choose the Lyapunov candidate function:

$$
V(s) = \frac{1}{2} s^2
$$

Compute its time derivative:

$$
\dot{V} = s \dot{s}
$$

From system and control law:

$$
\dot{s} = c \dot{x}_1 + \dot{x}_2 = c x_2 + \dot{x}_2
$$

Substitute dynamics and control input:

$$
\dot{s} = c x_2 + \left( -\frac{g}{l} \sin(x_1) - \frac{k}{m} x_2 + \frac{1}{ml^2} u \right)
$$

Plug in $u$ from the control law:

$$
\dot{s} = c x_2 - \frac{g}{l} \sin(x_1) - \frac{k}{m} x_2 + \frac{1}{ml^2} \cdot ml^2 \left[ \frac{g}{l} \sin(x_1) + \frac{k}{m} x_2 - c x_2 - \eta \cdot \text{sign}(s) \right]
$$

Simplifies to:

$$
\dot{s} = -\eta \cdot \text{sign}(s)
$$

Thus:

$$
\dot{V} = s \cdot \dot{s} = -\eta |s| \leq 0
$$

Therefore, $V$ is positive definite and $\dot{V}$ is negative semi-definite → the system is **globally stable** in the sense of Lyapunov.

### 4. Practical Remarks

To avoid chattering, replace $\text{sign}(s)$ with a smooth approximation like:

$$
\text{sat}\left(\frac{s}{\phi}\right) = \begin{cases}
1 & s > \phi \\
\frac{s}{\phi} & |s| \leq \phi \\
-1 & s < -\phi
\end{cases}
$$

The control law becomes:

$$
u = ml^2 \left[ \frac{g}{l} \sin(x_1) + \frac{k}{m} x_2 - c x_2 - \eta \cdot \text{sat}(s/\phi) \right]
$$

Choose $\eta >$ bound on disturbance for robustness.

### 5. Summary

- Sliding surface: $s = c x_1 + x_2$
- Lyapunov function: $V = \frac{1}{2}s^2$
- Control law drives $s \to 0$ using discontinuous (or smooth) control
- Stability is ensured by $\dot{V} = -\eta |s|$

## Backstepping Control

### 1. Define Error Coordinates

Define tracking errors:

$$
z_1 = x_1 - x_{1d}, \quad z_2 = x_2 - \alpha(x_1)
$$

Let the **virtual control law** (for $x_2$) be:

$$
\alpha(x_1) = -k_1 z_1
$$

where $k_1 > 0$ is a design gain.

Then:

$$
z_2 = x_2 + k_1 z_1
$$

### 2. Lyapunov Function Design

Construct a Lyapunov candidate:

$$
V = \frac{1}{2} z_1^2 + \frac{1}{2} z_2^2
$$

Compute its derivative:

$$
\dot{V} = z_1 \dot{z}_1 + z_2 \dot{z}_2
$$

From dynamics:

- $\dot{z}_1 = \dot{x}_1 = x_2 = z_2 - k_1 z_1$

So:

$$
z_1 \dot{z}_1 = z_1(z_2 - k_1 z_1)
$$

To compute $\dot{z}_2$:

$$
\dot{z}_2 = \dot{x}_2 + k_1 \dot{z}_1
$$

From system:

$$
\dot{x}_2 = -\frac{g}{l} \sin(x_1) - \frac{k}{m} x_2 + \frac{1}{ml^2} u
$$

Thus:

$$
\dot{z}_2 = -\frac{g}{l} \sin(x_1) - \frac{k}{m} x_2 + \frac{1}{ml^2} u + k_1(z_2 - k_1 z_1)
$$

### 3. Final Control Law

Choose:

$$
u = ml^2 \left[ \frac{g}{l} \sin(x_1) + \frac{k}{m} x_2 - k_1(z_2 - k_1 z_1) - k_2 z_2 \right]
$$

where $k_2 > 0$ is an additional design gain.

### 4. Closed-Loop Lyapunov Derivative

Substitute $u$ into $\dot{z}_2$, we get:

$$
\dot{z}_2 = -k_2 z_2
$$

Then:

$$
\dot{V} = z_1(z_2 - k_1 z_1) + z_2(-k_2 z_2) = -k_1 z_1^2 - k_2 z_2^2 + z_1 z_2
$$

Using Young’s inequality:

$$
z_1 z_2 \leq \frac{1}{2}(z_1^2 + z_2^2)
$$

So for sufficiently large $k_1$, $k_2$, we get:

$$
\dot{V} \leq -\left(k_1 - \frac{1}{2}\right) z_1^2 - \left(k_2 - \frac{1}{2}\right) z_2^2 \leq 0
$$

The system is **globally asymptotically stable**.

### 5. Summary of Backstepping Steps

1. Define error coordinates:
- $z_1 = x_1$, $z_2 = x_2 + k_1 x_1$

2. Choose Lyapunov: $V = \frac{1}{2} z_1^2 + \frac{1}{2} z_2^2$

3. Design control law:
$$
u = ml^2 \left[ \frac{g}{l} \sin(x_1) + \frac{k}{m} x_2 - k_1(z_2 - k_1 z_1) - k_2 z_2 \right]
$$

4. Prove $\dot{V} \leq 0$ ⇒ stability

### 6.Remarks

- Backstepping is constructive and modular — useful for strict-feedback systems.
- Can be extended to trajectory tracking (with feedforward terms).
- Requires full state measurement (or state observer).

## Nonlinear Model Predictive Control (NMPC)

### 1. NMPC Problem Formulation

Minimize cost function over prediction horizon $T_p$:

$$
J = \int_0^{T_p} \left[ (x(t) - x_d)^T Q (x(t) - x_d) + u(t)^T R u(t) \right] dt
$$

Where:
- $x_d$: desired state (e.g., upright $\theta = 0$)
- $Q \succeq 0$: state penalty
- $R \succ 0$: input penalty

### 2. Constraints

You may include constraints such as:

- Input bounds: $u_{\min} \leq u(t) \leq u_{\max}$
- State constraints: e.g., $|\theta| \leq \pi$, $|\dot{\theta}| \leq \omega_{\max}$

### 3. Discretization (for implementation)

Use Euler or Runge-Kutta method to discretize:

$$
x_{k+1} = x_k + f(x_k, u_k) \cdot \Delta t
$$

Then formulate discrete-time optimization problem:

$$
\min_{\{u_k\}_{k=0}^{N-1}} \sum_{k=0}^{N-1} \left[ (x_k - x_d)^T Q (x_k - x_d) + u_k^T R u_k \right]
$$

Subject to:
- $x_{k+1} = x_k + \Delta t \cdot f(x_k, u_k)$
- $u_{\min} \leq u_k \leq u_{\max}$
- $x_0 = x_{\text{current}}$

### 4. Stability Consideration

To guarantee stability:
- Use **terminal cost**: $V_f(x_N) = (x_N - x_d)^T P (x_N - x_d)$
- Add **terminal constraint**: $x_N \in \mathcal{X}_f$ (a stable region)

### 5. Summary of NMPC Steps

1. At each time step, observe current state $x_0$
2. Solve finite-horizon optimization problem (nonlinear program)
3. Apply **first control input** $u_0$
4. Repeat at next time step (receding horizon)

### 6. Pros and Cons

| Aspect | Description |
|------------|-----------------------------------------------------------------------------|
| ✅ Pros | Handles nonlinearities and constraints directly |
| | Predictive → anticipates future behavior |
| ❌ Cons | Requires solving an optimization problem at each time step (computational) |
| | May need good initial guess for convergence |