Lab 10-02: System Identification for Real Robots

Objectives

By the end of this lab, you will be able to:

• Perform frequency response experiments on real robots
• Identify system parameters (inertia, friction, damping)
• Fit dynamic models to experimental data
• Validate identified parameters in simulation

Prerequisites

• Completed Lab 10-01 (Domain Randomization)
• Understanding of dynamics (Module 05)
• Access to TurtleBot 4 or similar platform

Materials

Type	Name	Tier	Notes
hardware	TurtleBot 4 Lite	required	Low-cost mobile robot platform
software	ROS2 Humble	required	Robot interface
software	Python 3.10+	required	Data analysis
software	SciPy, NumPy	required	Signal processing, optimization

Background

Why System Identification?

To transfer from simulation to reality, we need accurate models:

• Inertial parameters: Mass, moment of inertia
• Friction models: Coulomb, viscous, Stribeck effects
• Actuator dynamics: Motor constants, gearbox ratios
• Sensor characteristics: Noise, bias, calibration

Identification Approaches

1. Frequency response: Input swept-sine, measure output
2. Step response: Apply step input, fit transfer function
3. Trajectory tracking: Execute rich trajectory, regress parameters
4. Least squares: Minimize prediction error over data

Instructions

Step 1: Set Up Robot Interface

Connect to TurtleBot 4 via ROS2:

#!/usr/bin/env python3
"""
robot_interface.py
Interface for TurtleBot 4 system identification.
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
import numpy as np
import time

class TurtleBotInterface(Node):
    """ROS2 interface for TurtleBot 4."""

    def __init__(self):
        super().__init__('turtlebot_interface')

        # Publishers
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # Subscribers
        self.odom_sub = self.create_subscription(
            Odometry, '/odom', self.odom_callback, 10
        )
        self.imu_sub = self.create_subscription(
            Imu, '/imu', self.imu_callback, 10
        )

        # Data storage
        self.odom_data = []
        self.imu_data = []
        self.cmd_data = []

        # Current state
        self.current_odom = None
        self.current_imu = None

        self.get_logger().info('TurtleBot interface initialized')

    def odom_callback(self, msg):
        """Store odometry data."""
        self.current_odom = msg
        self.odom_data.append({
            'time': time.time(),
            'x': msg.pose.pose.position.x,
            'y': msg.pose.pose.position.y,
            'vx': msg.twist.twist.linear.x,
            'vy': msg.twist.twist.linear.y,
            'omega': msg.twist.twist.angular.z
        })

    def imu_callback(self, msg):
        """Store IMU data."""
        self.current_imu = msg
        self.imu_data.append({
            'time': time.time(),
            'ax': msg.linear_acceleration.x,
            'ay': msg.linear_acceleration.y,
            'omega_z': msg.angular_velocity.z
        })

    def send_velocity_command(self, linear_x, angular_z):
        """
        Send velocity command to robot.

        Args:
            linear_x: Forward velocity (m/s)
            angular_z: Angular velocity (rad/s)
        """
        cmd = Twist()
        cmd.linear.x = float(linear_x)
        cmd.angular.z = float(angular_z)

        self.cmd_vel_pub.publish(cmd)
        self.cmd_data.append({
            'time': time.time(),
            'linear_x': linear_x,
            'angular_z': angular_z
        })

    def stop(self):
        """Send stop command."""
        self.send_velocity_command(0.0, 0.0)

    def get_data_arrays(self):
        """
        Convert collected data to numpy arrays.

        Returns:
            odom_array, imu_array, cmd_array
        """
        if not self.odom_data:
            return None, None, None

        odom_times = np.array([d['time'] for d in self.odom_data])
        odom_vx = np.array([d['vx'] for d in self.odom_data])
        odom_omega = np.array([d['omega'] for d in self.odom_data])

        cmd_times = np.array([d['time'] for d in self.cmd_data])
        cmd_vx = np.array([d['linear_x'] for d in self.cmd_data])
        cmd_omega = np.array([d['angular_z'] for d in self.cmd_data])

        return {
            'odom': {'time': odom_times, 'vx': odom_vx, 'omega': odom_omega},
            'cmd': {'time': cmd_times, 'vx': cmd_vx, 'omega': cmd_omega}
        }

# Initialize interface
rclpy.init()
robot = TurtleBotInterface()

print("TurtleBot interface ready")
print("To use: robot.send_velocity_command(linear, angular)")

Expected: ROS2 connection established, odometry and IMU data streaming.

Step 2: Perform Step Response Experiment

Execute step input and record response:

#!/usr/bin/env python3
"""
step_response_experiment.py
Measure robot step response for identification.
"""

import rclpy
import numpy as np
import matplotlib.pyplot as plt
from robot_interface import TurtleBotInterface
import time

def run_step_response_experiment(robot, cmd_value=0.2, duration=5.0):
    """
    Run step response experiment.

    Args:
        robot: TurtleBotInterface instance
        cmd_value: Step command magnitude (m/s)
        duration: Experiment duration (s)

    Returns:
        data: Collected data dictionary
    """
    print(f"Running step response experiment: {cmd_value} m/s for {duration}s")

    # Clear previous data
    robot.odom_data = []
    robot.cmd_data = []

    # Wait for steady state
    robot.stop()
    time.sleep(2.0)

    # Apply step input
    start_time = time.time()
    robot.send_velocity_command(cmd_value, 0.0)

    # Collect data
```python
```python
    while time.time() - start_time < duration:

        rclpy.spin_once(robot, timeout_sec=0.01)
        time.sleep(0.01)

    # Stop robot
    robot.stop()
    time.sleep(1.0)

    # Get data
    data = robot.get_data_arrays()

    print(f"Collected {len(data['odom']['time'])} odometry samples")

    return data

def analyze_step_response(data, cmd_value):
    """
    Analyze step response to identify first-order system.

    Model: G(s) = K / (τs + 1)

    Args:
        data: Experimental data
        cmd_value: Step input magnitude

    Returns:
        params: Identified parameters {K, tau}
    """
    odom_time = data['odom']['time']
    odom_vx = data['odom']['vx']

    # Normalize time
    t = odom_time - odom_time[0]

    # Find steady-state value
    v_ss = np.mean(odom_vx[-100:])  # Average last 100 samples

    # Compute gain
    K = v_ss / cmd_value

    # Find time constant (time to reach 63.2% of steady state)
    target_velocity = 0.632 * v_ss
    idx_63 = np.where(odom_vx >= target_velocity)[0]

```python
```python
    if len(idx_63) > 0:

        tau = t[idx_63[0]]
    else:
        tau = 0.5  # Default if not reached

    params = {
        'K': K,
        'tau': tau,
        'v_ss': v_ss
    }

    print(f"\nIdentified Parameters:")
    print(f"  Gain K: {K:.3f}")
    print(f"  Time constant τ: {tau:.3f} s")
    print(f"  Steady-state velocity: {v_ss:.3f} m/s")

    return params

def plot_step_response(data, params, cmd_value):
    """Plot experimental and fitted step response."""
    odom_time = data['odom']['time']
    odom_vx = data['odom']['vx']

    t = odom_time - odom_time[0]

    # First-order model prediction
    K = params['K']
    tau = params['tau']
    v_model = K * cmd_value * (1 - np.exp(-t / tau))

    plt.figure(figsize=(10, 6))
    plt.plot(t, odom_vx, 'b-', label='Measured', linewidth=2)
    plt.plot(t, v_model, 'r--', label=f'Model (K={K:.2f}, τ={tau:.2f}s)', linewidth=2)
    plt.axhline(params['v_ss'], color='g', linestyle=':', label='Steady State')
    plt.axhline(0.632 * params['v_ss'], color='orange', linestyle=':', label='63.2%')

    plt.xlabel('Time (s)', fontsize=12)
    plt.ylabel('Linear Velocity (m/s)', fontsize=12)
    plt.title('Step Response: Experimental vs Model', fontsize=14)
    plt.legend(fontsize=10)
    plt.grid(True, alpha=0.3)
    plt.tight_layout()
    plt.savefig('step_response.png', dpi=150)
    print("\nPlot saved as step_response.png")

# Run experiment
print("Starting step response experiment...")
print("Make sure robot has clear space in front!")
input("Press Enter to start...")

data = run_step_response_experiment(robot, cmd_value=0.2, duration=5.0)
params = analyze_step_response(data, cmd_value=0.2)
plot_step_response(data, params, cmd_value=0.2)

Expected: Step response data collected, first-order model fitted with gain and time constant.

Step 3: Identify Friction Parameters

Use constant velocity tests to estimate friction:

#!/usr/bin/env python3
"""
friction_identification.py
Identify friction parameters from velocity experiments.
"""

import numpy as np
from scipy.optimize import least_squares
import matplotlib.pyplot as plt

def run_constant_velocity_tests(robot, velocities, duration=3.0):
    """
    Run multiple constant velocity tests.

    Args:
        robot: TurtleBotInterface
        velocities: List of test velocities
        duration: Time at each velocity

    Returns:
        results: List of {cmd, measured_velocity, measured_torque}
    """
    results = []

    for v_cmd in velocities:
        print(f"Testing velocity: {v_cmd:.3f} m/s")

        # Clear data
        robot.odom_data = []
        robot.imu_data = []

        # Wait for steady state
        robot.stop()
        time.sleep(2.0)

        # Apply command
        start_time = time.time()
        robot.send_velocity_command(v_cmd, 0.0)

        # Collect data
```python
```python
        while time.time() - start_time < duration:

            rclpy.spin_once(robot, timeout_sec=0.01)
            time.sleep(0.01)

        robot.stop()

        # Get data
        data = robot.get_data_arrays()

        if data['odom'] is not None:
            # Get steady-state velocity (average last half)
            n = len(data['odom']['vx'])
            v_measured = np.mean(data['odom']['vx'][n//2:])

            results.append({
                'v_cmd': v_cmd,
                'v_measured': v_measured
            })

        time.sleep(1.0)

    return results

def fit_friction_model(results):
    """
    Fit friction model: F_friction = F_c*sign(v) + b*v

    Args:
        results: Experimental data

    Returns:
        params: {F_c, b} Coulomb and viscous friction
    """
    # Extract data
    v_cmd = np.array([r['v_cmd'] for r in results])
    v_measured = np.array([r['v_measured'] for r in results])

    # Assume force proportional to command error at steady state
    # F_applied = k_motor * v_cmd
    # At steady state: F_applied = F_friction
    # F_friction = F_c*sign(v) + b*v

    # Simplified: fit linear model to positive velocities
```python
```python
    positive_mask = v_cmd > 0

    v_pos = v_measured[positive_mask]
    cmd_pos = v_cmd[positive_mask]

    # Assume motor constant k
    k_motor = 1.0  # Normalize

    # Friction force = k * (cmd - measured)
    F_friction = k_motor * (cmd_pos - v_pos)

    # Fit: F = F_c + b*v
    A = np.column_stack([np.ones_like(v_pos), v_pos])
    params_fit = np.linalg.lstsq(A, F_friction, rcond=None)[0]

    F_c = params_fit[0]
    b = params_fit[1]

    print(f"\nFriction Model:")
    print(f"  Coulomb friction F_c: {F_c:.4f}")
    print(f"  Viscous damping b: {b:.4f}")

    return {'F_c': F_c, 'b': b, 'k_motor': k_motor}

# Run friction identification
velocities = np.linspace(0.05, 0.5, 8)
print(f"Testing {len(velocities)} velocities...")

results = run_constant_velocity_tests(robot, velocities, duration=3.0)
friction_params = fit_friction_model(results)

Expected: Friction parameters identified from constant velocity tests.

Step 4: Frequency Response Analysis

Perform swept-sine input for frequency response:

#!/usr/bin/env python3
"""
frequency_response.py
Identify frequency response using chirp signal.
"""

import numpy as np
from scipy import signal
import matplotlib.pyplot as plt

def generate_chirp_signal(duration, f_min, f_max, amplitude, dt=0.01):
    """
    Generate chirp (swept-sine) signal.

    Args:
        duration: Signal duration (s)
        f_min, f_max: Frequency range (Hz)
        amplitude: Signal amplitude
        dt: Time step

    Returns:
        t, signal: Time and signal arrays
    """
    t = np.arange(0, duration, dt)
    chirp = amplitude * signal.chirp(t, f_min, duration, f_max, method='linear')
    return t, chirp

def run_chirp_experiment(robot, amplitude=0.1, duration=20.0, f_min=0.1, f_max=5.0):
    """
    Run chirp excitation experiment.

    Args:
        robot: TurtleBotInterface
        amplitude: Chirp amplitude (m/s)
        duration: Experiment duration
        f_min, f_max: Frequency range

    Returns:
        input_signal, output_signal, time
    """
    print(f"Running chirp experiment: {f_min}-{f_max} Hz, {duration}s")

    # Generate chirp
    t_chirp, chirp_signal = generate_chirp_signal(duration, f_min, f_max, amplitude)

    # Clear data
    robot.odom_data = []

    # Execute chirp
    start_time = time.time()
    for i, cmd in enumerate(chirp_signal):
```python
```python
        if i % 10 == 0:  # Every 100ms

            robot.send_velocity_command(cmd, 0.0)
            rclpy.spin_once(robot, timeout_sec=0.001)
        time.sleep(0.01)

    robot.stop()

    # Get data
    data = robot.get_data_arrays()

    return chirp_signal, data

def compute_frequency_response(input_signal, output_signal, dt=0.01):
    """
    Compute frequency response from chirp data.

    Returns:
        frequencies, magnitude, phase
    """
    # Ensure same length
    n = min(len(input_signal), len(output_signal))
    u = input_signal[:n]
    y = output_signal[:n]

    # Compute FFT
    U = np.fft.fft(u)
    Y = np.fft.fft(y)

    # Frequency response H = Y / U
    H = Y / (U + 1e-10)

    # Frequencies
    freqs = np.fft.fftfreq(n, dt)

    # Keep positive frequencies
```python
```python
    positive_freqs = freqs > 0

    freqs = freqs[positive_freqs]
    H = H[positive_freqs]

    # Magnitude and phase
    magnitude = np.abs(H)
    phase = np.angle(H)

    return freqs, magnitude, phase

def plot_bode(freqs, magnitude, phase):
    """Plot Bode diagram."""
    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))

    # Magnitude plot
    ax1.semilogx(freqs, 20 * np.log10(magnitude), 'b-', linewidth=2)
    ax1.set_ylabel('Magnitude (dB)', fontsize=12)
    ax1.set_title('Frequency Response (Bode Plot)', fontsize=14)
    ax1.grid(True, which='both', alpha=0.3)

    # Phase plot
    ax2.semilogx(freqs, np.rad2deg(phase), 'r-', linewidth=2)
    ax2.set_xlabel('Frequency (Hz)', fontsize=12)
    ax2.set_ylabel('Phase (degrees)', fontsize=12)
    ax2.grid(True, which='both', alpha=0.3)

    plt.tight_layout()
    plt.savefig('bode_plot.png', dpi=150)
    print("\nBode plot saved as bode_plot.png")

# Run frequency response
chirp_input, data = run_chirp_experiment(robot, amplitude=0.1, duration=20.0)

if data['odom'] is not None:
    output = data['odom']['vx']
    freqs, mag, phase = compute_frequency_response(chirp_input, output)
    plot_bode(freqs, mag, phase)

Expected: Frequency response computed, Bode plot showing magnitude and phase vs frequency.

Step 5: Validate Parameters in Simulation

Update simulation with identified parameters:

#!/usr/bin/env python3
"""
validate_parameters.py
Validate identified parameters in MuJoCo simulation.
"""

import mujoco
import numpy as np
import matplotlib.pyplot as plt

def create_validated_model(params):
    """
    Create MuJoCo model with identified parameters.

    Args:
        params: Dictionary of identified parameters

    Returns:
        model: MuJoCo model
    """
    # Load base model
    model = mujoco.MjModel.from_xml_path("turtlebot_base.xml")

    # Update parameters
    if 'mass' in params:
        model.body_mass[1] = params['mass']  # Assuming body 1 is robot base

    if 'damping' in params:
        model.dof_damping[:] = params['damping']

    if 'friction' in params:
        model.geom_friction[0, 0] = params['friction']

    return model

def compare_real_vs_sim(real_data, sim_data):
    """
    Compare real robot data with simulation.

    Args:
        real_data: Data from real robot
        sim_data: Data from simulation

    Returns:
        error_metrics: RMSE, max error
    """
    # Interpolate to same time base
    t_real = real_data['time']
    v_real = real_data['velocity']

    t_sim = sim_data['time']
    v_sim = sim_data['velocity']

    # Resample sim to real time
    v_sim_interp = np.interp(t_real, t_sim, v_sim)

    # Compute errors
    error = v_real - v_sim_interp
    rmse = np.sqrt(np.mean(error**2))
    max_error = np.max(np.abs(error))

    print(f"\nValidation Metrics:")
    print(f"  RMSE: {rmse:.4f} m/s")
    print(f"  Max error: {max_error:.4f} m/s")
    print(f"  Mean error: {np.mean(error):.4f} m/s")

    # Plot comparison
    plt.figure(figsize=(10, 6))
    plt.plot(t_real, v_real, 'b-', label='Real Robot', linewidth=2)
    plt.plot(t_real, v_sim_interp, 'r--', label='Simulation', linewidth=2)
    plt.fill_between(t_real, v_real - rmse, v_real + rmse, alpha=0.2, label=f'±RMSE ({rmse:.3f})')

    plt.xlabel('Time (s)', fontsize=12)
    plt.ylabel('Velocity (m/s)', fontsize=12)
    plt.title('Real vs Simulation Validation', fontsize=14)
    plt.legend(fontsize=10)
    plt.grid(True, alpha=0.3)
    plt.tight_layout()
    plt.savefig('validation.png', dpi=150)

    return {'rmse': rmse, 'max_error': max_error}

print("\nSystem identification and validation complete!")
print("Parameters identified and validated in simulation")

Expected: Simulation updated with identified parameters, validation error computed.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • robot_interface.py: ROS2 interface for TurtleBot
   • step_response_experiment.py: Step response identification
   • friction_identification.py: Friction parameter estimation
   • frequency_response.py: Bode plot analysis
   • validate_parameters.py: Sim-to-real validation

2. Understanding:

   • System identification methods
   • Parameter estimation from experimental data
   • Model validation approaches
   • Sources of sim-to-real gap

3. Experimental results:

   • Identified parameters for real robot
   • Validated model with <10% RMSE
   • Bode plots showing frequency response

Rubric

Criterion	Points	Description
Step Response	20	Correct first-order model fit
Friction ID	20	Coulomb and viscous parameters
Frequency Response	20	Bode plot from chirp data
Validation	25	Sim matches real within 10%
Analysis	15	Parameter interpretation
Total	100

Common Errors

Error: Robot doesn't respond to commands Solution: Check ROS2 topic names, verify robot is in correct mode.

Error: Noisy velocity measurements Solution: Low-pass filter odometry, increase averaging window.

Error: Parameters don't transfer to simulation Solution: Check units, verify parameter mapping is correct.

Extensions

For advanced students:

1. Multi-axis identification: Identify lateral and rotational dynamics
2. Nonlinear friction: Fit Stribeck friction model
3. Adaptive identification: Online parameter estimation
4. Grey-box modeling: Combine physics and data-driven models

Related Content

• Theory: Module 10 theory.md, Section 10.2 (System ID)
• Previous Lab: Lab 10-01 (Domain Randomization)
• Next Lab: Lab 10-03 (Sim-to-Real Transfer)