I designed and built a self-balancing robot with model-based control, sensor fusion and real-time embedded firmware. My work covers the mechanical model, electronics, controller implementation and repeatable hardware tests.
Test setup and findings
I used a repeatable bench procedure with logged UART telemetry to compare controller and estimator configurations during position-hold and trajectory-tracking runs. The test data exposed the effect of encoder latency, step quantisation and balance-point drift; adding wheel-encoder and pitch Kalman filters gave the most stable overall hardware behaviour.
Best overall hardware configuration
LQR + wheel-encoder KF + pitch KF
This combination gave me the most stable overall behaviour on the real robot. The four-state pitch KF learns the balance point and gyro bias; the wheel KFs estimate actual joint motion instead of assuming every commanded step occurred.
With position hold: roughly 4–5 mm final reported position error and 21.0 mm tracking RMS. This is my preferred configuration, not a claim that it wins every metric: step counting was slightly smoother on holding acceleration-command RMS.
Hardware
3D-printed chassis, two direct-drive stepper motors and a LiPo battery. Total mass: 1.0175 kg; wheel diameter: 62.5 mm; wheel separation: 182 mm. The clips below show the working prototype and its telemetry interface; playback is muted and loops after pressing play.
Electronics
- Processor: NUCLEO-G474RE / STM32G474RE.
- Inertial sensing: LSM6DS0 accelerometer and gyroscope.
- Wheel sensing: two AS5600 magnetic encoders on separate I2C buses.
- Motor drive: two A4988 drivers, hardware-PWM step generation, 1/16 microstepping.
- Communication: 115200-baud UART; DMA telemetry at 10 Hz.
Simulation
Python nonlinear inverted-pendulum model with measured mass and centre of mass, estimated inertia, step quantisation and a 35 ms actuator lag. Gains, limits and plant constants are exported to the C firmware from one configuration. ROS 2 Jazzy / Gazebo Harmonic provides a separate interactive simulation. This is a model-to-firmware parameter-export workflow, not Simulink auto-coding or physical hardware-in-the-loop (HIL).
(Iy + m h2) θ̈
= m g h sin θ − m h a cos θ − c θ̇
m: body mass; h: body centre-of-mass height; Iy: body pitch inertia; a: wheel acceleration; c: damping.
Control
Integral-augmented LQR regulates travel, velocity and pitch. The position integral is implemented as a bounded pitch trim with saturation-aware updates. A pitch-PD/velocity controller provides the PID baseline; its configured pitch integral gain is zero. Both output acceleration, integrated into wheel speed.
z = [∫ ex dt, ex, ev, eθ, θ̇]T
acmd = −Kz
J = ∫0∞ (zTQz + R acmd2) dt
ex, ev, eθ: position, velocity and pitch errors relative to their references. Firmware represents the integral contribution as a pitch-reference trim.
acmd = kpeθ + kdθ̇ + kv(v − vref)
Positive velocity-feedback sign for the acceleration-input convention. Motor lag is included when checking closed-loop poles.
Limits: 0.8 m/s wheel speed, 4 m/s2 commanded acceleration and 20 kHz step rate. Steering splits forward speed between the wheels:
vL = v − ωb/2, vR = v + ωb/2
ω: commanded turn rate; b: wheel separation.
State estimation
The four-state pitch Kalman filter estimates angle, angular rate, gyro bias and the mechanical balance point. Unlike a gravity-only accelerometer correction, its model accounts for wheel acceleration and IMU lever-arm motion.
s = [θ, θ̇, bg, d]T
θ̈ = α(θ − d) − cθθ̇ − βa
ygyro = θ̇ + bg
yaccel ≈ θ − (a + hIMUθ̈)/g
d: balance-point angle; bg: gyro bias. a is the lagged command; α, β and cθ are plant constants.
The controller uses pitch relative to the learned balance point, with the LQR integral reduced to 25% strength. Complementary and two-state Kalman filters remain selectable for comparison; innovation gates and covariance checks reject inconsistent data.
Each wheel has an angle/speed Kalman filter: prediction every 2 ms, encoder correction every 10 ms. A delayed measurement model handles the 2.3 ms encoder latency:
sw = [φ, φ̇]T, yenc = φ − τencφ̇
φ: wheel angle; τenc: sensor delay.
Lost-step detection resynchronises wheel speed. Repeated encoder failures trigger a reported step-count fallback without jumping the hold reference. Wheel odometry does not measure tyre slip against the floor.
State-space models
The controller and estimators share the same mechanics, but use different states. Below: the upright acceleration-input plant, the four-state pitch KF and the two-state wheel KF. Hats denote estimates; T = 0.002 s is the control period.
1. Mechanics → LQR plant
Taking moments about the axle gives gravity, translational inertial torque and pitch damping. With body mass m, centre-of-mass height h and body inertia Iy:
Jp = Iy + mh2
Jpθ̈ = mgh sinθ
− mha cosθ − cθ̇
θ̈ ≈ αθ − βa − γθ̇
α = mgh/Jp, β = mh/Jp
γ = c/Jp
Small-angle approximation: sinθ ≈ θ, cosθ ≈ 1. a is forward wheel acceleration, not motor torque.
Add travel dynamics ẋ = v, v̇ = a and position-error integral η̇ = ex. For a stationary reference, z = [η, ex, ev, θ, θ̇]T. The repository's nominal gain design omits damping and actuator lag:
ż = ALz + BLa
AL =01000001000000000001000α0BL =0010−βFor the damped physical plant, the last diagonal entry is −γ. The gain-design matrix above matches the actual repository.
J = ∫0∞(zTQz + Ra2)dt
ALTP + PAL
− PBLR−1BLTP + Q = 0
K = R−1BLTP, a = −Kz
Firmware uses estimated travel/speed, pitch relative to the learned balance point, and a bounded pitch trim for the integral term. With offset KF, the trim integral runs at 25% strength; limits and anti-windup remain active.
Actuator lag
The simulation tracks commanded wheel velocity vcmd through a first-order lag τm = 35 ms. Locally, before limits and step quantisation:
τmv̇ + v = vcmd
v̇cmd = acmd
τmȧ + a = acmd
af,k = (1 − λ)af,k−1 + λacmd,k−1
λ = T/(τm + T)
The pitch KF uses this discrete acceleration proxy from the previous tick, not measured wheel acceleration. The nonlinear simulation uses an exponential velocity-lag update and acceleration limits.
2. Pitch KF: balance point and gyro bias
Extend the linear pitch model with gyro bias bg and balance-point angle d. Both are constant in prediction, with random-walk process noise so the filter can adapt:
s = [θ, θ̇, bg, d]T
θ̈ = α(θ − d) − γθ̇ − βaf
ṡ = Aps + Bpaf + w
Forward Euler prediction gives Fp = I + TAp and Gp = TBp, exactly as implemented:
Qp = diag(0, qrT, qbT, qdT): rate disturbance, bias drift and balance-point drift. The q terms are squared noise amplitudes.
The gyro measures rate plus bias. Accelerometer-derived pitch also sees translation and angular acceleration at IMU height ℓ above the axle:
yg = θ̇ + bg + ng
ya ≈ θ − (af + ℓθ̈)/g + na
y = Hps + Dpaf + n
Rows: gyro, accelerometer angle. Substituting the pitch dynamics produces the lever-arm terms and input feedthrough. Firmware applies scalar gyro/accel updates sequentially; accel-norm and innovation gates reject unreliable corrections.
Controller outputs: θ̂ − d̂ and low-pass-filtered (yg − b̂g). The estimated rate state supports prediction; it is not directly the rate feedback sent to LQR.
3. Wheel KF: angle and speed
Each wheel has sw = [φ, Ω]T, with Ω = φ̇. The step generator changes speed at the tick and then holds it. Thus the known input is the change in commanded angular speed ΔΩcmd, not absolute speed:
sw,k− = Fwsw,k−1+ + GwΔΩcmd
Fw =1T01Gw =T1Qw / qw =T3/3T2/2T2/2TIntegrated white angular-acceleration noise models deviations from commanded motion. Prediction: 500 Hz; encoder updates: 100 Hz.
The encoder angle is τenc = 2.3 ms old. First-order back-projection gives φ(t − τ) ≈ φ(t) − τΩ(t):
yenc = Hwsw + nenc
Hw =1−τencx̂ = r(φ̂L + φ̂R)/2 − x0
v̂ = r(Ω̂L + Ω̂R)/2
r: wheel radius; x0: handover offset preserving position continuity. Count correction, unwrapping and axis signs precede the KF. Persistent disagreement triggers resynchronisation; wheel odometry cannot detect floor slip.
Kalman prediction and correction
Both estimators combine model prediction with measurement innovations. L below is the Kalman gain, distinct from the LQR gain K:
ŝ− = Fŝ+prev + Gu
P− = FP+prevFT + Q
ν = y − Hŝ− − Du
S = HP−HT + R
L = P−HTS−1
ŝ+ = ŝ− + Lν
P+ = (I − LH)P−(I − LH)T
+ LRLT
Noise covariances Q/R are estimator-specific. Pitch uses Joseph-form covariance updates; the wheel implementation uses the algebraically equivalent compact scalar update. Rejected observations leave the prediction unchanged.
Alternative: two-state pitch/bias KF
This selectable baseline integrates gyro rate rather than predicting pendulum dynamics. With s2 = [θ, bg]T, θ̇ = yg − bg and ḃg = 0:
s2,k− = F2s2,k−1+ + G2yg
F2 =1−T01G2 =T0H2 =10Q2 =qθT + qbT3/3−qbT2/2−qbT2/2qbTObservation: acceleration-compensated accelerometer angle. Bias random walk induces the negative cross-covariance; qθ and qb are squared noise amplitudes. Unlike the four-state KF, this model does not learn the mechanical balance point.
Hardware tests
LQR with position hold: four pitch-estimator/wheel-feedback combinations, two runs each of 0 → +0.3 m → 0. The balance-offset filter reduced final reported position error from roughly 14–15 mm to 4–5 mm. With offset pitch estimation, encoder feedback gave 21.0 mm tracking RMS versus 23.8 mm for step counting.
LQR + wheel KF + balance-offset pitch KF is my preferred, most stable overall hardware setup. These runs support the tracking/hold advantage; the smoother step-count acceleration-command metric is a separate trade-off. All four recorded configurations used LQR, so this dataset is not a hardware PID-vs-LQR ranking.
Embedded software
500 Hz TIM6 loop: IMU acquisition → state estimation → control → PWM step rates. Interrupt-driven encoder reads and DMA telemetry keep communication separate from the control path. Firmware modules isolate sensors, estimators, controllers, motor drivers and latched faults.
- 100 ms watchdog; explicit sensor, numerical and timing faults.
- Upright boot calibration; arming below 0.15 rad; fall fault beyond 0.70 rad.
- 0.5 s command timeout zeros drive/turn requests while balancing stays active.
- Host tests exercise the real C modules with a mocked HAL and simulated sensors, including encoder failure, teleoperation and fall faults.
The software stop is not an emergency stop; testing requires an independent motor-power cutoff.
Teleoperation
Tkinter keyboard interface over UART: ramped forward/turn commands, controller and estimator selection, live pitch/velocity plots and fault reporting. Releasing the keys holds the new position; disarming switches off the motors.
Earlier builds
The original project photographs below document the earlier hardware and chassis. The videos above show the current running robot.
