уравнения
состояние [θ, Y, x, Z]
θ̇ = Y
Ẏ = ( g·sin θ − (u + ветер)·cos θ ) ⁄ L
ẋ = Z
Ż = u |u| ≤ u_max
AUTO u = Kp_θ·θ + Kd_θ·Y + Kp_x·(x − x₀) + Kd_x·Z
MANUAL u = ±u_manual
SWING-UP u = k·E·Y·cos θ
E = m·(L·θ̇)²⁄2 + m·g·L·(cos θ − 1)
подхват: θ < π/9 и |E| < 0.5 (isControllable)
рельса ±0.1 м — swing-up-constraints.py:
u = −0.5·sign(θ̇·cos θ), между качками гасим скорость
энкодер реальный — цифровой контур 200 Гц, как в прошивке:
θ_изм = precision · trunc(θ ⁄ precision)
θ̇_изм = (θ_изм − θ_изм_пред) ⁄ dt
наблюдатель (его файл, поправка только в угол):
θ̂̇ = Ŷ + k·(θ_изм − θ̂)
Ŷ̇ = ( g·sin θ̂ − u·cos θ̂ ) ⁄ L
RK4, фиксированный шаг 1 мс, развязан с кадрами.
Сверено с scipy.integrate.odeint на его константах.