munetauvsim.control

Control algorithms and autopilot functions for AUV motion regulation.

Implements the Control block of GNC design for generating actuator commands to track guidance references while maintaining vehicle stability and rejecting disturbances. Provides autopilot implementations for heading, depth, and pitch regulation with anti-windup protection.

Functions

Heading Control

headingPID(vehicle) : PID autopilot for yaw angle regulation via rudder.

Depth Control

depthPID(vehicle) : Cascade PI-PID autopilot for depth regulation via stern plane. pitchPID(vehicle) : PID autopilot for pitch angle regulation via stern plane.

Propulsion Control

constProp(vehicle) : Constant propeller speed command (baseline propulsion).

Notes

GNC Architecture Context:

The Control block operates as the third component in the GNC (Guidance, Navigation, Control) design pattern:

  1. Guidance : Computes desired trajectories and state change commands

  2. Navigation : Estimates vehicle state from sensors and kinematics

  3. Control : Generates actuator commands to track guidance references

Control Block Scope:

  • Inputs:

    • State change commands from Guidance block (desired heading, depth, pitch)

    • State vectors from Navigation block (position, attitude, velocities)

  • Outputs:

    • Actuator commands: rudder angle (delta_r), stern plane angle (delta_s), propeller RPM (n)

Typical Control Components:

  1. Controller:

    Generate control forces/moments (desired control action) from tracking errors. Implemented as feedback control laws in this module (e.g., PID, cascade control).

  2. Control Allocation:

    Translate force/moment commands into physical actuator commands (angles, RPM, power). Combined with controller in current implementation via direct actuator command generation.

Design Philosophy:

This module provides a library of control algorithms that interface with vehicle objects through standardized attributes (gains, states, setpoints). The modular design allows:

  • Drop-in replacement of control laws

  • Vehicle-specific tuning via gain parameters

  • Cascade and multi-loop architectures

  • Extension to additional control axes (roll, speed, etc.)

Main control functions are assigned to vehicles as callable methods, enabling different control strategies without modifying vehicle dynamics or guidance systems.

Current Implementation:

The present module implements classical PID-based autopilots suitable for:

  • Waypoint-based path following (headingPID + depthPID cascade)

  • Target tracking and formation control (headingPID + pitchPID direct)

  • Constant-speed operations (constProp baseline propulsion)

Future Extensions:

The control library architecture supports addition of:

  • Advanced control laws (LQR, LQG, H-infinity, adaptive control)

  • Multi-input multi-output (MIMO) controllers

  • Speed regulation autopilots (speedPID, thrustPID)

  • Model predictive control (MPC) for constrained optimization

  • Nonlinear and robust control methods

References

[1] Fossen, T.I. (2021). Handbook of Marine Craft Hydrodynamics and Motion Control. 2nd Edition, Wiley. https://www.fossen.biz/wiley

[2] Fossen, T.I. Python Vehicle Simulator. GitHub repository. https://github.com/cybergalactic/PythonVehicleSimulator

[3] Fossen, T. I. and Perez, T. (2004). Marine Systems Simulator (MSS). https://github.com/cybergalactic/MSS

[4] Astrom, K.J. and Murray, R.M. (2008). Feedback Systems: An Introduction for Scientists and Engineers. Princeton University Press. http://www.cds.caltech.edu/~murray/amwiki

Functions

constProp(vehicle)

Constant propeller speed command (no active speed control).

depthPID(vehicle)

Cascade PI-PID controller for depth regulation via stern plane commands.

headingPID(vehicle)

PID controller for heading (yaw) angle regulation via rudder commands.

pitchPID(vehicle)

PID controller for pitch angle regulation via stern plane commands.
munetauvsim.control.headingPID(vehicle)[source]

PID controller for heading (yaw) angle regulation via rudder commands.

Computes rudder deflection angle to track desired heading setpoint while rejecting disturbances. Uses smallest signed angle error to ensure shortest rotation path. Includes integral action for zero steady-state error and anti-windup to prevent integrator saturation.

Parameters:

vehicle (Vehicle) –

Vehicle object with control parameters and state. Must have attributes:

Gains:

  • Kp_psifloat

    Proportional gain for yaw error.

  • Ki_psifloat

    Integral gain for yaw error accumulation.

  • Kd_psifloat

    Derivative gain on yaw rate (damping).

State Variables:

  • etandarray, shape (6,)

    Position/attitude [x, y, z, phi, theta, psi]. psi = eta[5].

  • nundarray, shape (6,)

    Body-frame velocities [u, v, w, p, q, r]. r = nu[5] (yaw rate).

  • psi_dfloat

    Desired heading angle in radians (setpoint).

  • psi_intfloat

    Integral term state (accumulated error). Updated by this function.

Parameters:

  • sampleTimefloat

    Euler integration time step in seconds.

  • deltaMax_rfloat

    Maximum rudder deflection in radians.

Returns:

delta_r (float) – Rudder angle command in radians, saturated to [-deltaMax_r, +deltaMax_r]. Positive rudder deflection produces positive yaw moment (turn right).

Return type:

float

Notes

Side Effects

Updates vehicle.psi_int with new integral state via anti-windup logic.

Control Law

  • PID Equation:

    delta_r = -K_p * ssa(psi - psi_d) - K_i * I - K_d * r

    where:

    • psi: Current yaw angle (eta[5])

    • psi_d: Desired yaw angle (psi_d)

    • r: Yaw rate (nu[5])

    • I: Integral term (psi_int)

    • ssa(): Smallest signed angle function

  • Integral Update:

    Without saturation:

    I_(k+1) = I_k + err_psi * h

    With saturation (anti-windup):

    I_(k+1) = I_k + (delta_r_sat - delta_r) / K_i

    where:

    • h: sampleTime.

    • err_psi: difference in yaw actual from yaw setpoint

    • delat_r_sat: delta_r clamped to saturation limit

  • Error Calculation:

    Uses smallest signed angle to wrap heading error to [-pi, pi]:

    err_psi = ssa(psi - psi_d)

    Ensures controller takes shortest rotation path:

    • Current: 350 deg (6.11 rad), Desired: 10 deg (0.17 rad)

    • Naive error: 350 deg - 10 deg = 340 deg (turn left 340 deg)

    • SSA error: ssa(340 deg) = -20 deg (turn right 20 deg) <- Preferred

Sign Convention:

Gains typically negative because:

  • Positive heading error -> need negative rudder to correct

  • Standard marine convention: positive rudder -> positive yaw moment -> turn right

  • Negative gains provide negative feedback for stability

Anti-Windup Mechanism:

When rudder saturates (delta_r =/= delta_r):

  1. Compute difference: d = delta_r_sat - delta_r

  2. Back-calculate integral: psi_int += d / Ki_psi

  3. Prevents accumulating integral effects during saturation

Without anti-windup:

  • Integral continues growing while saturated

  • Causes large overshoot when error reverses

  • Recovery time increases significantly

Derivative Term:

Uses measured yaw rate r (not error derivative) because:

  • Cleaner signal (gyro measurement vs. numerical derivative)

  • Avoids noise amplification from differentiating error

  • Provides damping proportional to rotation speed

Zero Ki Handling:

If Ki_psi = 0 (pure PD control):

  • Anti-windup condition never triggers

  • Integral updated normally (but with zero weight)

  • No steady-state error elimination (acceptable for some applications)

Tuning Impact:

Increasing absoluate value of Kp_psi:

  • Faster response to heading errors

  • Risk: Oscillation if too high

Increasing absoluate value of Ki_psi:

  • Better disturbance rejection

  • Eliminates steady-state heading bias

  • Risk: Overshoot, slower settling

Increasing absoluate value of Kd_psi:

  • More damping, reduces overshoot

  • Faster settling time

  • Risk: Noise sensitivity

See also

gnc.ssa

Smallest signed angle wrapping

gnc.saturation

Control output limiting

navigation.headingFilterLOS

Heading observer with LOS guidance

guidance.ALOSlaw

Provides desired heading psi_d

References

[1] Fossen, T. I., “An Adaptive Line-of-Sight (ALOS) Guidance Law for Path Following of Aircraft and Marine Craft,” in IEEE Transactions on Control Systems Technology, 31(6), 2887-2894, Nov. 2023, doi: 10.1109/TCST.2023.3259819.

Examples

### Basic usage:

>>> import munetauvsim.vehicles as veh
>>> auv = veh.Remus100s()
>>> auv.psi_d = np.pi / 4  # Desired heading: 45 deg
>>> auv.eta[5] = 0.0       # Current heading: 0 deg
>>> auv.nu[5] = 0.0        # Yaw rate: 0 rad/s
>>> auv.psi_int = 0.0      # Reset integral
>>>
>>> import munetauvsim.control as ctrl
>>> delta_r = ctrl.headingPID(auv)
>>> print(f"Rudder command: {np.degrees(delta_r):.2f} deg")
Rudder command: -7.85 deg  # Proportional response to 45 deg error

### Simulation loop:

>>> import munetauvsim.guidance as guid
>>> for i in range(1000):
...     # Update desired heading from guidance
...     auv.psi_d = guid.ALOSlaw(auv, pt1, pt2)
...
...     # Compute control
...     delta_r = ctrl.headingPID(auv)
...
...     # Apply to dynamics
...     u_control = np.array([delta_r, 0, 1200])
...     auv.nu, _ = auv.dynamics(u_control)
...
...     # Update position
...     auv.eta, _ = auv.Attitude(auv)

### Verify anti-windup:

>>> auv = veh.Remus100s()
>>> auv.psi_d = np.pi  # 180 deg turn
>>> auv.eta[5] = 0.0
>>> auv.deltaMax_r = np.radians(30)  # +/- 30 deg limit
>>>
>>> for i in range(100):
...     delta_r = ctrl.headingPID(auv)
...     if abs(delta_r) >= auv.deltaMax_r:
...         print(f"Saturated at iteration {i}")
...         # Verify psi_int doesn't grow unbounded
...         print(f"Integral state: {auv.psi_int:.3f}")
munetauvsim.control.depthPID(vehicle)[source]

Cascade PI-PID controller for depth regulation via stern plane commands.

Two-loop control architecture with outer depth PI controller generating desired pitch angle, and inner pitch PID controller computing stern plane deflection. Cascade structure provides improved disturbance rejection and prevents depth overshoot from aggressive pitch commands.

Parameters:

vehicle (Vehicle) –

Vehicle object with control parameters and state. Must have attributes:

Outer Loop Gains (Depth PI):

  • Kp_zfloat

    Proportional gain for depth error.

  • Ki_zfloat

    Integral gain for depth error.

Inner Loop Gains (Pitch PID):

  • Kp_theta, Ki_theta, Kd_thetafloat

    Gains for pitch control (see pitchPID documentation).

State Variables:

  • etandarray, shape (6,)

    Position/attitude [x, y, z, …]. z = eta[2] (depth).

  • z_dfloat

    Desired depth in meters (setpoint).

  • z_intfloat

    Depth integral term state. Updated by this function.

  • theta_dfloat

    Desired pitch angle (intermediate setpoint). Set by this function.

  • theta_intfloat

    Pitch integral term state. Updated by pitchPID.

Parameters:

  • sampleTimefloat

    Integration time step.

  • deltaMax_sfloat

    Maximum stern plane deflection in radians.

Returns:

delta_s (float) – Stern plane angle command in radians, saturated to [-deltaMax_s, +deltaMax_s]. Positive stern plane produces negative pitch moment (nose down).

Return type:

float

Notes

Side Effects

  • Updates vehicle.z_int with new depth integral state

  • Sets vehicle.theta_d to desired pitch angle for inner loop

  • Updates vehicle.theta_int via pitchPID call

Control Architecture

  • Cascade Structure:

    z_d, z -> [Depth PI] -> theta_d theta_d, theta, q -> [Pitch PID] -> delta_s

  • Outer Loop (Depth PI):

    theta_d = Kp_z * (z - z_d) + Ki_z * integral[(z - z_d) dt]

    Generates desired pitch angle from depth error. Saturated to +/- deltaMax_s to prevent excessive pitch commands.

  • Inner Loop (Pitch PID):

    delta_s = Kp_theta * (theta - theta_d) +

    Ki_theta * integral[(theta - theta_d) dt] + Kd_theta * q

    Generates stern plane command from pitch error (see pitchPID for details).

  • Anti-Windup (Outer Loop):

    When theta_d saturates:

    I_z = I_z + (theta_d_sat - theta_d) / Ki_z

    Prevents depth integral accumulation during pitch saturation.

Why Cascade Control:

Advantages over single-loop depth control:

  1. Improved Dynamics: Inner loop responds faster than single depth loop

  2. Disturbance Rejection: Pitch disturbances handled by inner loop

  3. Overshoot Prevention: Pitch saturation limits depth rate of change

  4. Decoupling: Separates slow depth response from fast pitch response

Design Rationale:

Cascade allows:

  • Slow outer loop for smooth depth tracking

  • Fast inner loop for tight pitch regulation

  • Better overall performance than single loop

Gain Relationships:

Typical cascade tuning puts the inner loop bandwidth approximately 5-10x faster than outer loop to ensure loops don’t fight each other.

Saturation Interaction:

Two saturation points:

  1. theta_d saturated to +/- deltaMax_s

  2. delta_s saturated to +/- deltaMax_s (in pitchPID)

Double anti-windup:

  • Outer loop: Prevents z_int growth when theta_d saturates

  • Inner loop: Prevents theta_int growth when delta_s saturates

Positive Depth Convention:

END frame: Positive z is down (depth increases with z). Error calculation: err_z = z - z_d

  • If deeper than desired (z > z_d): Positive error -> pitch up

  • If shallower than desired (z < z_d): Negative error -> pitch down

Integral Action:

Outer loop integral eliminates steady-state depth error from:

  • Buoyancy errors

  • Trim angle offsets

  • Constant vertical currents

Inner loop integral (in pitchPID) eliminates pitch bias.

Tuning Guidelines:

  1. Tune inner loop (pitch) first with theta_d fixed

  2. Then tune outer loop (depth) with inner loop active

  3. Outer loop gains much smaller than inner loop

  4. Ki_z very small (0.001-0.01) to avoid depth overshoot

See also

pitchPID

Inner loop pitch controller

gnc.saturation

Control output limiting

navigation.depthFilter

Depth setpoint filter

guidance.pathFollow

Provides desired depth z_d

References

[1] Fossen, T.I. (2021). Handbook of Marine Craft Hydrodynamics and Motion Control. 2nd Edition, Wiley. https://www.fossen.biz/wiley

Examples

### Basic depth change:

>>> import munetauvsim.vehicles as veh
>>> auv = veh.Remus100s()
>>> auv.z_d = 25.0     # Desired depth: 25m
>>> auv.eta[2] = 15.0  # Current depth: 15m
>>> auv.z_int = 0.0    # Reset integral
>>>
>>> import munetauvsim.control as ctrl
>>> delta_s = ctrl.depthPID(auv)
>>> print(f"Stern plane: {np.degrees(delta_s):.2f} deg")
>>> print(f"Intermediate pitch cmd: {np.degrees(auv.theta_d):.2f} deg")
Stern plane: -2.85 deg  # Nose down to increase depth
Intermediate pitch cmd: -1.00 deg

### Simulation loop:

>>> for i in range(2000):
...     # Update desired depth from guidance
...     auv.z_d = waypoint[2]
...
...     # Compute control
...     delta_s = ctrl.depthPID(auv)
...
...     # Apply to dynamics
...     u_control = np.array([0, delta_s, 1200])
...     auv.nu, _ = auv.dynamics(u_control)
...
...     # Update position
...     auv.eta, _ = auv.Attitude(auv)
munetauvsim.control.pitchPID(vehicle)[source]

PID controller for pitch angle regulation via stern plane commands.

Computes stern plane deflection to track desired pitch angle setpoint. Used as inner loop in cascade depth control or standalone for direct pitch control in target tracking scenarios. Includes integral action and anti-windup.

Parameters:

vehicle (Vehicle) –

Vehicle object with control parameters and state. Must have attributes:

Gains:

  • Kp_thetafloat

    Proportional gain for pitch error.

  • Ki_thetafloat

    Integral gain for pitch error.

  • Kd_thetafloat

    Derivative gain on pitch rate.

State Variables:

  • etandarray, shape (6,)

    Position/attitude […, phi, theta, psi]. theta = eta[4].

  • nundarray, shape (6,)

    Body velocities […, p, q, r]. q = nu[4] (pitch rate).

  • theta_dfloat

    Desired pitch angle in radians (setpoint).

  • theta_intfloat

    Integral term state. Updated by this function.

Parameters:

  • sampleTimefloat

    Integration time step.

  • deltaMax_sfloat

    Maximum stern plane deflection in radians.

Returns:

delta_s (float) – Stern plane angle command in radians, saturated to [-deltaMax_s, +deltaMax_s]. Positive deflection produces negative pitch moment (nose down).

Return type:

float

Notes

Side Effects

Updates vehicle.theta_int with new integral state via anti-windup logic.

Control Law

  • PID Equation:

    delta_s = Kp_theta * ssa(theta - theta_d) + Ki_theta * I + Kd_theta * q

    where:

    • theta: Current pitch angle (eta[4])

    • theta_d: Desired pitch angle (theta_d)

    • q: Pitch rate (nu[4])

    • I: Integral term (theta_int)

    • ssa(): Smallest signed angle

  • Integral Update:

    Without saturation:

    I_(k+1) = I_k + err_theta * h

    With saturation (anti-windup):

    I_(k+1) = I_k + (delta_s_sat delta_s) / Ki_theta

  • Error Wrapping:

    Uses smallest signed angle for pitch error to handle wraparound:

    err_theta = ssa(theta - theta_d)

    Though pitch typically +/- 30 deg, ssa ensures robustness for extreme maneuvers.

Usage Contexts:

  1. Cascade Depth Control (Common):

    Called by depthPID as inner loop. theta_d comes from outer depth controller.

  2. Direct Pitch Control (Target Tracking):

    Called directly for APF velocity guidance. theta_d computed from desired vertical velocity.

Sign Convention:

Stern plane and pitch angle relationship:

  • Positive delta_s (stern plane down): Nose down moment -> pitch decreases

  • Negative delta_s (stern plane up): Nose up moment -> pitch increases

Gains negative to provide correct sign:

  • Pitch too high (theta > theta_d): Positive error -> need negative delta_s (nose up)

  • Negative Kp produces negative delta_s from positive error

Anti-Windup Importance:

Stern plane saturates frequently during:

  • Aggressive depth changes

  • Steep dive/climb maneuvers

  • Disturbance rejection

Without anti-windup:

  • Integral grows during saturation

  • Large overshoot when error reverses

  • Oscillatory depth response in cascade

Derivative Term:

Uses measured pitch rate q (not error derivative):

  • Gyro provides clean q measurement

  • Avoids numerical differentiation noise

  • Provides proportional damping

Tuning Impact:

Increasing absoluate value of Kp_theta:

  • Faster pitch response

  • Risk: Oscillation, actuator wear

Increasing absoluate value of Ki_theta:

  • Eliminates pitch bias from trim/buoyancy

  • Risk: Overshoot in cascade depth control

Increasing absoluate value of Kd_theta:

  • More damping, smoother response

  • Reduces control effort

  • Risk: Noise amplification

See also

depthPID

Outer loop calling this controller

gnc.ssa

Angle wrapping function

gnc.saturation

Control output limiting

guidance.targetTrack

Direct pitch control usage

References

[1] Fossen, T.I. (2021). Handbook of Marine Craft Hydrodynamics and Motion Control. 2nd Edition, Wiley. https://www.fossen.biz/wiley

Examples

### Direct pitch control:

>>> import munetauvsim.vehicles as veh
>>> auv = veh.Remus100s()
>>> auv.theta_d = np.radians(-5)  # 5 deg nose down
>>> auv.eta[4] = 0.0              # Level
>>> auv.nu[4] = 0.0               # No pitch rate
>>> auv.theta_int = 0.0           # Reset integral
>>>
>>> import munetauvsim.control as ctrl
>>> delta_s = ctrl.pitchPID(auv)
>>> print(f"Stern plane: {np.degrees(delta_s):.2f} deg")
Stern plane: 2.5 deg  # Positive deflection for nose down

### Cascade usage (called by depthPID):

>>> # Outer loop sets theta_d
>>> auv.theta_d = np.radians(-3)
>>> # Inner loop tracks pitch
>>> delta_s = ctrl.pitchPID(auv)
>>> # Used in depthPID return value
munetauvsim.control.constProp(vehicle)[source]

Constant propeller speed command (no active speed control).

Simple propeller allocation that returns fixed RPM setpoint. Provides baseline propulsion for path following and tracking scenarios where speed regulation is not critical. Vehicle maintains approximate constant speed through propeller thrust characteristics.

Parameters:

vehicle (Vehicle) –

Vehicle object with propeller setpoint. Must have attribute:

  • n_setptfloat

    Propeller speed setpoint in RPM (revolutions per minute).

Returns:

n (float) – Propeller RPM command (same as vehicle.n_setpt). No saturation or transformation applied.

Return type:

float

Notes

Design Philosophy:

AUV speed control often unnecessary because:

  • Path following cares about trajectory, not speed

  • Propeller thrust roughly balances drag at equilibrium

  • Speed variations minor for typical maneuvers

  • Simplifies control architecture

When Speed Control Needed:

Active speed control beneficial for:

  • Time-critical missions (rendezvous, docking)

  • Formation keeping (multi-vehicle coordination)

  • Energy optimization (variable speed profiles)

  • Strong current compensation

Future extension: Implement speedPID() for thrust regulation.

Load Assignment:

n_setpt typically assigned in vehicle initialization:

>>> auv = Remus100s()
>>> auv.loadConstantProp(n_setpt=1200)  # Sets vehicle.n_setpt

Or modified dynamically during mission:

>>> if mission_phase == 'transit':
...     auv.n_setpt = 1400  # Fast
>>> elif mission_phase == 'survey':
...     auv.n_setpt = 1000  # Slow, stable

Propeller Dynamics:

Even with constant command, actual propeller speed varies due to:

  • Actuator dynamics (1st order lag with time constant ~1-2s)

  • Load variations (drag changes with attitude, speed)

  • Thrust allocation (propeller shares power with control surfaces)

These effects modeled in vehicle.dynamics().

Integration in Control Loop:

Typical usage as PropCmd method:

>>> auv.PropCmd = constProp  # Assign function pointer
>>> # Later in guidance/control:
>>> n = auv.PropCmd(auv)  # Call assigned function

Or direct call:

>>> n = constProp(auv)

See also

vehicles.Vehicle.loadConstantProp

Assigns n_setpt and PropCmd

vehicles.Vehicle.dynamics

Models propeller dynamics

guidance.pathFollow

Uses constProp for propulsion

Examples

### Basic usage:

>>> import munetauvsim.vehicles as veh
>>> import munetauvsim.control as ctrl
>>> auv = veh.Remus100s()
>>> auv.n_setpt = 1200  # Set cruise RPM
>>> n_cmd = ctrl.constProp(auv)
>>> print(f"Propeller command: {n_cmd} RPM")
Propeller command: 1200 RPM

### Load during path following:

>>> auv.loadPathFollowing()
>>> auv.loadConstantProp(n_setpt=1300)
>>> # Now PropCmd assigned to constProp
>>>
>>> for i in range(N):
...     # Guidance and control
...     delta_r = control.headingPID(auv)
...     delta_s = control.depthPID(auv)
...     n = auv.PropCmd(auv)  # Calls constProp
...
...     # Package control vector
...     u_control = np.array([delta_r, delta_s, n])

### Speed profile mission:

>>> auv.loadConstantProp(n_setpt=1000)  # Initial slow speed
>>>
>>> for i in range(N):
...     # Change speed based on mission phase
...     if auv.eta[0] > 500:  # Past 500m East
...         auv.n_setpt = 1500  # Speed up
...
...     n = constProp(auv)
...     # ... rest of control loop ...