← Dynamical Systems as ROS Nodes

Crazyflie Multi-Sensor Fusion: From Python to ROS2

In the Crazyflie Sensor Fusion tutorial, we designed a multi-sensor Kalman filter system in Python. Now we’ll deploy this system as ROS2 nodes, demonstrating a more complex architecture with three separate sensor nodes.

Key Insight: Multi-sensor fusion maps naturally to ROS!

Unlike single-sensor systems (like TurtleBot), the Crazyflie has:

• Three sensor nodes: Motion capture, barometer, IMU

• One fusion node: Kalman filter subscribing to all three

This demonstrates how ROS handles multi-rate, heterogeneous sensor systems.

Conceptual Foundation: From Dynamical Systems to ROS Nodes

Recall from the Crazyflie Sensor Fusion tutorial that we represented our multi-sensor fusion system as a composition of dynamical systems:

Each block is a DynamicalSystem with state evolution (f) and observation (h) functions. The sensor fusion node combines data from three separate sensor nodes (mocap, barometer, IMU).

The ROS Mapping

There is a direct correspondence between this dynamical system representation and a ROS node architecture:

The correspondence:

• Each dynamical system block becomes a ROS node

• Each data connection becomes a ROS topic

• The parameter dictionary is replaced by message passing

• The multi-sensor fusion becomes multi-topic subscription

Note: In the Python version, we had three separate sensor nodes publishing to different topics. In ROS, this multi-rate sensor fusion is handled naturally through the ROSNode staleness policies.

Adding Teleoperation

For aerial vehicles like the Crazyflie, we use joystick teleoperation instead of keyboard input. Teleoperation provides direct velocity control, bypassing the high-level navigation pipeline entirely:

Key Architectural Difference:

• Autonomous Navigation: Setpoint Generator → Position Controller → /cmd_vel → Plant

• Teleoperation: Joystick Teleop → /cmd_vel → Plant

The Joystick Teleop node publishes velocity commands directly to /cmd_vel, completely bypassing both the setpoint generator and position controller. The multi-sensor Kalman filter continues running to provide state estimates for monitoring and visualization.

This design pattern is common in aerial robotics: teleoperation provides low-level velocity control for manual flight, while autonomous mode uses the full planning and control pipeline for waypoint following.

Note

This tutorial implements the basic ROS node architecture (without teleop). The teleop version will be demonstrated in a later tutorial.

Individual Node Blocks

Each ROS node wraps a dynamical system block. Here are the individual components:

Setpoint Generator

Position Controller

Crazyflie Plant

Kalman Filter Observer

In the following sections, we’ll implement each of these blocks as ROSNode instances, demonstrating how multi-sensor fusion from the theory notebook translates directly to ROS’s multi-topic subscription pattern.

Architecture Comparison: Python vs ROS2 Multi-Sensor System

Python Simulation (theory notebook):

# Concatenate measurements from 3 sensors
y_mocap = mocap_sensor(xk) + noise_mocap
y_baro = baro_sensor(xk) + noise_baro
y_imu = imu_sensor(xk) + noise_imu
yk_combined = np.vstack([y_mocap, y_baro, y_imu])

# Single KF update with stacked measurements
xhat_P = observer.step(y=yk_combined, ...)

ROS2 Nodes (this notebook):

setpoint_node → /setpoint (Vector3)
                     ↓
controller_node → /cmd_vel (Twist)
                     ↓
crazyflie_simulator_node → /mocap (Vector3)
                         → /baro (Float64)
                         → /imu (Vector3)
                         → /true_state (Odometry)
      ↓                  ↓                  ↓
      └──────────────────┴──────────────────┘
                         ↓
              kalman_filter_node → /estimate (Odometry)

Note

The three sensor topics converge at the Kalman filter node. This is the hallmark of sensor fusion in ROS!

ROS2 Multi-Sensor System Architecture

Our system consists of five ROS nodes (vs four for TurtleBot):

Node 1: Setpoint Generator

• Publishes: /setpoint (Vector3)

• Function: Generates 3D position references

Node 2: Position Controller

• Subscribes: /setpoint, /estimate

• Publishes: /cmd_vel (Twist)

• Function: 3D proportional position control

Node 3: Crazyflie Simulator

• Subscribes: /cmd_vel

• Publishes: /mocap, /baro, /imu, /true_state

• Function: Simulates dynamics + three sensors

Node 4: Multi-Sensor Kalman Filter

• Subscribes: /mocap, /baro, /imu, /cmd_vel

• Publishes: /estimate (Odometry)

• Function: Fuses 3 heterogeneous sensors

Setup: Imports and Core Functions

import numpy as np
import matplotlib.pyplot as plt
from pykal import DynamicalSystem
from pykal.ros.ros_node import ROSNode
from pykal.algorithm_library.estimators.kf import KF
from pykal.data_change import corrupt

# ROS message types
from geometry_msgs.msg import Twist, Vector3
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import rclpy
import time

print("Imports successful! Ready to deploy Crazyflie to ROS.")

Core Crazyflie Functions (from Theory Notebook)

# ============================================================================
# Crazyflie Dynamics
# ============================================================================


def crazyflie_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:
    """3D constant-velocity dynamics: [x, y, z, vx, vy, vz]."""
    pos = xk[:3]
    vel = xk[3:]
    pos_new = pos + vel * dt
    vel_new = uk  # Assume instantaneous velocity response
    return np.vstack([pos_new, vel_new])


def crazyflie_h_mocap(xk: np.ndarray) -> np.ndarray:
    """Motion capture: measures [x, y, z]."""
    return xk[:3]


def crazyflie_h_baro(xk: np.ndarray) -> np.ndarray:
    """Barometer: measures z only."""
    return xk[2:3]


def crazyflie_h_imu(xk: np.ndarray) -> np.ndarray:
    """IMU: measures velocity [vx, vy, vz]."""
    return xk[3:]


def h_multisensor(xk: np.ndarray) -> np.ndarray:
    """Combined sensor model: [mocap(3), baro(1), imu(3)] = 7D."""
    return np.vstack([crazyflie_h_mocap(xk), crazyflie_h_baro(xk), crazyflie_h_imu(xk)])


# ============================================================================
# KF Jacobians
# ============================================================================


def compute_F_crazyflie(dt: float) -> np.ndarray:
    """State transition Jacobian (constant for linear system)."""
    I3 = np.eye(3)
    return np.block([[I3, I3 * dt], [np.zeros((3, 3)), I3]])


def compute_H_multisensor() -> np.ndarray:
    """Measurement Jacobian for multi-sensor fusion (7x6)."""
    return np.array(
        [
            [1, 0, 0, 0, 0, 0],  # mocap x
            [0, 1, 0, 0, 0, 0],  # mocap y
            [0, 0, 1, 0, 0, 0],  # mocap z
            [0, 0, 1, 0, 0, 0],  # baro z
            [0, 0, 0, 1, 0, 0],  # imu vx
            [0, 0, 0, 0, 1, 0],  # imu vy
            [0, 0, 0, 0, 0, 1],  # imu vz
        ]
    )


# ============================================================================
# Noise Covariances
# ============================================================================

Q_crazyflie = np.diag([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])  # Process noise

# Individual sensor noise
R_mocap = np.diag([0.005, 0.005, 0.005])  # 5mm std
R_baro = np.array([[0.1]])  # 10cm std
R_imu = np.diag([0.1, 0.1, 0.1])  # 0.1 m/s std

# Combined (block-diagonal)
R_combined = np.block(
    [
        [R_mocap, np.zeros((3, 1)), np.zeros((3, 3))],
        [np.zeros((1, 3)), R_baro, np.zeros((1, 3))],
        [np.zeros((3, 3)), np.zeros((3, 1)), R_imu],
    ]
)

print("Core Crazyflie functions defined!")
print(f"  State dimension: 6 (position + velocity)")
print(f"  Measurement dimension: 7 (mocap:3 + baro:1 + imu:3)")

---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
Cell In[1], line 6
      1 # ============================================================================
      2 # Crazyflie Dynamics
      3 # ============================================================================
----> 6 def crazyflie_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:
      7     """3D constant-velocity dynamics: [x, y, z, vx, vy, vz]."""
      8     pos = xk[:3]

NameError: name 'np' is not defined

Node 1: Setpoint Generator

ROS Wrapper Pattern: Publishes 3D position setpoints.

Message Format: Vector3 = (3,) array [x, y, z]

Key Difference from TurtleBot: No orientation (just position for quadrotor)

def create_setpoint_node(initial_position, transitions=None, rate_hz=10.0):
    """
    Create ROS node that publishes position setpoints.

    Parameters
    ----------
    initial_position : np.ndarray
        Starting position [x, y, z]
    transitions : list of tuples
        Each tuple is (time, new_position)
    rate_hz : float
        Publishing rate
    """
    if transitions is None:
        transitions = []

    current_setpoint = np.array(initial_position).reshape(-1, 1)
    transition_idx = 0

    def setpoint_callback(tk):
        nonlocal current_setpoint, transition_idx

        # Check for transitions
        if transition_idx < len(transitions):
            t_switch, new_pos = transitions[transition_idx]
            if tk >= t_switch:
                current_setpoint = np.array(new_pos).reshape(-1, 1)
                transition_idx += 1

        # Return as Vector3
        return {"setpoint": current_setpoint.flatten()}

    node = ROSNode(
        node_name="setpoint_generator",
        callback=setpoint_callback,
        subscribes_to=[],
        publishes_to=[
            ("setpoint", Vector3, "/setpoint"),
        ],
        rate_hz=rate_hz)

    return node


# Create setpoint node
setpoint_node = create_setpoint_node(
    initial_position=[0.0, 0.0, 1.0],
    transitions=[(20.0, [1.0, 1.0, 1.5]), (40.0, [0.0, 0.0, 1.0])],
    rate_hz=10.0)

print("Setpoint generator node created!")
print(f"  Publishes: /setpoint (Vector3)")
print(f"  Initial: [0, 0, 1.0] m")
print(f"  Transitions: t=20s → [1, 1, 1.5], t=40s → [0, 0, 1.0]")

Node 2: Position Controller

ROS Wrapper Pattern: 3D proportional control.

Controller Law:

\[ \vec{v}_{cmd} = K_p (\vec{r} - \hat{\vec{p}}) \]

where \(\vec{r} \in \mathbb{R}^3\) is the reference position and \(\hat{\vec{p}} \in \mathbb{R}^3\) is the estimated position.

def create_position_controller_node(Kp=0.8, max_vel=0.5, rate_hz=50.0):
    """
    Create ROS node for proportional position controller.
    """

    def controller_callback(tk, setpoint, estimate):
        """
        Args:
            tk (float): Current time
            setpoint (np.ndarray): (3) [x_r, y_r, z_r]
            estimate (np.ndarray): (13) Odometry

        Returns:
            dict: {'cmd_vel': (6) Twist}
        """
        # Extract position from setpoint
        r = setpoint

        # Extract position from estimate
        phat = estimate[:3]

        # Proportional control
        error = r - phat
        v_cmd = Kp * error
        v_cmd = np.clip(v_cmd, -max_vel, max_vel)

        # Pack as Twist: [vx, vy, vz, wx, wy, wz]
        cmd_vel = np.concatenate([v_cmd, np.zeros(3)])

        return {"cmd_vel": cmd_vel}

    node = ROSNode(
        node_name="position_controller",
        callback=controller_callback,
        subscribes_to=[
            ("/setpoint", Vector3, "setpoint"),
            ("/estimate", Odometry, "estimate"),
        ],
        publishes_to=[
            ("cmd_vel", Twist, "/cmd_vel"),
        ],
        rate_hz=rate_hz,
        required_topics={"setpoint", "estimate"})

    return node


controller_node = create_position_controller_node(Kp=0.8, max_vel=0.5, rate_hz=50.0)

print("Position controller node created!")
print(f"  Subscribes: /setpoint, /estimate")
print(f"  Publishes: /cmd_vel (Twist)")
print(f"  Gain: Kp={0.8}")

Node 3: Crazyflie Simulator with Multi-Sensor Output

ROS Wrapper Pattern: Publishes three separate sensor topics.

Dynamics: Linear 3D motion

\[ \vec{p}_{k+1} = \vec{p}_k + \vec{v}_k \Delta t \]

\[ \vec{v}_{k+1} = \vec{v}_{cmd} \]

Three Sensor Models:

1. Motion Capture: \(y_{mocap} = \vec{p} + \mathcal{N}(0, R_{mocap})\) (high precision)

2. Barometer: \(y_{baro} = p_z + \text{bias} + \mathcal{N}(0, R_{baro})\) (drift + noise)

3. IMU: \(y_{imu} = \vec{v} + \mathcal{N}(0, R_{imu}) + \text{spikes}\) (noise + vibration)

Note

Unlike TurtleBot’s single /odom topic, we publish three separate sensor topics to simulate realistic multi-sensor hardware.

def create_crazyflie_simulator_node(dt=0.01, rate_hz=100.0):
    """
    Create ROS node that simulates Crazyflie with multi-sensor output.
    """
    # Internal state: [x, y, z, vx, vy, vz]
    xk = np.array([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0]])

    def simulator_callback(tk, cmd_vel):
        """
        Simulate dynamics and generate noisy sensor measurements.

        Args:
            tk (float): Current time
            cmd_vel (np.ndarray): (6) Twist

        Returns:
            dict: {'mocap', 'baro', 'imu', 'true_state'}
        """
        nonlocal xk

        # Extract velocity commands
        uk = cmd_vel[:3].reshape(-1, 1)

        # Update state
        xk = crazyflie_f(xk, uk, dt)

        # Extract state components
        pos = xk[:3].flatten()
        vel = xk[3:].flatten()

        # Generate true sensor measurements
        mocap_clean = pos
        baro_clean = pos[2]
        imu_clean = vel

        # Add realistic noise
        # Mocap: Gaussian noise (high precision)
        mocap_noisy = mocap_clean + np.random.multivariate_normal(np.zeros(3), R_mocap)

        # Barometer: Bias + drift + Gaussian noise
        baro_noisy = corrupt.with_bias(baro_clean, bias=0.02)  # 2cm offset
        baro_noisy = baro_noisy + np.random.normal(0, np.sqrt(R_baro[0, 0]))

        # IMU: Gaussian noise + occasional spikes (vibration)
        imu_noisy = imu_clean + np.random.multivariate_normal(np.zeros(3), R_imu)
        if np.random.rand() < 0.03:  # 3% spike rate
            spike_idx = np.random.randint(0, 3)
            imu_noisy[spike_idx] += np.random.choice([-1, 1]) * 0.2

        # True state as Odometry (for visualization)
        true_state = np.concatenate(
            [
                pos,  # position
                [0.0, 0.0, 0.0, 1.0],  # quaternion (identity)
                vel,  # linear velocity
                [0.0, 0.0, 0.0],  # angular velocity
            ]
        )

        return {
            "mocap": mocap_noisy,
            "baro": np.array([baro_noisy]),
            "imu": imu_noisy,
            "true_state": true_state,
        }

    node = ROSNode(
        node_name="crazyflie_simulator",
        callback=simulator_callback,
        subscribes_to=[
            ("/cmd_vel", Twist, "cmd_vel"),
        ],
        publishes_to=[
            ("mocap", Vector3, "/mocap"),
            ("baro", Float64, "/baro"),
            ("imu", Vector3, "/imu"),
            ("true_state", Odometry, "/true_state"),
        ],
        rate_hz=rate_hz)

    return node


simulator_node = create_crazyflie_simulator_node(dt=0.01, rate_hz=100.0)

print("Crazyflie simulator node created!")
print(f"  Subscribes: /cmd_vel (Twist)")
print(f"  Publishes:")
print(f"    - /mocap (Vector3): 3D position, ~5mm noise")
print(f"    - /baro (Float64): Altitude, ~10cm noise + drift")
print(f"    - /imu (Vector3): Velocity, ~0.1 m/s noise + spikes")
print(f"    - /true_state (Odometry): Ground truth")
print(f"  Rate: 100 Hz (conceptually represents fastest sensor)")

Node 4: Multi-Sensor Kalman Filter

ROS Wrapper Pattern: Subscribes to three sensor topics + /cmd_vel.

Sensor Fusion: Stacks measurements into 7D vector

\[\begin{split} y_k = \begin{bmatrix} y_{mocap} \\ y_{baro} \\ y_{imu} \end{bmatrix} \in \mathbb{R}^7 \end{split}\]

Measurement Jacobian:

\[\begin{split} H = \begin{bmatrix} I_3 & 0 \\ \begin{bmatrix}0 & 0 & 1\end{bmatrix} & 0 \\ 0 & I_3 \end{bmatrix} \end{split}\]

Block-Diagonal Noise: \(R = \text{diag}(R_{mocap}, R_{baro}, R_{imu})\)

Note

This is where sensor fusion happens! The KF callback receives all three sensor messages and combines them optimally based on their noise characteristics.

def create_kalman_filter_node(dt=0.01, rate_hz=100.0, Q=None, R=None):
    """
    Create ROS node for multi-sensor Kalman filter.
    """
    if Q is None:
        Q = Q_crazyflie
    if R is None:
        R = R_combined

    # Initial estimate
    xhat = np.array([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0]])
    P = np.diag([0.1, 0.1, 0.1, 1.0, 1.0, 1.0])

    def filter_callback(tk, mocap, baro, imu, cmd_vel):
        """
        Multi-sensor KF update.

        Args:
            tk (float): Current time
            mocap (np.ndarray): (3) [x, y, z]
            baro (np.ndarray): (1) [z]
            imu (np.ndarray): (3) [vx, vy, vz]
            cmd_vel (np.ndarray): (6) Twist

        Returns:
            dict: {'estimate': (13) Odometry}
        """
        nonlocal xhat, P

        # Concatenate measurements: [mocap(3), baro(1), imu(3)] = 7D
        yk = np.vstack([mocap.reshape(-1, 1), baro.reshape(-1, 1), imu.reshape(-1, 1)])

        # Extract control input
        uk = cmd_vel[:3].reshape(-1, 1)

        # Compute Jacobians
        Fk = compute_F_crazyflie(dt)
        Hk = compute_H_multisensor()

        # Run KF update
        f_params = {"xk": xhat, "uk": uk, "dt": dt}
        h_params = {"xk": xhat}

        xhat_new, P_new = KF.f(
            x_P=(xhat, P),
            y=yk,
            u=uk,
            f=crazyflie_f,
            F=lambda **params: Fk,
            Q=lambda **params: Q,
            h=h_multisensor,
            H=lambda **params: Hk,
            R=lambda **params: R,
            f_params=f_params,
            F_params={},
            Q_params={},
            h_params=h_params,
            H_params={},
            R_params={})

        # Update state
        xhat = xhat_new
        P = P_new

        # Extract estimate
        pos_est = xhat[:3].flatten()
        vel_est = xhat[3:].flatten()

        # Convert to Odometry
        estimate = np.concatenate(
            [
                pos_est,
                [0.0, 0.0, 0.0, 1.0],  # quaternion
                vel_est,
                [0.0, 0.0, 0.0],  # angular velocity
            ]
        )

        return {"estimate": estimate}

    node = ROSNode(
        node_name="kalman_filter",
        callback=filter_callback,
        subscribes_to=[
            ("/mocap", Vector3, "mocap"),
            ("/baro", Float64, "baro"),
            ("/imu", Vector3, "imu"),
            ("/cmd_vel", Twist, "cmd_vel"),
        ],
        publishes_to=[
            ("estimate", Odometry, "/estimate"),
        ],
        rate_hz=rate_hz,
        required_topics={"mocap", "baro", "imu", "cmd_vel"},
        stale_config={
            "mocap": {"after": 0.2, "policy": "hold"},  # 10 Hz sensor
            "baro": {"after": 0.1, "policy": "hold"},  # 20 Hz sensor
            "imu": {"after": 0.02, "policy": "hold"},  # 100 Hz sensor
        })

    return node


kf_node = create_kalman_filter_node(dt=0.01, rate_hz=100.0)

print("Multi-sensor Kalman filter node created!")
print(f"  Subscribes:")
print(f"    - /mocap (Vector3)")
print(f"    - /baro (Float64)")
print(f"    - /imu (Vector3)")
print(f"    - /cmd_vel (Twist)")
print(f"  Publishes: /estimate (Odometry)")
print(f"  Fusion: Combines 3 sensors with optimal weighting")
print(f"  Staleness policies: mocap(200ms), baro(100ms), imu(20ms)")

Running the Multi-Sensor ROS2 System

# Initialize ROS2
rclpy.init()

# Create all nodes
print("Creating ROS2 nodes...")
setpoint_node.create_node()
controller_node.create_node()
simulator_node.create_node()
kf_node.create_node()
print("All nodes created!")

# Start all nodes
print("\nStarting nodes...")
setpoint_node.start()
print("  ✓ Setpoint generator running")
simulator_node.start()
print("  ✓ Crazyflie simulator running (publishing 3 sensor topics!)")
kf_node.start()
print("  ✓ Multi-sensor Kalman filter running")
controller_node.start()
print("  ✓ Position controller running")

print("\n🚀 Crazyflie multi-sensor ROS system is live!")
print("\nROS2 Topic Graph:")
print("  /setpoint ← setpoint_generator")
print("     ↓")
print("  position_controller → /cmd_vel")
print("     ↓")
print("  crazyflie_simulator → /mocap, /baro, /imu, /true_state")
print("     ↓         ↓        ↓")
print("     └─────────┴────────┘")
print("            ↓")
print("  kalman_filter (FUSION!) → /estimate")
print("     ↓ (feedback)")
print("  position_controller")

print("\n💡 Tip: Run 'rqt_graph' to see the multi-sensor fusion architecture!")
print("         You'll see THREE sensor topics converging at kalman_filter.")

Data Collection

# Data logger node
def create_data_logger_node():
    data_log = {
        "time": [],
        "setpoint": [],
        "cmd_vel": [],
        "true_state": [],
        "mocap": [],
        "baro": [],
        "imu": [],
        "estimate": [],
    }

    def logger_callback(tk, setpoint, cmd_vel, true_state, mocap, baro, imu, estimate):
        data_log["time"].append(tk)
        data_log["setpoint"].append(setpoint.copy())
        data_log["cmd_vel"].append(cmd_vel.copy())
        data_log["true_state"].append(true_state.copy())
        data_log["mocap"].append(mocap.copy())
        data_log["baro"].append(baro.copy())
        data_log["imu"].append(imu.copy())
        data_log["estimate"].append(estimate.copy())
        return {}

    node = ROSNode(
        node_name="data_logger",
        callback=logger_callback,
        subscribes_to=[
            ("/setpoint", Vector3, "setpoint"),
            ("/cmd_vel", Twist, "cmd_vel"),
            ("/true_state", Odometry, "true_state"),
            ("/mocap", Vector3, "mocap"),
            ("/baro", Float64, "baro"),
            ("/imu", Vector3, "imu"),
            ("/estimate", Odometry, "estimate"),
        ],
        publishes_to=[],
        rate_hz=100.0)

    return node, data_log


logger_node, data_log = create_data_logger_node()
logger_node.create_node()
logger_node.start()
print("Data logger started!")

# Run simulation
import time as pytime

T_sim = 60.0
print(f"\nRunning multi-sensor simulation for {T_sim} seconds...")
pytime.sleep(T_sim)

print(f"\nSimulation complete! Collected {len(data_log['time'])} samples.")

Stopping the System

print("Stopping nodes...")
logger_node.stop()
setpoint_node.stop()
controller_node.stop()
simulator_node.stop()
kf_node.stop()

rclpy.shutdown()
print("All nodes stopped. ROS2 shutdown complete.")

Visualization: Multi-Sensor Fusion Analysis

Show code cell source

Hide code cell source

# Convert to arrays
time_vec = np.array(data_log["time"])
true_states = np.array(data_log["true_state"])
estimates = np.array(data_log["estimate"])
mocap_meas = np.array(data_log["mocap"])
baro_meas = np.array(data_log["baro"])
imu_meas = np.array(data_log["imu"])
setpoints = np.array(data_log["setpoint"])

# Extract positions
true_pos = true_states[:, :3]
est_pos = estimates[:, :3]

# Plotting
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure(figsize=(15, 10))

# Plot 1: 3D Trajectory
ax = fig.add_subplot(2, 3, 1, projection="3d")
ax.plot(
    true_pos[:, 0],
    true_pos[:, 1],
    true_pos[:, 2],
    "b-",
    label="True",
    linewidth=2,
    alpha=0.7)
ax.plot(
    est_pos[:, 0],
    est_pos[:, 1],
    est_pos[:, 2],
    "r--",
    label="Estimate",
    linewidth=2,
    alpha=0.7)
ax.scatter(
    setpoints[:, 0],
    setpoints[:, 1],
    setpoints[:, 2],
    c="green",
    s=100,
    marker="*",
    label="Setpoints")
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_zlabel("Z (m)")
ax.set_title("3D Trajectory", fontweight="bold")
ax.legend()

# Plot 2: XY Trajectory
ax = fig.add_subplot(2, 3, 2)
ax.plot(true_pos[:, 0], true_pos[:, 1], "b-", label="True", linewidth=2, alpha=0.7)
ax.plot(est_pos[:, 0], est_pos[:, 1], "r--", label="Estimate", linewidth=2, alpha=0.7)
ax.scatter(setpoints[::500, 0], setpoints[::500, 1], c="green", s=100, marker="*")
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_title("XY Trajectory (Top View)", fontweight="bold")
ax.legend()
ax.grid(True, alpha=0.3)
ax.axis("equal")

# Plot 3: Z Position with ALL Sensors
ax = fig.add_subplot(2, 3, 3)
ax.plot(time_vec, true_pos[:, 2], "k-", label="Truth", linewidth=2, alpha=0.8)
ax.plot(time_vec, est_pos[:, 2], "r--", label="Fused Estimate", linewidth=2, alpha=0.7)
ax.scatter(
    time_vec[::100], mocap_meas[::100, 2], c="green", s=5, alpha=0.3, label="Mocap"
)
ax.scatter(
    time_vec[::50], baro_meas[::50].flatten(), c="purple", s=5, alpha=0.3, label="Baro"
)
ax.set_xlabel("Time (s)")
ax.set_ylabel("Z (m)")
ax.set_title("Altitude: Multi-Sensor Fusion", fontweight="bold")
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)

# Plot 4: X Velocity with IMU
ax = fig.add_subplot(2, 3, 4)
true_vel = true_states[:, 7:10]
est_vel = estimates[:, 7:10]
ax.plot(time_vec, true_vel[:, 0], "b-", label="True Vx", alpha=0.7)
ax.plot(time_vec, est_vel[:, 0], "r--", label="Est. Vx", alpha=0.7)
ax.scatter(
    time_vec[::100], imu_meas[::100, 0], c="gray", s=5, alpha=0.3, label="IMU Vx"
)
ax.set_xlabel("Time (s)")
ax.set_ylabel("Vx (m/s)")
ax.set_title("X Velocity: IMU Fusion", fontweight="bold")
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 5: Position Error
ax = fig.add_subplot(2, 3, 5)
pos_error = np.linalg.norm(true_pos - est_pos, axis=1)
ax.plot(time_vec, pos_error * 1000, "m-", linewidth=1.5)
ax.set_xlabel("Time (s)")
ax.set_ylabel("Error (mm)")
ax.set_title("3D Position Error", fontweight="bold")
ax.grid(True, alpha=0.3)

# Plot 6: Sensor Comparison for Z
ax = fig.add_subplot(2, 3, 6)
mocap_z_error = np.abs(mocap_meas[:, 2] - true_pos[:, 2])
baro_z_error = np.abs(baro_meas.flatten() - true_pos[:, 2])
fused_z_error = np.abs(est_pos[:, 2] - true_pos[:, 2])
ax.plot(time_vec, mocap_z_error * 1000, "g-", label="Mocap Error", alpha=0.7)
ax.plot(time_vec, baro_z_error * 1000, "purple", label="Baro Error", alpha=0.7)
ax.plot(
    time_vec, fused_z_error * 1000, "r--", label="Fused Error", linewidth=2, alpha=0.8
)
ax.set_xlabel("Time (s)")
ax.set_ylabel("Z Error (mm)")
ax.set_title("Fusion Improves Z Estimate", fontweight="bold")
ax.legend()
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

print(f"\nPerformance Metrics (ROS Multi-Sensor Deployment):")
print(f"  Mean position error: {np.mean(pos_error)*1000:.2f} mm")
print(f"  Max position error: {np.max(pos_error)*1000:.2f} mm")
print(f"  RMS position error: {np.sqrt(np.mean(pos_error**2))*1000:.2f} mm")

Summary: Multi-Sensor Python → ROS2 Deployment

We’ve successfully deployed the Crazyflie multi-sensor fusion system to ROS2!

Key Difference from TurtleBot:

Aspect	TurtleBot (Single-Sensor)	Crazyflie (Multi-Sensor)
Sensors	1 topic (/odom)	3 topics (/mocap, /baro, /imu)
KF Subscribes	2 topics	4 topics
Measurement dim	3D	7D (stacked)
Noise matrix	3×3	7×7 (block-diagonal)
ROS pattern	Simple	Sensor fusion convergence

Multi-Sensor ROS Pattern:

Python: Concatenate arrays in code
  y = np.vstack([y1, y2, y3])

ROS: Topics converge at subscriber
  /sensor1 ──┐
  /sensor2 ──┼──→ kalman_filter (fusion!)
  /sensor3 ──┘

Architecture Preservation:

• Block diagram → ROS graph (with 3 sensor branches)

• Sensor fusion visible in rqt_graph

• Same KF algorithm as theory notebook

Sensor Characteristics Preserved:

• Mocap: High precision (5mm)

• Barometer: Drift + bias (10cm)

• IMU: Noise + spikes (0.1 m/s)

Next Step: In the Gazebo tutorial, we’ll adapt this architecture to work with Gazebo’s actual sensor suite!

Architecture 2: Teleoperation Mode

In the previous section, we demonstrated autonomous navigation with setpoint tracking. Now we’ll demonstrate teleoperation mode, where a human operator directly controls the Crazyflie’s velocity using a joystick.

Key Architectural Difference:

• Autonomous (previous): Setpoint Gen → Position Controller → /cmd_vel → Plant → Multi-Sensor Observer

• Teleoperation (this section): Joystick Teleop → /cmd_vel → Plant → Multi-Sensor Observer

The teleoperation node (shown in RED in diagrams) is an external ROS2 tool, not wrapped in pykal.ROSNode. For aerial vehicles like the Crazyflie, we use joystick control instead of keyboard for smoother 3D maneuvering.

Nodes Used in Teleoperation Mode:

1. ✅ Crazyflie Simulator - Receives /cmd_vel from teleop

2. ✅ Multi-Sensor Kalman Filter - Continues fusing mocap, barometer, IMU data

3. ❌ Setpoint Generator - Not needed (bypassed)

4. ❌ Position Controller - Not needed (bypassed)

5. 🔴 Joystick Teleop - External tool (joy_node + teleop_twist_joy)

# Re-initialize ROS2 (if needed after previous shutdown)
if not rclpy.ok():
    rclpy.init()

# Create fresh simulator and KF nodes for teleoperation
print("Creating nodes for teleoperation mode...")
simulator_teleop_cf = create_crazyflie_simulator_node(dt=0.01, rate_hz=50.0)
kf_teleop_cf = create_kalman_filter_node(dt=0.01, rate_hz=50.0, Q=Q_crazyflie, R=R_multisensor)

simulator_teleop_cf.create_node()
kf_teleop_cf.create_node()
print("✓ Simulator and multi-sensor Kalman filter created")

# Start nodes
print("\nStarting nodes...")
simulator_teleop_cf.start()
print("  ✓ Crazyflie simulator running")
kf_teleop_cf.start()
print("  ✓ Multi-sensor Kalman filter running")

print("\n🚀 System ready for teleoperation!")
print("\nActive ROS2 Topics:")
print("  /cmd_vel ← (waiting for joystick teleop)")
print("  /mocap, /baro, /imu ← crazyflie_simulator (3 sensor topics!)")
print("  /estimate ← kalman_filter")
print("\nNOTE: Setpoint generator and position controller are NOT running (bypassed!)")

# Create a simulated joystick teleop node
def create_simulated_joystick_teleop():
    """
    Simulate joystick teleop with programmed 3D maneuvers.
    
    This mimics what teleop_twist_joy would publish,
    demonstrating the teleoperation architecture for aerial vehicles.
    """
    import time
    
    # Programmed 3D maneuver sequence (aerial flight)
    maneuvers = [
        (0, 5, (0.3, 0.0, 0.0)),       # 0-5s: Forward
        (5, 10, (0.2, 0.2, 0.1)),      # 5-10s: Forward-right + climb
        (10, 15, (0.0, 0.0, 0.0)),     # 10-15s: Hover
        (15, 20, (-0.2, 0.0, 0.0)),    # 15-20s: Backward
        (20, 25, (0.0, 0.3, 0.0)),     # 20-25s: Strafe right
        (25, 30, (0.0, 0.0, -0.1)),    # 25-30s: Descend
        (30, 35, (0.2, 0.0, 0.0)),     # 30-35s: Forward
    ]
    
    start_time = None
    
    def teleop_callback(tk):
        nonlocal start_time
        
        if start_time is None:
            start_time = tk
        
        elapsed = tk - start_time
        
        # Find current maneuver
        vx, vy, vz = 0.0, 0.0, 0.0
        for t_start, t_end, (vx_cmd, vy_cmd, vz_cmd) in maneuvers:
            if t_start <= elapsed < t_end:
                vx, vy, vz = vx_cmd, vy_cmd, vz_cmd
                break
        
        # Pack as Twist: [vx, vy, vz, wx, wy, wz]
        cmd_vel = np.array([vx, vy, vz, 0.0, 0.0, 0.0])
        
        return {"cmd_vel": cmd_vel}
    
    node = ROSNode(
        node_name="simulated_joystick_teleop",
        callback=teleop_callback,
        subscribes_to=[],
        publishes_to=[
            ("cmd_vel", Twist, "/cmd_vel"),
        ],
        rate_hz=50.0)
    
    return node


# Create and start simulated joystick teleop
print("Creating simulated joystick teleop...")
joystick_teleop = create_simulated_joystick_teleop()
joystick_teleop.create_node()
joystick_teleop.start()
print("✓ Simulated joystick teleop running (mimics joy_node + teleop_twist_joy)")
print("\nThis simulates a human operator flying the Crazyflie with a joystick!")
print("In real usage, you would run: ros2 run joy joy_node && ros2 run teleop_twist_joy teleop_node")

# Stop all teleoperation nodes
print("Stopping teleoperation nodes...")
teleop_logger_cf.stop()
joystick_teleop.stop()
simulator_teleop_cf.stop()
kf_teleop_cf.stop()

# Shutdown ROS2
rclpy.shutdown()
print("All nodes stopped. ROS2 shutdown complete.")

# Convert teleoperation data to arrays
time_teleop_cf = np.array(teleop_data_cf["time"])
cmd_teleop_cf = np.array(teleop_data_cf["cmd_vel"])
mocap_teleop = np.array(teleop_data_cf["mocap"])
baro_teleop = np.array(teleop_data_cf["baro"])
imu_teleop = np.array(teleop_data_cf["imu"])
est_teleop_cf = np.array(teleop_data_cf["estimate"])

# Extract 3D positions (ground truth from mocap)
mocap_x = mocap_teleop[:, 0]
mocap_y = mocap_teleop[:, 1]
mocap_z = mocap_teleop[:, 2]

# Extract estimates
est_x_cf = est_teleop_cf[:, 0]
est_y_cf = est_teleop_cf[:, 1]
est_z_cf = est_teleop_cf[:, 2]

# Extract velocity commands
vx_cmd_cf = cmd_teleop_cf[:, 0]
vy_cmd_cf = cmd_teleop_cf[:, 1]
vz_cmd_cf = cmd_teleop_cf[:, 2]

# Plotting
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure(figsize=(15, 10))

# Plot 1: 3D Trajectory (teleoperation)
ax = fig.add_subplot(2, 3, 1, projection='3d')
ax.plot(mocap_x, mocap_y, mocap_z, 'b-', linewidth=2, label='Mocap (Ground Truth)', alpha=0.7)
ax.plot(est_x_cf, est_y_cf, est_z_cf, 'r--', linewidth=2, label='KF Estimate', alpha=0.7)
ax.scatter(mocap_x[0], mocap_y[0], mocap_z[0], c='green', s=150, marker='o', label='Start')
ax.scatter(mocap_x[-1], mocap_y[-1], mocap_z[-1], c='red', s=150, marker='X', label='End')
ax.set_xlabel('X (m)', fontsize=10)
ax.set_ylabel('Y (m)', fontsize=10)
ax.set_zlabel('Z (m)', fontsize=10)
ax.set_title('Teleoperation: 3D Flight Path', fontsize=13, fontweight='bold')
ax.legend()

# Plot 2: XY Trajectory (top view)
ax = fig.add_subplot(2, 3, 2)
ax.plot(mocap_x, mocap_y, 'b-', linewidth=2, label='Mocap', alpha=0.7)
ax.plot(est_x_cf, est_y_cf, 'r--', linewidth=2, label='KF Estimate', alpha=0.7)
ax.scatter(mocap_x[0], mocap_y[0], c='green', s=150, marker='o')
ax.scatter(mocap_x[-1], mocap_y[-1], c='red', s=150, marker='X')
ax.set_xlabel('X (m)', fontsize=10)
ax.set_ylabel('Y (m)', fontsize=10)
ax.set_title('XY Trajectory (Top View)', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)
ax.axis('equal')

# Plot 3: Altitude (Z)
ax = fig.add_subplot(2, 3, 3)
ax.plot(time_teleop_cf, mocap_z, 'b-', label='Mocap Z', alpha=0.7)
ax.plot(time_teleop_cf, est_z_cf, 'r--', linewidth=2, label='KF Est. Z', alpha=0.7)
ax.plot(time_teleop_cf, baro_teleop, 'g:', alpha=0.5, label='Barometer')
ax.set_xlabel('Time (s)', fontsize=10)
ax.set_ylabel('Altitude (m)', fontsize=10)
ax.set_title('Altitude: Multi-Sensor Fusion', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 4: Velocity Commands
ax = fig.add_subplot(2, 3, 4)
ax.plot(time_teleop_cf, vx_cmd_cf, label='vx', alpha=0.7)
ax.plot(time_teleop_cf, vy_cmd_cf, label='vy', alpha=0.7)
ax.plot(time_teleop_cf, vz_cmd_cf, label='vz', alpha=0.7)
ax.set_xlabel('Time (s)', fontsize=10)
ax.set_ylabel('Command (m/s)', fontsize=10)
ax.set_title('Joystick Commands to /cmd_vel', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 5: Position Error
ax = fig.add_subplot(2, 3, 5)
pos_error_cf = np.sqrt((mocap_x - est_x_cf)**2 + (mocap_y - est_y_cf)**2 + (mocap_z - est_z_cf)**2)
ax.plot(time_teleop_cf, pos_error_cf * 1000, 'm-', linewidth=1.5)
ax.set_xlabel('Time (s)', fontsize=10)
ax.set_ylabel('Error (mm)', fontsize=10)
ax.set_title('Multi-Sensor KF Error', fontsize=13, fontweight='bold')
ax.grid(True, alpha=0.3)

# Plot 6: Sensor Data (IMU velocity)
ax = fig.add_subplot(2, 3, 6)
ax.plot(time_teleop_cf, imu_teleop[:, 0], label='IMU vx', alpha=0.6)
ax.plot(time_teleop_cf, imu_teleop[:, 1], label='IMU vy', alpha=0.6)
ax.plot(time_teleop_cf, imu_teleop[:, 2], label='IMU vz', alpha=0.6)
ax.set_xlabel('Time (s)', fontsize=10)
ax.set_ylabel('Velocity (m/s)', fontsize=10)
ax.set_title('IMU Sensor Data', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

print(f"\nTeleoperation Performance (3D Flight):")
print(f"  Mean position error: {np.mean(pos_error_cf)*1000:.2f} mm")
print(f"  Max position error: {np.max(pos_error_cf)*1000:.2f} mm")
print(f"  Total 3D distance: {np.sum(np.sqrt(np.diff(mocap_x)**2 + np.diff(mocap_y)**2 + np.diff(mocap_z)**2)):.2f} m")
print(f"  Altitude range: [{np.min(mocap_z):.2f}, {np.max(mocap_z):.2f}] m")

Summary: Two ROS Architectures with Multi-Sensor Fusion

This notebook demonstrated two distinct control architectures for the Crazyflie ROS system, both utilizing multi-sensor fusion:

Architecture 1: Autonomous Navigation (Setpoint Tracking)

Nodes Active:

• ✅ Setpoint Generator (pykal.ROSNode)

• ✅ Position Controller (pykal.ROSNode)

• ✅ Crazyflie Simulator (pykal.ROSNode) - publishes 3 sensor topics

• ✅ Multi-Sensor Kalman Filter (pykal.ROSNode) - fuses mocap + baro + IMU

Data Flow: Setpoint Gen → Controller → /cmd_vel → Plant → [mocap, baro, IMU] → Observer → (feedback)

Use Case: Autonomous waypoint following with high-level planning

---

Architecture 2: Teleoperation (Manual Joystick Control)

Nodes Active:

• 🔴 Joystick Teleop (external tools: joy_node + teleop_twist_joy)

• ✅ Crazyflie Simulator (pykal.ROSNode) - publishes 3 sensor topics

• ✅ Multi-Sensor Kalman Filter (pykal.ROSNode) - fuses mocap + baro + IMU

Nodes Bypassed:

• ❌ Setpoint Generator (not needed)

• ❌ Position Controller (not needed)

Data Flow: Joystick Teleop → /cmd_vel → Plant → [mocap, baro, IMU] → Observer

Use Case: Direct manual 3D flight control for intervention or testing

---

Key Insights

1. Multi-Sensor Fusion Preserved: The 7D measurement fusion (mocap + baro + IMU) works in both modes

2. Modularity: Easy mode switching by activating/deactivating nodes

3. Red Nodes: External teleop tools shown in RED to distinguish from pykal.ROSNode wrappers

4. Observer Persistence: Kalman filter provides state estimation in both autonomous and teleop modes

5. 3D Control: Aerial vehicles benefit from joystick’s analog input for smooth 3D maneuvering

6. Common Pattern: Dual-mode architecture (autonomous + teleop) is standard for drones

Architectural Consistency:

• Theory (sensor fusion notebook): Multi-sensor fusion as DynamicalSystem

• ROS (this notebook): Multi-sensor fusion as ROSNode with 3 topic subscriptions

• Gazebo (next tutorial): Same architecture with physics simulation

The multi-sensor architecture remains consistent across the entire pipeline: Theory → Software → ROS → Gazebo → Hardware!

Visualization: Teleoperation Results

Stopping Teleoperation Nodes

# Create logger for teleoperation data
def create_teleop_logger_cf():
    """Logger for teleoperation mode (no setpoint reference)."""
    teleop_data_cf = {
        "time": [],
        "cmd_vel": [],
        "mocap": [],
        "baro": [],
        "imu": [],
        "estimate": [],
    }
    
    def logger_callback(tk, cmd_vel, mocap, baro, imu, estimate):
        teleop_data_cf["time"].append(tk)
        teleop_data_cf["cmd_vel"].append(cmd_vel.copy())
        teleop_data_cf["mocap"].append(mocap.copy())
        teleop_data_cf["baro"].append(baro.copy())
        teleop_data_cf["imu"].append(imu.copy())
        teleop_data_cf["estimate"].append(estimate.copy())
        return {}
    
    node = ROSNode(
        node_name="teleop_logger_cf",
        callback=logger_callback,
        subscribes_to=[
            ("/cmd_vel", Twist, "cmd_vel"),
            ("/mocap", Vector3, "mocap"),
            ("/baro", Float64, "baro"),
            ("/imu", Vector3, "imu"),
            ("/estimate", Odometry, "estimate"),
        ],
        publishes_to=[],
        rate_hz=50.0)
    
    return node, teleop_data_cf


# Create and start logger
from std_msgs.msg import Float64
from geometry_msgs.msg import Vector3

teleop_logger_cf, teleop_data_cf = create_teleop_logger_cf()
teleop_logger_cf.create_node()
teleop_logger_cf.start()
print("Data logger started!")

# Run teleoperation for 35 seconds
import time as pytime

T_teleop_cf = 35.0
print(f"\nRunning teleoperation for {T_teleop_cf} seconds...")
print("Crazyflie is being controlled via simulated joystick input!")
print("Multi-sensor fusion (mocap + baro + IMU) continues in background...")
pytime.sleep(T_teleop_cf)

print(f"\nTeleoperation complete! Collected {len(teleop_data_cf['time'])} samples.")

Data Collection During Teleoperation

Running the Joystick Teleop Tool

To control the Crazyflie manually with a joystick, we use the standard ROS2 joy and teleop_twist_joy packages.

In separate terminals, run:

# Terminal 1: Start joystick node
ros2 run joy joy_node

# Terminal 2: Start teleop twist joy node
ros2 run teleop_twist_joy teleop_node

Joystick Controls (typical configuration):

• Left stick Y-axis - Forward/backward velocity

• Left stick X-axis - Left/right velocity

• Right stick X-axis - Yaw rotation

• Buttons - Enable/disable, speed scaling

Note

For aerial vehicles, joystick control provides smoother 3D velocity commands compared to keyboard discrete inputs. The teleop publishes 6-DOF Twist messages: [vx, vy, vz, wx, wy, wz].

For this notebook demo, we’ll simulate joystick commands programmatically to demonstrate the architecture:

Setting Up Teleoperation Mode

We only need to run the simulator and multi-sensor observer nodes. The setpoint generator and position controller are not used in teleoperation mode.

← Dynamical Systems as ROS Nodes