bode plot of a magnetic levitator

 Magnetic levitation is one of the classic teaching plants in control because of what its Bode plot reveals, so this is a fun one. Let me lay out where the plot comes from, then show it.

The plant is open-loop unstable

An electromagnet holds a steel ball at gap $𝑥$ below it. The attractive force grows as the ball gets *closer*, which is positive feedback — exactly what makes the thing want to either slam up or fall away. The force model is $𝐹=𝑘(𝑖\mathrm{/}𝑥{)}^2$, and the ball obeys

$$𝑚\ddot{𝑥}=𝑚𝑔-𝑘\frac{{𝑖}^2}{{𝑥}^2}\mathrm{.}$$

Linearizing about the hovering equilibrium $({𝑥}_0,{𝑖}_0)$ where $𝑚𝑔=𝑘{𝑖}_0^2\mathrm{/}{𝑥}_0^2$ gives

$$\frac{\mathrm{\Delta }𝑋(𝑠)}{\mathrm{\Delta }𝐼(𝑠)}=\frac{-{𝐾}_{𝑖}\mathrm{/}𝑚}{{𝑠}^2-{𝑎}^2},𝑎=\sqrt{\frac{2𝑔}{{𝑥}_0}}\mathrm{.}$$.

That ${𝑠}^2-{𝑎}^2$ in the denominator factors into $(𝑠-𝑎)(𝑠+𝑎)$: one pole in the right half-plane at $+𝑎$ (the instability) and one in the left half-plane at $-𝑎$. With a 1 cm gap, $𝑎=\sqrt{2\cdot 9.81\mathrm{/}0.01}\approx 44$ rad/s ($\approx$ 7 Hz) — notice it depends only on the gap, not the mass or current. (A real coil adds one more stable pole at $-𝑅\mathrm{/}𝐿$ from its $𝐿𝑑𝑖\mathrm{/}𝑑𝑡$ dynamics, steepening the high-frequency rolloff, but the interesting behavior lives in the mechanical pair.)

Let me load the plotting setup and draw it.

Reading the plot

The striking thing — and the whole reason maglev is a teaching favorite — is that the maglev's magnitude curve is *identical* to that of a perfectly stable system with a double pole at $-𝑎$. Both sit flat at the DC gain, both break at $𝜔=𝑎$, both roll off at $-40$ dB/decade with no resonant peak (the denominator $-{𝜔}^2-{𝑎}^2$ never gets small, so nothing rings).

The phase is where they diverge completely. The stable look-alike sweeps the familiar $0\mathrm{°}\to -180\mathrm{°}$, passing $-90\mathrm{°}$ at the corner. The maglev's phase stays **flat**. That happens because the right-half-plane pole contributes $+90\mathrm{°}$ of phase *lead* across the band, exactly canceling the left-half-plane pole's $-90\mathrm{°}$ of lag. (Depending on the sign convention from the linearization you'll see it pinned at $0\mathrm{°}$ or at $\pm 180\mathrm{°}$, but the point is it doesn't move.)

That frozen phase, decoupled from the magnitude rolloff, is the fingerprint of the RHP pole. It also breaks Bode's gain–phase relationship, which holds only for minimum-phase systems — which is precisely why you cannot read the maglev's instability off the magnitude plot at all. Two plants, same gain curve, opposite stability.

Why it matters for control

Because the open-loop plant has a pole in the right half-plane, the usual shortcut of reading gain and phase margins off the open-loop Bode plot doesn't tell you whether the closed loop is stable. You fall back to the full Nyquist criterion, $𝑍=𝑁+𝑃$ with $𝑃=1$: the Nyquist contour has to encircle $-1$ once counterclockwise just to *reach* stability, rather than avoid encirclement. In practice you wrap a lead or PD/PID compensator around the plant, adding phase lead near the crossover to pull that RHP pole into the left half-plane and let the ball actually hover.

Drag the air-gap slider and you'll see the only thing that moves is the pole frequency $𝑎=\sqrt{2𝑔\mathrm{/}{𝑥}_0}$ — a smaller gap pushes the unstable pole higher, meaning faster divergence and a harder, faster controller requirement.

Here's the Nyquist locus of the open-loop maglev. I've kept the coil's electrical pole in the model (the $-R/L$ term from before) so the plot is a proper curve rather than the degenerate line segment the idealized second-order plant gives — more on that below.

Reading the plot

The whole locus lives in the right half-plane — every point has positive real part. You can prove it directly: $L(j\omega) = \dfrac{K\,(p - j\omega)}{(\omega^2 + a^2)(p^2 + \omega^2)}$, and the real part $\dfrac{Kp}{(\omega^2+a^2)(p^2+\omega^2)}$ is strictly positive for all $\omega$. So the curve is a teardrop pinned to the origin at $\omega = \pm\infty$, bulging out to the loop gain on the positive real axis at $\omega = 0$. It traverses clockwise as $\omega$ runs from $-\infty$ to $+\infty$.

Now apply the criterion. The plant has $P = 1$ pole in the right half-plane, so for a stable closed loop the locus must encircle the $-1$ point **once counterclockwise** ($Z = N + P = 0$ requires $N = -1$). But $-1$ sits on the *negative* real axis, and the locus never crosses to that side at all. So $N = 0$, giving $Z = N + P = 1$: one unstable closed-loop pole. The ball won't hover under simple feedback.

Drag the gain slider and watch what it can't do. More gain just stretches the teardrop further out along the *positive* real axis — away from $-1$, never around it. Less gain shrinks it toward the origin. No proportional gain, of either sign, ever delivers the encirclement, which is the Nyquist-domain statement of what the flat phase curve told us earlier: this is a non-minimum-phase plant and the magnitude/gain knob alone is powerless.

To stabilize it you need something that bends the locus into the left half-plane near the critical point — phase lead. A PD or lead compensator rotates the high-frequency tail counterclockwise so the curve sweeps around $-1$ and supplies the missing encirclement.

(For the idealized second-order plant from the Bode discussion, with no coil pole, the same algebra collapses the teardrop onto the bare positive-real-axis segment from $K/a^2$ back to $0$ — same conclusion, just flattened.)

Anybots inc.

 Anybots inc.

This is the Anybots Telepresence robot. (now dead) 

Oct 26, 2012

  Anybots Robot with Choke Out Cancer, San Jose Non Profit. 

HBO - Silicon Valley put the Anybots logo in the Season 2 & 3 Opening Credits.

YC effectively spun out of Anybots. for some perspective.

https://x.com/anybots

The last working Anybots QB

 

https://www.youtube.com/watch?v=ZnHR_XmBSvA

This is the last of 120 robots make that is still running. 

If you have an Anybots QB and would like to get it running, please contact me. john.sokol@gmail.com

Remote Control Balancing Robot "Jiggy" #3dprinting #pidcontroller

They Tried Teleoperating an Excavator That is 40kms Away... Here´s What ...

Ditch the Angles, Embrace the Quaternions for IMU Work

 

 Today we're going to talk about something crucial for anyone working with Inertial Measurement Units (IMUs) and 3D orientation: quaternions. You've probably encountered Euler angles (roll, pitch, yaw) at some point, and they seem intuitive, right? Well, for IMU work, they are often a headache waiting to happen. Today, I'm going to convince you why quaternions are the superior way to represent and manipulate 3D rotations from your IMU data.

The Allure (and the Lie) of Euler Angles:

Euler angles are tempting because they break down any 3D rotation into three sequential rotations around defined axes (like X, Y, and Z). We can easily visualize tilting forward (pitch), turning sideways (yaw), and leaning (roll). This direct mapping to our intuitive understanding of movement makes them seem straightforward.  

However, this seemingly simple representation hides a nasty secret: gimbal lock.

Gimbal Lock: The Euler Angle Villain:

Imagine a physical gimbal system – rings rotating within rings. Gimbal lock occurs when two of these axes align. When this happens, you lose a degree of freedom in your rotation. Suddenly, two seemingly independent rotations become coupled, and you can no longer express a full range of 3D orientations.  

Mathematically, gimbal lock manifests as a singularity in the transformation equations between Euler angles and rotation matrices (or other orientation representations). This means that for certain specific orientations (typically when pitch approaches +/- 90 degrees), you lose the ability to independently control all three axes.

Why is Gimbal Lock a Disaster for IMUs?

• Data Interpretation Breakdown: When your IMU passes through or near a gimbal lock orientation, the Euler angle outputs become erratic and can jump wildly, even for smooth physical movements. This makes accurate interpretation of your sensor data nearly impossible.
• Control System Instability: If you're using IMU-derived Euler angles to control a robotic system or stabilize a platform, gimbal lock can lead to unpredictable and unstable behavior. Imagine a drone suddenly losing its ability to control yaw because it pitched too far!
• Interpolation and Filtering Issues: Many sensor fusion algorithms and filtering techniques rely on smooth and continuous representations of orientation. Gimbal lock introduces discontinuities and non-linearities that can severely degrade the performance of these algorithms.  

Enter the Hero: Quaternions

Quaternions, a mathematical concept extending complex numbers, offer a robust and elegant solution to the problems posed by Euler angles. A quaternion represents a 3D rotation using four numbers: one real part (often denoted as 'w' or 'q0') and three imaginary parts (often denoted as 'x', 'y', 'z' or 'q1', 'q2', 'q3').  

The Advantages of Quaternions for IMU Work:

1. Gimbal Lock Freedom: The most significant advantage is that quaternions do not suffer from gimbal lock. Their mathematical formulation avoids the singularities inherent in Euler angle representations. This ensures a smooth and continuous representation of orientation throughout the entire range of 3D motion.

2. Smooth Interpolation: Interpolating between two quaternion orientations (finding intermediate rotations) is well-defined and produces smooth, physically meaningful results using techniques like spherical linear interpolation (Slerp). Interpolating Euler angles can lead to bizarre and non-intuitive paths due to the singularities.  

3. Computational Efficiency: Manipulating quaternions for rotations (e.g., composing rotations, rotating vectors) is often more computationally efficient than using rotation matrices derived from Euler angles. This is crucial for real-time IMU processing on embedded systems.  

4. Compact Representation: While they use four numbers compared to three for Euler angles, quaternions are still a relatively compact way to represent 3D rotations, especially when compared to 3x3 rotation matrices (which have nine elements).

5. Uniqueness (Mostly): For a given 3D rotation, there are two quaternion representations (q and -q) that represent the same orientation. This "double cover" is a minor inconvenience compared to the severe issues of gimbal lock in Euler angles.

Working with Quaternions in IMU Applications:

• IMU Firmware Output: Many modern IMUs and their associated libraries directly provide orientation data as quaternions. This saves you the trouble of converting from raw sensor data (accelerometer, gyroscope, magnetometer) using sensor fusion algorithms.  
• Sensor Fusion Algorithms: Algorithms like Kalman filters and Madgwick filters often internally represent and update orientation using quaternions due to their stability and smooth behavior.  
• Rotation Operations: To rotate a vector using a quaternion, you perform a specific quaternion multiplication involving the vector (represented as a pure quaternion) and the rotation quaternion.  
• Conversion (When Necessary): While it's generally best to stick with quaternions for internal calculations, you might occasionally need to convert them to Euler angles for visualization or human understanding. However, be aware that this conversion can reintroduce the possibility of gimbal lock issues if not handled carefully, and the conversion from a quaternion to Euler angles is not unique.

In Conclusion:

For robust, stable, and accurate 3D orientation tracking and manipulation with IMUs, quaternions are the clear winner over Euler angles. While Euler angles might seem more intuitive initially, their susceptibility to gimbal lock makes them unreliable for many IMU applications. Embrace the power and elegance of quaternions, and you'll save yourself countless headaches and build more reliable and accurate systems.

So, the next time you're working with IMU data, remember this lesson: ditch the angles, and let the quaternions guide your rotations. You'll thank me later.

import numpy as np
import math

def quaternion_to_euler(q):
    """
    Converts a quaternion (w, x, y, z) to Euler angles (roll, pitch, yaw) in radians.
    Handles potential gimbal lock by checking the pitch range.
    """
    w, x, y, z = q

    # Roll (x-axis rotation)
    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    # Pitch (y-axis rotation)
    sinp = 2 * (w * y - z * x)
    if abs(sinp) >= 1:
        pitch = np.sign(sinp) * math.pi / 2 # Use +/- 90 degrees if out of range
    else:
        pitch = np.arcsin(sinp)

    # Yaw (z-axis rotation)
    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

def euler_to_quaternion(roll, pitch, yaw):
    """
    Converts Euler angles (roll, pitch, yaw) in radians to a quaternion (w, x, y, z).
    """
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

    return w, x, y, z

def quaternion_multiply(q1, q2):
    """
    Multiplies two quaternions.
    """
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2

    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    return w, x, y, z

def rotate_vector_by_quaternion(vector, quaternion):
    """
    Rotates a 3D vector by a given quaternion.
    """
    w, x, y, z = quaternion
    v_x, v_y, v_z = vector

    # Convert vector to a pure quaternion
    vector_q = (0, v_x, v_y, v_z)

    # Perform quaternion multiplication: q * v * q_conjugate
    q_conjugate = (w, -x, -y, -z)
    rotated_q = quaternion_multiply(quaternion_multiply(quaternion, vector_q), q_conjugate)

    return rotated_q[1], rotated_q[2], rotated_q[3]

# --- Working Example ---

print("--- Euler Angles and Potential Gimbal Lock ---")
roll_deg = 0
pitch_deg = 89  # Close to gimbal lock
yaw_deg = 0

print(f"Euler Angles (deg): Roll={roll_deg}, Pitch={pitch_deg}, Yaw={yaw_deg}")
roll_rad = np.deg2rad(roll_deg)
pitch_rad = np.deg2rad(pitch_deg)
yaw_rad = np.deg2rad(yaw_deg)

q_from_euler = euler_to_quaternion(roll_rad, pitch_rad, yaw_rad)
print(f"Quaternion from Euler: {q_from_euler:.4f}")

# Introduce a small change in yaw
yaw_deg_perturbed = 10
yaw_rad_perturbed = np.deg2rad(yaw_deg_perturbed)
q_from_euler_perturbed = euler_to_quaternion(roll_rad, pitch_rad, yaw_rad_perturbed)
print(f"Quaternion from Perturbed Yaw: {q_from_euler_perturbed:.4f}")

# Convert back to Euler angles
roll_back, pitch_back, yaw_back = quaternion_to_euler(q_from_euler)
print(f"Euler Angles back (deg): Roll={np.rad2deg(roll_back):.2f}, Pitch={np.rad2deg(pitch_back):.2f}, Yaw={np.rad2deg(yaw_back):.2f}")

roll_back_perturbed, pitch_back_perturbed, yaw_back_perturbed = quaternion_to_euler(q_from_euler_perturbed)
print(f"Euler Angles back (deg) - Perturbed Yaw: Roll={np.rad2deg(roll_back_perturbed):.2f}, Pitch={np.rad2deg(pitch_back_perturbed):.2f}, Yaw={np.rad2deg(yaw_back_perturbed):.2f}")
print("Notice how a small change in yaw near high pitch can cause large changes in roll/yaw when converting back.")

print("\n--- Quaternion Rotation ---")
initial_vector = (1, 0, 0)  # A vector pointing along the x-axis
rotation_degrees = (0, 45, 0) # Rotate 45 degrees around the y-axis
rotation_radians = np.deg2rad(rotation_degrees)
rotation_quaternion = euler_to_quaternion(*rotation_radians)
print(f"Rotation Quaternion (for 45 deg around y): {rotation_quaternion:.4f}")

rotated_vector = rotate_vector_by_quaternion(initial_vector, rotation_quaternion)
print(f"Initial Vector: {initial_vector}")
print(f"Rotated Vector: {rotated_vector:.4f}")

print("\n--- Smooth Quaternion Interpolation (SLERP - simplified) ---")
def slerp_simplified(q1, q2, t):
    """
    Simplified SLERP for demonstration (assumes normalized quaternions).
    """
    dot = np.dot(q1, q2)

    # Ensure shortest path
    if dot < 0:
        q2 = -np.array(q2)
        dot = -dot

    if dot > 0.9995:
        # Quaternions are very close, linear interpolation is sufficient
        result = q1 + t * (q2 - q1)
        return result / np.linalg.norm(result)

    theta_0 = np.arccos(np.clip(dot, -1, 1))
    sin_theta_0 = np.sin(theta_0)
    theta = theta_0 * t
    sin_theta = np.sin(theta)

    s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
    s1 = sin_theta / sin_theta_0

    result = (s0 * np.array(q1)) + (s1 * np.array(q2))
    return result / np.linalg.norm(result)

q_start = (1, 0, 0, 0)  # No rotation
q_end = euler_to_quaternion(np.deg2rad(30), np.deg2rad(60), np.deg2rad(90))

num_steps = 5
for i in range(num_steps + 1):
    t = i / num_steps
    q_interpolated = slerp_simplified(q_start, q_end, t)
    euler_interpolated = quaternion_to_euler(q_interpolated)
    print(f"t={t:.2f}, Quaternion={q_interpolated:.4f}, Euler (deg)={np.rad2deg(euler_interpolated):.2f}")
print("Notice the smooth transition in both quaternion and Euler angles during interpolation.")

Explanation of the Code:

1. quaternion_to_euler(q):

   • Takes a quaternion (w, x, y, z) as input.
   • Implements the standard formulas to convert a quaternion to roll, pitch, and yaw (in radians).
   • Includes a check for potential gimbal lock around pitch = +/- 90 degrees to avoid NaN values from arcsin.

2. euler_to_quaternion(roll, pitch, yaw):

   • Takes Euler angles (roll, pitch, yaw) in radians as input.
   • Implements the standard formulas to convert Euler angles to a quaternion (w, x, y, z).

3. quaternion_multiply(q1, q2):

   • Performs quaternion multiplication of two quaternions. This is essential for composing rotations.

4. rotate_vector_by_quaternion(vector, quaternion):

   • Demonstrates how to rotate a 3D vector using a quaternion.
   • Converts the vector into a pure quaternion (with a real part of 0).
   • Applies the quaternion rotation formula: q * v * q_conjugate, where v is the pure quaternion representing the vector and q_conjugate is the conjugate of the rotation quaternion.

5. Working Example:

   • Euler Angles and Potential Gimbal Lock:

     • Sets Euler angles with a pitch close to 90 degrees (a region prone to gimbal lock).
     • Converts these Euler angles to a quaternion and then back to Euler angles.
     • Introduces a small perturbation in the yaw angle and repeats the conversion.
     • Observation: Notice how a small change in yaw near a high pitch value can result in significant and potentially unexpected changes in the recovered roll and yaw angles. This illustrates the instability around gimbal lock.

   • Quaternion Rotation:

     • Defines an initial vector and a rotation in Euler angles.
     • Converts the Euler rotation to a quaternion.
     • Rotates the vector using the quaternion multiplication method.

   • Smooth Quaternion Interpolation (Simplified SLERP):

     • Defines two quaternions representing different orientations.
     • Implements a simplified version of Spherical Linear Interpolation (SLERP) to find intermediate rotations between the two quaternions.
     • Converts the interpolated quaternions back to Euler angles for visualization.
     • Observation: Notice how the quaternion interpolation produces a smooth and gradual change in both the quaternion components and the resulting Euler angles, even through orientations that might cause issues with direct Euler angle interpolation.

How to Use This Code for IMU Work:

1. IMU Data: If your IMU directly outputs quaternion data, you can use that directly for orientation representation and manipulation.
2. Sensor Fusion: If you're implementing your own sensor fusion (e.g., using accelerometer and gyroscope data to estimate orientation), you would typically perform the fusion calculations directly on quaternions to avoid gimbal lock. Libraries like scipy.spatial.transform.Rotation in Python provide tools for quaternion manipulation and conversion.
3. Rotation and Transformation: Use the rotate_vector_by_quaternion function to rotate sensor readings (like acceleration vectors) from the sensor frame to a world frame based on the IMU's orientation.
4. Interpolation: If you need to animate or smooth orientation data, use quaternion interpolation techniques like Slerp for more stable and visually correct results.
5. Conversion (Use with Caution): Only convert quaternions to Euler angles for final visualization or when a human-readable format is strictly required, and be mindful of the potential for gimbal lock artifacts in the Euler angle representation.

This working example provides a foundation for understanding and using quaternions in your IMU projects. Remember to prioritize quaternion-based operations for internal calculations and only convert to Euler angles when absolutely necessary for human interpretation.