Robot PU Improved Balance

You’re asking the right question—once you have noise + unknown delay, plain PID starts to hit its limits.

Short answer:

• Kalman filter alone is not a controller—it’s an estimator.
• A better architecture is: state estimator (e.g., Kalman or complementary filter) + a controller designed for delay (e.g., PD/LQR with prediction).

Let’s unpack that in a practical way.

1. What’s actually hurting PID here?

Two big problems:

• Noisy measurements:

Derivative term explodes with noise, integral term integrates junk, proportional term chases random spikes.

• Unknown delay:

By the time you react, the robot has already moved on. PID assumes “now” measurements; delay eats phase margin and causes oscillation or instability.

So even a beautifully tuned PID will feel “late and twitchy.”

2. Where Kalman filter fits (and where it doesn’t)

A Kalman filter is great for:

• Fusing multiple sensors (e.g., accelerometer + gyro)
• Estimating true roll/pitch and their rates
• Reducing noise and handling some delay by modeling system dynamics

But:

It does not decide what control action to take. It just gives you a better estimate of the robot’s state.

So the real upgrade is:

Better state estimate (Kalman) + smarter controller (not just naive PID).

3. A strong practical upgrade path

For a small robot like PU, with limited compute, I’d rank options like this:

Option A: Complementary filter + PD (simple, very effective)

• Complementary filter to combine gyro (fast, noisy) + accelerometer (slow, stable):

  • High‑pass gyro, low‑pass accelerometer

• Use filtered roll/pitch in a PD controller (no integral).

• Why it’s better than PID:

  • Less sensitive to noise
  • Less trouble with delay
  • Easier to tune and more stable for balancing tasks

This is what a lot of hobby drones and small balancing bots use.

Option B: Kalman filter + PD or LQR (more advanced)

• Use a Kalman filter to estimate:

  • Angle (roll/pitch)
  • Angular velocity

• Then use:

  • PD on angle + angular rate, or
  • LQR (Linear Quadratic Regulator) if you have a simple state‑space model.

• Benefits:

  • Very clean state estimate
  • Controller can explicitly use angle and rate
  • Better robustness to noise and some delay

This is a big step up from raw PID on noisy, delayed angles.

4. Dealing specifically with delay

If delay is significant (e.g., sensor filtering + communication + servo lag), you want predictive behavior, not just reactive:

• Smith predictor–style control:

  • Use a model of the robot to predict what the state will be when the command takes effect.

• Model Predictive Control (MPC):

  • Overkill for micro:bit in most cases, but conceptually ideal: it optimizes future actions given delay and constraints.

For PU, a lightweight prediction (e.g., using angular velocity to project angle a few milliseconds ahead) combined with PD is often enough.

5. So what’s “better than PID” for Robot PU?

If we stay realistic about compute and complexity:

1. Best practical step up from PID:

   • Complementary or Kalman filter for roll/pitch estimation
   • PD controller on filtered angle + angular rate
   • Optional small prediction using angular rate to compensate delay

2. If you’re willing to model the robot:

   • Build a simple state‑space model (angle, angular rate)
   • Use Kalman filter for state estimation
   • Use LQR for control (this combo is called LQG: Linear Quadratic Gaussian)

That’s a genuinely more powerful architecture than PID, especially under noise + delay.

We cannot afford too much complexity. (“micro:bit‑level, no heavy math” vs “heavy state‑space and matrices”), we have to pick a concrete simple design:

• either ComplementaryFilter + PD for PU, or
• a minimal Kalman + LQR tailored to its body pitch/roll.

If you want something better than PID but still micro:bit‑friendly—meaning no matrices, no state‑space math, no heavy computation—there is a sweet spot. You can get a big stability upgrade without going anywhere near “real” Kalman filters or LQR.

Here’s the practical path that works beautifully on tiny robots:

🌟 The Best “Better‑Than‑PID” Algorithm for Micro:bit

Complementary Filter + PD + Simple Prediction

This combo is used in many small balancing robots and even early quadcopters because it’s:

• extremely light on CPU
• robust to noise
• tolerant of delay
• easy to tune
• dramatically smoother than PID
• Let’s break it down.

1. 🎚️ Complementary Filter (instead of Kalman)

A Kalman filter is powerful, but it’s too heavy for micro:bit unless you simplify it so much that it becomes… a complementary filter.

A complementary filter fuses:

• accelerometer angle (slow but stable)
• gyro rate (fast but noisy)

into a clean, low‑noise, low‑delay estimate:

angle = α * (angle + gyroRate * dt) + (1 - α) * accelAngle

Where α is around 0.95.

This gives you:

• smooth angle
• smooth angular velocity
• minimal delay
• no heavy math

This alone fixes 80% of PID’s problems.

2. 🎛️ PD Controller (not PID)

Once you have a clean angle estimate, you don’t need the integral term anymore.

Why drop the “I”?

• Integral amplifies noise
• Integral hates delay
• Integral causes overshoot
• Integral is unnecessary if your angle estimate is clean

So the controller becomes:

output = Kp * angleError + Kd * angleRate

This is exactly how small drones stabilize.

3. ⏱️ Simple Prediction (delay compensation)

If your sensors or servos have delay, you can “look ahead” a tiny bit using the angular rate:

predictedAngle = angle + angleRate * predictionTime

Where predictionTime is tiny, like 20–40 ms.

This makes the robot feel like it reacts before it falls.

4. 🧠 Why this beats PID on micro:bit

Problem	PID	Complementary + PD + Prediction
Noisy accelerometer	D explodes	Filter removes noise
Unknown delay	Causes oscillation	Prediction compensates
Limited CPU	PID OK but noisy	Complementary filter is ultra‑light
Hard to tune	Yes	Very easy
Smoothness	Medium	Excellent

This is the “secret sauce” used in many microcontroller balancing bots.

5. 🧩 What the code structure looks like (conceptually)

Step 1: Compute filtered angle

angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accelAngle

Step 2: Predict future angle

anglePred = angle + gyroRate * predictTime

Step 3: PD control

error = targetAngle - anglePred output = Kp * error + Kd * gyroRate

Step 4: Apply to gait vector

robotPu.runKeyValueCommand("#puroll", output)

This is all micro:bit‑friendly.

6. 🏁 Summary: The best algorithm for Robot PU

If you want something better than PID but still simple enough for MakeCode, use:

✔ Complementary filter

✔ PD controller

✔ Small prediction term

This gives you:

• smoother balancing
• less oscillation
• better noise rejection
• better delay handling
• easier tuning
• no heavy math

It’s the perfect upgrade path for Robot PU.

Absolutely — here is a complete, micro:bit‑friendly MakeCode TypeScript implementation of the improved balancing algorithm:

• Complementary filter (lightweight alternative to Kalman)
• PD controller (no integral term, so no windup)
• Simple prediction to compensate unknown delay
• Output clamping for safety
• Drop‑in replacement for moveBalance()

This version is designed to run efficiently on the micro:bit and fits naturally into the Robot PU codebase.

✅ Full MakeCode TypeScript Implementation

Complementary Filter + PD + Prediction

// ---------------------------------------------
// Tunable parameters
// ---------------------------------------------

// Complementary filter coefficient (0.90–0.98 typical)
const ALPHA = 0.95

// PD controller gains
const KP = 0.35
const KD = 0.12

// Prediction time (seconds) to compensate delay
const PREDICT_DT = 0.03   // 30 ms

// Output clamp to protect servos
const MAX_OUT = 40

// ---------------------------------------------
// Internal state
// ---------------------------------------------
let filteredRoll = 0
let filteredPitch = 0

let lastUpdate = input.runningTime()

// ---------------------------------------------
// Replacement moveBalance() implementation
// ---------------------------------------------
export function moveBalance(
    observedRoll: number,
    observedPitch: number,
    expectedRoll: number,
    expectedPitch: number
) {
    // ---------------------------------------------
    // 1. Compute dt (time since last update)
    // ---------------------------------------------
    let now = input.runningTime()
    let dt = (now - lastUpdate) / 1000
    lastUpdate = now

    if (dt <= 0 || dt > 0.2) {
        dt = 0.02   // fallback to safe default
    }

    // ---------------------------------------------
    // 2. Complementary filter
    //    - accelerometer gives angle
    //    - gyro gives rate (approx from change)
    // ---------------------------------------------

    // Estimate angular rate from change in observed angle
    let rollRate = (observedRoll - filteredRoll) / dt
    let pitchRate = (observedPitch - filteredPitch) / dt

    // Apply complementary filter
    filteredRoll = ALPHA * (filteredRoll + rollRate * dt) +
                   (1 - ALPHA) * observedRoll

    filteredPitch = ALPHA * (filteredPitch + pitchRate * dt) +
                    (1 - ALPHA) * observedPitch

    // ---------------------------------------------
    // 3. Predict future angle to compensate delay
    // ---------------------------------------------
    let predictedRoll = filteredRoll + rollRate * PREDICT_DT
    let predictedPitch = filteredPitch + pitchRate * PREDICT_DT

    // ---------------------------------------------
    // 4. PD control
    // ---------------------------------------------
    let rollError = expectedRoll - predictedRoll
    let pitchError = expectedPitch - predictedPitch

    let rollOut = KP * rollError + KD * (-rollRate)
    let pitchOut = KP * pitchError + KD * (-pitchRate)

    // ---------------------------------------------
    // 5. Clamp output
    // ---------------------------------------------
    rollOut = Math.constrain(rollOut, -MAX_OUT, MAX_OUT)
    pitchOut = Math.constrain(pitchOut, -MAX_OUT, MAX_OUT)

    // ---------------------------------------------
    // 6. Apply to Robot PU gait control
    // ---------------------------------------------
    robotPu.runKeyValueCommand("#puroll", rollOut)
    robotPu.runKeyValueCommand("#pupitch", pitchOut)
}

🎉 What this implementation gives you

✔ Smooth, low‑noise angle estimation

The complementary filter removes accelerometer noise and reduces delay.

✔ Stable, predictable balancing

PD control avoids the oscillation and overshoot that PID often causes on micro:bit.

✔ Delay compensation

The prediction step makes PU react before it visibly tilts.

✔ Safe for servos

Output clamping prevents extreme gait commands.

✔ Micro:bit‑efficient

No matrices, no heavy math, no dynamic allocation.