LQR Inverted Pendulum

The inverted pendulum (cart-pole) is a classic benchmark for control theory: an inherently unstable, nonlinear, underactuated system. I built this physical benchtop testbed from scratch to take modern state-space control theory from mathematical derivation all the way to bare-metal embedded execution on a microcontroller.

Mathematical Modeling & Control Design

1. System Modeling: Derived the nonlinear equations of motion using Lagrangian mechanics, modeling cart mass (M), pendulum point mass (m), pole length (l), and rotational inertia (I).
2. Linearization: Linearized the continuous-time dynamics around the upright unstable equilibrium (θ = 0) into state-space representation:
   • State vector: x = [cart position, cart velocity, pole angle, pole angular velocity]^T
   • Linear state-space model: dx/dt = Ax + Bu
3. LQR Optimization: Defined cost weighting matrices Q (penalizing pole angle deflection and cart drift) and R (penalizing motor control effort). Solved the continuous-time Algebraic Riccati Equation to calculate the optimal state feedback gain matrix K.
4. Control Law: Computed the optimal control effort u = -Kx at each discrete time step.

Embedded Implementation (RP2040 C/C++ SDK)

• Hardware PIO Encoders: Configured RP2040 Programmable I/O (PIO) state machines to continuously decode quadrature pulses from both the cart and pendulum optical encoders. This eliminated CPU interrupt overhead and guaranteed zero missed ticks during high-velocity swings.
• Dual-Core Architecture: Core 0 executes the deterministic 500 Hz (2 ms) timer interrupt control loop, while Core 1 streams real-time telemetry over USB serial for live parameter tuning and safety monitoring.
• Velocity Estimation & Filtering: Raw finite-difference velocity calculation $(\Delta \theta / \Delta t)$ amplified discrete quantization noise. Implemented an exponential moving average low-pass filter to provide smooth velocity estimates without adding phase lag.

Physical Results

• Recovers reliably from external physical taps and disturbance impulses.
• Maintains continuous stable balance along the linear rail indefinitely with zero steady-state drift.
• Full CAD models, dynamic simulation scripts, and C++ firmware are published on GitHub.