This is the companion page to my Furuta Pendulum build log. The main post tells the story; this page collects the actual math — the dynamic model, the controller, the observer, and the swing-up law — with just enough narration to follow along.

1. Coordinates and Parameters

The system has two degrees of freedom: the arm angle $\theta$ (the only coordinate the motor drives, rotating about the vertical axis) and the pendulum angle $\varphi$, measured from upright — so $\varphi = 0$ is balanced and $\varphi = \pm\pi$ is hanging. The pendulum is a bare uniform steel rod, so its center of mass sits at half its length and its inertia follows from the standard rod formula.

Symbol

Meaning

Value

R

Arm length (pivot radius)

50 mm

r

Pendulum rod length

100 mm

l = r/2

Distance to the rod's center of mass

50 mm

mp

Pendulum rod mass

measured

Ip = (1/12) mp r²

Rod inertia about its own center of mass

computed

Jp = Ip + mp l²

Rod inertia about its pivot

5.25 × 10⁻⁵ kg·m² (identified)

JA

Arm assembly inertia about the motor axis

≈ 2–2.9 × 10⁻³ kg·m² (identified)

Kt = Ke

Motor torque / back-EMF constant

≈ 0.098 N·m/A (identified)

Rphase

Motor phase resistance

9.167 Ω (measured)

τmax

Peak torque at the 8 V firmware cap

≈ 0.087 N·m

A satisfying validation: the pendulum inertia derived from first principles (5.18 × 10⁻⁵ kg·m²) matched the value identified from a free-swing experiment (5.25 × 10⁻⁵ kg·m²) to within 1.3%.

2. The Lagrangian

I derived the equations of motion with Lagrangian mechanics — energies instead of forces — which sidesteps tracking constraint forces through the rotating frame. The one genuinely subtle point: the pendulum's pivot rides on the moving arm tip, which is not an inertially fixed point, so the pendulum's kinetic energy must be written with König's theorem (translation of the center of mass plus rotation about it):

$$ T_{pend} = \tfrac{1}{2} m_p\, v_{COM}^2 + \tfrac{1}{2} I_p\, \omega^2 $$

(The arm, which rotates about a fixed axis, is allowed the familiar single-term shortcut — the asymmetry between the two bodies is instructive.) Carrying out the kinematics and collecting terms gives the total kinetic and potential energies:

$$ T = \tfrac{1}{2}\!\left(J_A + m_p R^2 + J_p \sin^2\!\varphi\right)\dot\theta^2 + \tfrac{1}{2} J_p\, \dot\varphi^2 + m_p\, l\, R \cos\varphi\; \dot\theta\, \dot\varphi $$
$$ V = m_p\, g\, l \cos\varphi $$

Note that $V$ is maximal at upright — that sign is where the instability lives. Applying the Euler–Lagrange equations to $L = T - V$ for each coordinate yields two coupled nonlinear equations of motion, compactly:

$$ M(q)\,\ddot q + C(q,\dot q)\,\dot q + G(q) = \begin{bmatrix} \tau \\ 0 \end{bmatrix}, \qquad q = \begin{bmatrix} \theta \\ \varphi \end{bmatrix} $$
$$ M(q) = \begin{bmatrix} J_A + m_p R^2 + J_p \sin^2\!\varphi & m_p\, l\, R \cos\varphi \\ m_p\, l\, R \cos\varphi & J_p \end{bmatrix}, \qquad G(q) = \begin{bmatrix} 0 \\ -\, m_p\, g\, l \sin\varphi \end{bmatrix} $$

where $C$ collects the Coriolis/centrifugal terms and friction enters as damping on each joint. Every step of the hand derivation was cross-checked symbolically with sympy.

3. Linearization and the State-Space Model

For balancing, linearize about upright ($\varphi \approx 0$: small angles, drop products of rates). The coupled equations reduce to:

$$ \begin{aligned} \left(J_A + m_p R^2\right)\ddot\theta + m_p\, l\, R\, \ddot\varphi &= \tau - b_\theta\, \dot\theta \\ m_p\, l\, R\, \ddot\theta + J_p\, \ddot\varphi - m_p\, g\, l\, \varphi &= -\, b_\varphi\, \dot\varphi \end{aligned} $$

which assembles into the standard linear state-space form with state vector $x = [\,\theta,\ \varphi,\ \dot\theta,\ \dot\varphi\,]^T$ and input $u = \tau$:

$$ \dot x = A\,x + B\,u, \qquad y = C\,x $$

Two properties of this model shape everything that follows:

  • Instability. $A$ has a real eigenvalue at about +11.6 rad/s — the upright equilibrium is unstable, with errors growing on a ~85 ms timescale. (A back-of-envelope check: ignoring coupling and damping, the growth rate is $\sqrt{m_p g l / J_p}$, which lands within a few percent of the full eigenvalue.)

  • Controllability. The controllability matrix $[\,B \ AB \ A^2B \ A^3B\,]$ has full rank 4 — torque at the arm alone can steer the entire state. Balancing is guaranteed to be possible before building anything.

4. LQR Balance Control

The balance controller is a Linear Quadratic Regulator: choose the gain vector $K$ minimizing the quadratic cost

$$ J = \int_0^\infty \left( x^T Q\, x + u^T R\, u \right) dt, \qquad u = -K x $$

with $K$ computed from the algebraic Riccati equation (MATLAB's lqr(A, B, Q, R)). The closed-loop matrix $A - BK$ has all eigenvalues in the left half-plane — the unstable pole is pulled stable.

The physically important subtlety was the torque budget. An aggressive weighting demanded peak torques ~70× beyond the motor's ~0.087 N·m capability, so the flight controller is deliberately gentle: a large $R$ penalizes control effort, keeping the gains small enough that commands respect the hardware. Torque maps to the FOC voltage command through the motor's electrical model,

$$ V_q \approx \frac{R_{phase}}{K_t}\, u \;\approx\; 92.5\, u $$

clamped at ±8 V by the firmware, so saturation degrades gracefully instead of winding up. The gains that fly on the hardware (after empirically reconciling every sign convention between model and machine): K = [0.037, −0.77, 0.017, −0.045], executing at 500 Hz.

5. The State Observer

The encoders measure only the two angles; the two velocities must be estimated. Differentiating a 14-bit encoder staircase amplifies noise, and low-pass filtering trades that noise for lag — with no setting that is both clean and fast. The principled solution is a Luenberger observer: integrate a copy of the plant model in software, driven by the same input, and continuously correct it with the measurements:

$$ \dot{\hat x} = A\,\hat x + B\,u + L\left( y - C\,\hat x \right) $$

The correction term converts the position surprise into corrections on all four states — including the unmeasured velocities. The estimation error obeys

$$ \dot e = \left( A - L\,C \right) e $$

so placing the eigenvalues of $A - LC$ in the left half-plane makes the estimate converge to the truth automatically. This problem is the exact mathematical dual of the LQR design (place $A - BK$), so the same tools compute $L$. On the ESP32 the observer runs in discrete form at 500 Hz (matrices discretized via c2d at 2 ms) as a predict–correct pair:

$$ \hat x_k^- = A_d\, \hat x_{k-1} + B_d\, u_{k-1}, \qquad \hat x_k = \hat x_k^- + L_d \left( y_k - C\, \hat x_k^- \right) $$

One hard-won practical lesson: with an imperfect model, slow observer poles fail. Intuition says slow poles (trust the model) give smooth estimates, but trusting a model with real error injects velocity lag that feeds back as negative damping. The working design keeps the poles fast (around −150 to −180 rad/s) so the estimate stays tightly corrected by the encoders.

6. Innovation as a Model-Quality Meter

The observer's innovation — the per-step gap between the predicted and actual measurement,

$$ \nu_k = y_k - C\, \hat x_k^- $$

— is a live readout of model error. Flat, zero-mean noise means the model predicts the sensors as well as they can be predicted; structure (bias, drift, oscillation correlated with a state) points at a specific modeling error. During calm balancing this system's innovation is essentially zero-mean — the first-principles model predicts the encoders almost perfectly. The innovation also exposed a genuinely structural problem: an intermittent ~17 Hz limit cycle traced to an unmodeled degree of freedom (a slightly loose base), fixed mechanically rather than in the controller.

7. Energy-Based Swing-Up

Swing-up cannot be position control: at the bottom, torque on the arm has no direct authority over $\varphi$. Instead, control the pendulum's mechanical energy

$$ E = \tfrac{1}{2} J_p\, \dot\varphi^2 + m_p\, g\, l \cos\varphi, \qquad E_{top} = m_p\, g\, l $$

and pump it up until it reaches $E_{top}$ — exactly enough to arrive at vertical — then coast and let the LQR catch. The rate of energy injection from accelerating the pivot is

$$ \frac{dE}{dt} \;\propto\; \ddot\theta\; \dot\varphi \cos\varphi $$

so to always add energy, the arm acceleration should follow the sign of $\dot\varphi \cos\varphi$ — the Åström–Furuta law. The physics of the cosine term: push the pivot in the direction of the bob's horizontal velocity, hardest at the bottom — and crucially, the sign flips above horizontal, correctly reversing the push once the bob rises over the pivot. Dropping that term is exactly why a naive velocity-only pump stalls near 140° and can never top out. The implemented law is the saturated form

$$ V_q = K_S \cdot \operatorname{sign}\!\left( \dot\varphi \cos\varphi \right) \cdot V_{q,max} \quad \text{while } E < E_{top}, \text{ else coast} $$

with one crucial practical addition: a ~30 ms time debounce on the sign — a commanded direction must persist before it is adopted. This rejects noise-driven chattering near the turnarounds (where $\dot\varphi \approx 0$ by definition, so a velocity deadband would block the real reversals too) while preserving the true once-per-half-swing flips. The result is a single clean flick from hanging to upright at only ~4–7 V.

← Back to the build log