munetauvsim.guidance

Guidance algorithms and waypoint management for AUV path planning.

Implements the Guidance block of GNC design for computing desired trajectories (position, velocity, acceleration) and state change commands from current state.

Classes

Position

Data structure for 3D coordinates.

Waypoint

Waypoint database with path calculation utilities.

Functions

Path Following:

• pathFollow(vehicle) : Waypoint-based path following guidance system.

• ALOSlaw(vehicle, pt1, pt2) : Adaptive Line-of-Sight guidance law.

Target Tracking:

• targetTrack(vehicle) : Target-tracking guidance system.

• velocitySubsumptionCascadeAPF(vehicle) : APF velocity guidance.

• velCB(vehicle) : Constant bearing velocity guidance law.

APF Mission Functions:

• missionTargetFeedForwardAPF : Target velocity feed-forward vector.

APF Formation Keeping Functions:

• formationTargetNormPolyAPF : Attraction and repulsion to target vehicle.

APF Survival Functions:

• survivalGroupNormPolyAPF : Repulsion from group vehicles.

Depth Governing Functions

• depthSafetyLimit(vehicle, vel) : Depth constraint repulsion.

Waypoint Utilities:

• updateWpt(vehicle) : Update active waypoint based on position.

• getNextWpt(vehicle) : Get next waypoint coordinates.

• addEtaWpt(vehicle) : Add current position to waypoint database.

• generateRandomPath(num, start, psiLims, rLims, zLims) : Generate random path.

Prediction and State Estimation:

• predictNextEtaVel(vehicle) : Predict vehicle’s next position and velocity.

• predictSwarmState(vehicle) : Update swarm member position predictions.

Notes

• Guidance block inputs: State vectors from Navigation, operator commands, sensors

• Guidance block outputs: State change commands to Control, operator feedback

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

Functions

ALOSlaw(vehicle, pt1, pt2)	Compute desired heading using Adaptive Line-of-Sight (ALOS) guidance law.
addEtaWpt(vehicle)	Add current vehicle position to waypoint database.
dataclass([cls, init, repr, eq, order, ...])	Returns the same class as was passed in, with dunder methods added based on the fields defined in the class.
depthSafetyLimit(vehicle, vel)	Apply depth safety constraint.
fields(class_or_instance)	Return a tuple describing the fields of this dataclass.
formationTargetNormPolyAPF(vehicle)	Compute formation keeping APF vector using a normalized polynomial function.
generateRandomPath([num, start, psiLims, ...])	Generate random waypoint path with specified constraints.
getNextWpt(vehicle)	Get next waypoint from database, or continue on last bearing if complete.
missionTargetFeedForwardAPF(vehicle)	Compute feed-forward control velocity vector from leader velocity.
pathFollow(vehicle)	Waypoint-based path following guidance system.
predictNextEtaVel(vehicle)	Predict vehicle's own next position and velocity for communication messages.
predictSwarmState(vehicle)	Update predicted postion/velocity for swarm group members using data models.
survivalGroupNormPolyAPF(vehicle)	Compute collision avoidance APF vector with a normalized polynomial function
targetTrack(vehicle)	Target tracking guidance system.
updateWpt(vehicle)	Update active waypoint index when vehicle reaches acceptance radius.
velCB(vehicle)	Constant bearing velocity guidance law for target tracking.
velocitySubsumptionCascadeAPF(vehicle)	APF velocity guidance law with subsumption-based saturation cascade.

Classes

NDArray	alias of ndarray[Any, dtype[ScalarType]]
NPFltArr	alias of ndarray[Any, dtype[float64]]
Position(x, y, z)	Coordinate data structure holding 3D positions in END coordinates.
Waypoint([xPos, yPos, zPos])	Waypoint database for path planning and trajectory analysis.

class munetauvsim.guidance.Position(x, y, z)

Bases: object

Coordinate data structure holding 3D positions in END coordinates.

Attributes

xlist of float

East coordinates in meters.

ylist of float

North coordinates in meters.

zlist of float

Down coordinates in meters (positive downward).

Notes

• Uses __slots__ for memory efficiency.

• Supports indexing and slicing.

• Automatically converts scalar and array-like inputs to lists.

Examples

### Basic creation and access:

>>> import munetauvsim.guidance as guid
>>> pos = guid.Position([0, 100, 200], [50, 150, 250], [10, 15, 20])
>>> print(f"Number of points: {len(pos.x)}")
Number of points: 3

### Index to get coordinates at specific point:

>>> coords = pos[0]  # Get first point
>>> print(coords)
[0, 50, 10]
>>> x, y, z = pos[1]  # Unpack second point
>>> print(f"Point 1: East={x}m, North={y}m, Depth={z}m")
Point 1: East=100m, North=150m, Depth=15m

### Slice to get multiple points:

>>> first_two = pos[0:2]  # Get first two points
>>> print(first_two)
[[0, 100], [50, 150], [10, 15]]

### Access individual coordinate arrays:

>>> east_coords = pos.x
>>> print(f"East coordinates: {east_coords}")
East coordinates: [0, 100, 200]

### Convert scalar inputs to lists automatically:

>>> single_point = guid.Position(100, 200, 30)
>>> print(f"Single point: {single_point[0]}")
Single point: [100, 200, 30]

### Convert numpy arrays to lists:

>>> import numpy as np
>>> x_arr = np.array([0, 100, 200])
>>> y_arr = np.array([0, 50, 100])
>>> z_arr = np.array([5, 10, 15])
>>> pos_from_arrays = guid.Position(x_arr, y_arr, z_arr)
>>> print(f"From arrays: {pos_from_arrays[1]}")
From arrays: [100, 50, 10]

Parameters:

• x (List[float]) –

• y (List[float]) –

• z (List[float]) –

x: List[float]

y: List[float]

z: List[float]

__post_init__()

Enforce list type for all coordinate fields.

__getitem__(key)

Return [x, y, z] at index or slice.

Parameters:

key (int | slice) –

Return type:

List[float]

class munetauvsim.guidance.Waypoint(xPos=0, yPos=0, zPos=0)

Bases: object

Waypoint database for path planning and trajectory analysis.

Stores a sequence of (x, y, z) positions in END frame. Supports indexing, slicing, and path analysis (distances, headings, time estimates).

Parameters:

• xPos (float or list of float, default=0) – East coordinates in meters.

• yPos (float or list of float, default=0) – North coordinates in meters.

• zPos (float or list of float, default=0) – Down coordinates (depth) in meters.

Attributes

posPosition

Position object containing x, y, z coordinate lists.

Methods

insert(index, point)

Insert a waypoint before index.

calcWptDistance(index)

Compute distances between consecutive waypoints.

calcTotalDistance()

Sum total linear distance through all waypoints.

calcWptHeading(index)

Compute heading angles between consecutive waypoints.

estimateTime(speed, coeff)

Estimate travel time through all waypoints.

estimateAreaRoot(padding)

Determine bounding square containing all waypoints.

Notes

• Possible extensions could add additional waypoint attributes such as speed, heading, orientation (eta / pose), guidance directives, etc.

• Structure is based on the design used in Fossen’s MSS.

References

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

Examples

### Basic waypoint creation:

>>> import munetauvsim.guidance as guid
>>> wpt = guid.Waypoint([0, 500, 1000], [500, 500, 500], [20, 20, 20])
>>> print(f"Number of waypoints: {len(wpt)}")
Number of waypoints: 3
>>> print(wpt)
(     0.0,    500.0,     20.0)
(   500.0,    500.0,     20.0)
(  1000.0,    500.0,     20.0)

### Access waypoint coordinates:

>>> coords = wpt.pos[0]  # Get first waypoint as [x, y, z]
>>> x, y, z = wpt.pos[0]  # Unpack coordinates
>>> print(f"Waypoint 0: East={x}m, North={y}m, Depth={z}m")
Waypoint 0: East=0m, North=500m, Depth=20m

### Access coordinate arrays directly:

>>> east_coords = wpt.pos.x
>>> print(f"All East coordinates: {east_coords}")
All East coordinates: [0, 500, 1000]

### Slice waypoints (returns new Waypoint object):

>>> first_two = wpt[0:2]
>>> print(f"First two waypoints: {len(first_two)} points")
First two waypoints: 2 points
>>> print(first_two.pos.x)
[0, 500]

### Index single waypoint (returns new Waypoint object):

>>> single_wpt = wpt[1]
>>> print(f"Single waypoint: {single_wpt.pos[0]}")
Single waypoint: [500, 500, 20]

### Add waypoints dynamically:

>>> wpt.insert(1, [250, 400, 25])  # Insert before index 1
>>> print(f"After insert: {len(wpt)} waypoints")
After insert: 4 waypoints
>>> print(wpt.pos[1])
[250, 400, 25]

### Concatenate waypoint databases:

>>> wpt1 = guid.Waypoint([0, 100], [0, 100], [10, 15])
>>> wpt2 = guid.Waypoint([200, 300], [200, 300], [20, 25])
>>> combined = wpt1 + wpt2
>>> print(f"Combined length: {len(combined)}")
Combined length: 4

### Path analysis:

>>> distances = wpt.calcWptDistance()
>>> print(f"Leg distances: {distances}")
Leg distances: [269.30466019 269.30466019 500.        ]
>>> total_dist = wpt.calcTotalDistance()
>>> print(f"Total path length: {total_dist:.1f}m")
Total path length: 1038.6m

### Heading calculations:

>>> headings = wpt.calcWptHeading()
>>> print(f"Leg headings (rad): {headings}")
Leg headings (rad): [-0.38050638  0.38050638  0.        ]

### Time estimates:

>>> travel_time = wpt.estimateTime(speed=2.0)
>>> print(f"Estimated travel time: {travel_time:.1f} seconds")
Estimated travel time: 623.2 seconds

__init__(xPos=0, yPos=0, zPos=0)

Initialize waypoint database with coordinate lists.

Parameters:

• xPos (float | List[float]) –

• yPos (float | List[float]) –

• zPos (float | List[float]) –

pos

__getitem__(key)

Index or slice waypoints, returns new Waypoint object.

Parameters:

key (int | slice) –

Return type:

Self

__len__()

Return number of waypoints in database.

Return type:

int

__repr__()

Detailed description of Waypoint

Return type:

str

__str__()

Return user-friendly string representation of Waypoint

Return type:

str

__add__(other)

Concatenate two waypoint databases.

Parameters:

other (Self) –

Return type:

Self

insert(index, point)

Insert point into the database before the position at index.

Parameters:

• index (int) – Insert point before this index. Use len(wpt) to append to end.

• point (list of float, [x, y, z]) – Position coordinates to insert.

Return type:

None

calcWptDistance(index=None)

Return array of distances between consecutive waypoints.

Parameters:

index (int, optional) – If specified, return only distance at that index.

Returns:

distances (ndarray, shape (len-1,)) – Euclidean distances between waypoints in meters.

Return type:

ndarray[Any, dtype[float64]]

Notes

• Output will have length(self)-1.

• Using index = -1 will still return the final distance measurement.

calcTotalDistance()

Calculate total path length through all waypoints.

Returns:

distance (float) – Sum of distances between consecutive waypoints in meters.

Return type:

float64

calcWptHeading(index=None)

Return heading angles (rad) between consecutive waypoints.

Parameters:

index (int, optional) – If specified, return only heading from index to index+1.

Returns:

headings (ndarray, shape (len-1,)) – Azimuth angles in radians. East = 0 rad, North = pi/2 rad.

Return type:

ndarray[Any, dtype[float64]]

Notes

• Output will have length(self)-1.

• Using index = -1 will still return the final heading angle.

estimateTime(speed=2.0, coeff=1.2)

Estimate travel time to traverse full path through all waypoints.

Parameters:

• speed (float, default=2.0) – Vehicle speed in m/s.

• coeff (float, default=1.2) – Scaling coefficient. Adds buffer to account for variations in how particular guidance functions generate paths or from environmental influences.

Returns:

time (float) – Estimated travel time in seconds.

Return type:

float64

estimateAreaRoot(padding=300)

Compute bounding square dimensions for path area.

Parameters:

padding (int, default=300) – Buffer zone in meters added to all sides around path area.

Returns:

• length (int) – Side length of padded square containing path.

• offset (list of int) – Offset coordinates of origin (0,0) relative to lower-left corner.

Return type:

Tuple[int, int]

Notes

Computes the largest distance in the x or y direction and adds a buffer zone of padding. The padding is added on all sides, extending the calculated length by 2*padding. Root length is rounded up to the nearest integer.

munetauvsim.guidance.generateRandomPath(num=1, start=[0, 0, 0], psiLims=[-1.5707963267948966, 1.5707963267948966], rLims=[100, 200], zLims=[0, 100])

Generate random waypoint path with specified constraints.

Parameters:

• num (int) – Number of waypoints to generate.

• start ([x, y, z]) – Starting position in END.

• psiLims ([min, max]) – Heading change bounds in radians.

• rLims ([min, max]) – Distance change bounds in meters.

• zLims ([min, max]) – Depth change bounds in meters.

Returns:

wpt (Waypoint) – Generated waypoint database.

Return type:

Waypoint

Notes

• Intended for quick testing and demo purposes when no specific path or mission scenario is required.

• If the need arises for repeating these paths, incorporate a seed value that can be used by the RNG.

munetauvsim.guidance.addEtaWpt(vehicle)

Add current vehicle position to waypoint database.

Parameters:

vehicle (Vehicle) – Vehicle with eta, wpt, wpt_k, R_switch attributes. eta: [x, y, z, phi, theta, psi], vehicle position / attitude vector wpt: Waypoint database wpt_k: Index of previous active waypoint R_switch: Acceptance radius (m) for reaching a waypoint

Return type:

None

Notes

• Inserts position before current active waypoint.

• Only adds position if distance > R_switch between waypoints.

munetauvsim.guidance.getNextWpt(vehicle)

Get next waypoint from database, or continue on last bearing if complete.

Parameters:

vehicle (Vehicle) – Vehicle with wpt, wpt_k attributes. wpt: Waypoint database wpt_k: Index of previous active waypoint

Returns:

pt ([x, y, z]) – Next waypoint coordinates. If at end of database, continues on last bearing to distant extrapolated point.

Return type:

List[float]

Notes

Handling the vehicle behavior when reaching the end of the waypoint database should not be the responsibility of this function, but it’s here for now or until there is a need to develop an alternative. Present development is to simply continue on last bearing. Other behaviors to consider may be to return to the first waypoint, maintain the position (station keeping, or a holding pattern), return to the surface and power down, or end the simulation.

munetauvsim.guidance.updateWpt(vehicle)

Update active waypoint index when vehicle reaches acceptance radius.

Parameters:

vehicle (Vehicle) – Vehicle with wpt, wpt_k, R_switch, and eta attributes. wpt: Waypoint database wpt_k: Index of previous active waypoint R_switch: Acceptance radius (m) for reaching a waypoint eta : [x, y, z, phi, theta, psi], vehicle position/attitude vector

Returns:

• prevPt ([x, y, z]) – Previous active waypoint coordinates.

• nextPt ([x, y, z]) – Next active waypoint coordinates.

Return type:

List[List[float]]

Notes

• Currently only uses 2D (x,y) values to determine successful arrival at waypoint.

• Updates vehicle.wpt_k index when vehicle enters acceptance radius.

• Logs waypoint arrival events.

munetauvsim.guidance.predictNextEtaVel(vehicle)

Predict vehicle’s own next position and velocity for communication messages.

Generates forward-looking state estimates used in swarm coordination messages to help followers anticipate leader motion and compensate for communication delays.

Parameters:

vehicle (Vehicle) –

Vehicle object. Must have the following attributes defined:

• etandarray, shape (6,)

  Current position/attitude [x, y, z, phi, theta, psi] in END frame.

• wptWaypoint

  Waypoint database for path planning.

• n_ratefloat or None

  Propeller acceleration rate in rpm/s. If None, assumes constant speed.

• n_setptfloat

  Propeller setpoint RPM for constant speed scenarios.

• u_actualndarray, shape (3,)

  Actual control state [rudder, stern, propeller_rpm].

• xferN2Ucallable

  Method to convert propeller RPM to vehicle speed (m/s).

Returns:

• nextPoint (ndarray, shape (3,)) – Predicted next position [x, y, z] in meters. Determined from next waypoint in database via getNextWpt().

• nextVel (ndarray, shape (3,)) – Predicted next velocity [vx, vy, vz] in m/s. Computed from constant acceleration model.

Return type:

List[NPFltArr]

Notes

Position Prediction:

Next position is taken directly from the waypoint database via getNextWpt(). If at the end of waypoints, extrapolates along last bearing to distant point.

Velocity Prediction:

Uses kinematic constant acceleration model:

v_f^2 = v_i^2 + 2 a x

where:

• v_i: Current speed from xferN2U(u_actual[2])

• a: Acceleration from xferN2U(n_rate)

• x: Distance to next waypoint

• v_f: Predicted speed at next waypoint

Direction is along the vector from current position to next waypoint.

Usage Context:

This function is called by communication message generation routines to populate nextEta and nextVel fields in broadcast messages. Followers use these predictions with predictSwarmState() to extrapolate leader position during message delays.

Limitations:

• Assumes straight-line motion to next waypoint (no turning dynamics)

• Ignores environmental effects (currents, disturbances)

• Only valid for target using waypoint-based guidance

• If n_rate is None, assumes constant speed (no acceleration term)

See also

getNextWpt

Retrieves next waypoint coordinates

predictSwarmState

Uses predictions to extrapolate follower group states

munetauvsim.guidance.ALOSlaw(vehicle, pt1, pt2)

Compute desired heading using Adaptive Line-of-Sight (ALOS) guidance law.

Implements Fossen’s ALOS algorithm for path following with automatic compensation for ocean current drift via adaptive crab angle estimation. Provides robust tracking of straight-line path segments between waypoints.

Parameters:

• vehicle (Vehicle) –

  Vehicle object. Must have the following attributes defined:

  • sampleTimefloat

    Euler integration time step in seconds.

  • etandarray, shape (6,)

    Position/attitude [x, y, z, phi, theta, psi] in END frame.

  • beta_c_hatfloat

    Current estimate of crab angle in radians. Updated by this function.

  • Deltafloat

    Look-ahead distance in meters (typically 10-50 m). Larger values give smoother but less responsive tracking. Should be >> cross-track error.

  • gammafloat

    Adaptive gain constant (positive, typically 0.0001-0.001). Larger values give faster crab angle adaptation but may cause oscillation.

• pt1 (list of float, [x, y, z]) – Previous waypoint in END coordinates (meters).

• pt2 (list of float, [x, y, z]) – Next waypoint in END coordinates (meters).

Returns:

psi_ref (float) – Desired heading angle in radians. Range: [-pi, pi]. Feed to heading observer/autopilot for closed-loop tracking.

Return type:

float

Notes

Side Effects:

Updates vehicle.beta_c_hat via forward Euler integration.

ALOS Guidance Law:

The complete guidance equation is:

psi_ref = pi_h - beta_c_hat - atan(y_e / Delta)

where:

• psi_ref: Desired heading angle (output)

• pi_h: Path-tangential angle (azimuth) bearing from pt1 to pt2 wrt East

• beta_c_hat: Estimated crab angle (adapted online)

• y_e: Cross-track error (perpendicular distance to path)

• Delta: Look-ahead distance (design parameter)

Adaptive Crab Angle:

The crab angle beta_c compensates for sideslip caused by ocean currents or other environmental disturbances. The adaptation law:

d/dt beta_c_hat = gamma * Delta * y_e / sqrt(Delta^2 + y_e^2)

has the following properties:

• Converges to steady-state crab angle for constant current

• Integral action drives cross-track error to zero

• Normalized by sqrt term for bounded adaptation rate

• Gamma * Delta product determines convergence speed

Tuning Guidelines:

Look-ahead distance Delta:

• Larger: Smoother path, slower response, tolerates larger y_e

• Smaller: Tighter tracking, more sensitive to disturbances

• Rule of thumb: Delta = 2-5x vehicle length

• Must satisfy: Delta >> max expected y_e for stability

Adaptive gain gamma:

• Larger: Faster convergence, potential oscillation

• Smaller: Slower convergence, smoother behavior

• Typical range: 0.0001-0.001 for AUV applications

• Scale with 1/speed for speed-invariant adaptation

State Calculation Dependencies:

Internally calls nav.statePT(vehicle, pt1, pt2) which computes:

• x_e: Along-track error (progress along path)

• y_e: Cross-track error (perpendicular to path)

• pi_h: Path-tangential azimuth angle

Warning

• Do not set Delta too small (< 2 m) or division by small numbers causes numerical issues

• Gamma too large causes oscillation or instability

• beta_c_hat should be initialized to 0 or small value

• Singularity at Delta = 0 (avoided by proper parameter selection)

See also

navigation.statePT

Computes path-tangential state (x_e, y_e, pi_h)

navigation.headingFilterLOS

Heading observer for ALOS reference tracking

pathFollow

Complete guidance system using ALOS

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.

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

munetauvsim.guidance.pathFollow(vehicle)

Waypoint-based path following guidance system.

Implements a complete guidance system that coordinates waypoint tracking, heading observation, depth filtering, and autopilot control. Manages waypoint transitions and coordinates all guidance/control components. This function is designed to be assigned to a vehicle as the GuidSystem callable attribute.

Parameters:

vehicle (Vehicle) –

Vehicle object. Must have the following assigned methods and attributes:

• GuidLawcallable

  Core guidance algorithm implementing specific mathematical approaches for path generation.

• HeadingObscallable

  Heading state estimator and reference generator.

• HeadingAPcallable

  Heading autopilot implementing control commands for rudder defleciton.

• DepthObscallable

  Depth command filter and state estimator.

• DepthAPcallable

  Depth autopilot implementing control commands for stern plane deflection.

• PropCmdcallable

  Propeller command generator for thrust management.

• wptWaypoint

  Waypoint database.

• etandarray, shape (6,)

  Position/attitude state.

Returns:

u_control (ndarray, shape (3,)) – Control command vector [delta_r, delta_s, n] where:

• delta_rfloat

  Rudder angle in radians from HeadingAP output.

• delta_sfloat

  Stern plane angle in radians from DepthAP output.

• nfloat

  Propeller RPM command from PropCmd output.

Return type:

NPFltArr

Notes

Workflow:

The function implements a three-stage control pipeline with configurable components:

1. Waypoint Management

• Updates waypoint index when vehicle enters acceptance radius

• Provides previous and next waypoint coordinates to guidance law

• Logs waypoint arrival events for mission tracking

2. Guidance Law Computation

• Processes waypoint coordinates to compute desired trajectory

• Generates heading reference command (psi_ref) for path following

• Updates any internal guidance states (e.g., adaptive parameters)

3. Navigation and Control Coordination:

• Heading Channel:

  • HeadingObs(psi_ref): Processes heading reference through observer/ filter to generate heading commands and rate references

  • HeadingAP(): Converts heading error to rudder deflection command

• Depth Channel:

  • DepthObs(nextPt): Processes depth reference from next waypoint through observer/filter for depth command generation

  • DepthAP(): Converts depth error to stern plane deflection command

• Speed Control:

  • PropCmd(): Generates propeller command (RPM or thrust) for speed regulation or constant speed operation

Each component (GuidLaw, HeadingObs, HeadingAP, DepthObs, DepthAP, PropCmd) is assigned to the vehicle as a callable function, allowing different implementations to be plugged in without modifying this coordination workflow. This design supports various guidance algorithms (ALOS, LOS, etc.), observer types (Kalman filters, complementary filters), and autopilot configurations (PID, LQR, etc.) through a common interface.

See also

vehicles.Remus100s.loadPathFollowing

ALOSlaw

Computes desired heading

updateWpt

Manages waypoint transitions

navigation.headingFilterLOS

Heading observer

navigation.depthFilter

Depth filter

control.headingPID

Heading autopilot

control.depthPID

Depth autopilot

control.constProp

Constant propeller command

munetauvsim.guidance.velCB(vehicle)

Constant bearing velocity guidance law for target tracking.

Implements parallel navigation (constant bearing) where the follower maintains a constant bearing angle toward the target while matching target velocity. This function is designed to be assigned to a vehicle as the GuidLaw callable attribute as part of a target tracking guidance system.

Parameters:

vehicle (Vehicle) –

Vehicle object (follower). Must have:

• Deltafloat

  Look-ahead/approach distance in meters (typically 50-200 m).

• r_safefloat

  Minimum safe separation distance in meters.

• u_maxfloat

  Maximum vehicle speed in m/s (saturates output velocity).

• etandarray, shape (6,)

  Follower position [x, y, z, phi, theta, psi] in END frame.

• velocityndarray, shape (3,)

  Follower velocity [vx, vy, vz] in m/s (END frame).

• targetModel or Vehicle

  Leader/target vehicle. Must have:

  • etandarray, shape (6,)

    Target position in END frame.

  • velocityndarray, shape (3,)

    Target velocity in m/s.

  • nextEtandarray, shape (3,)

    Predicted next position (from communication).

Returns:

v_d (ndarray, shape (3,)) – Desired velocity vector [vx, vy, vz] in END frame (m/s). Saturated to magnitude u_max and depth-limited by network.

Return type:

NPFltArr

Notes

Constant Bearing:

Computes the desired velocity vector (v_d) using a constant bearing guidance law

v_d = v_t + v_los
v_los = -Kappa * p_los / norm(p_los)
Kappa = U_amax * norm(p_los) / sqrt(Delta^2 + (p_los*p_los^T))
p_los = p - p_t - r_safe

The follower uses parallel navigation to match the target velocity vector (v_t) and aligns the follower-target velocity (v_los) along the Line of Sight (LOS) vector between the follower and the target.

The LOS vector is constructed from the follower position (p) and the target position (p_t), modified by a minimum safe distance between the vehicles (r_safe). Delta is the look ahead distance, U_amax is the maximum speed along the LOS vector, and Kappa is the approach velocity regulation coefficient.

Velocity Triangle Geometry:

Applies law of sines/cosines to solve for the desired velocity that:

1. Maintains constant bearing to target

2. Matches target velocity (avoids separation/collision)

3. Results in closure (approaches target) or parallel motion

Design Parameter Delta (Look-ahead distance):

• Controls how aggressively to approach target

• Larger Delta: Gentle, smooth approach

• Smaller Delta: Tight, aggressive interception

• Typical values: 50-200 m for AUV swarms

See also

vehicles.Remus100s.loadTargetTracking

Assigns guidance system

velocitySubsumptionCascadeAPF

APF-based guidance

targetTrack

Target tracking guidance system

References

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

munetauvsim.guidance.predictSwarmState(vehicle)

Update predicted postion/velocity for swarm group members using data models.

Extrapolates each group member’s state based on last received message and elapsed communication delay. Enables followers to maintain accurate estimated positions of neighbors even during communication gaps. For simulations that do not use Direct Accessing.

Parameters:

vehicle (Vehicle) –

Follower vehicle whose swarm group state should be updated. Must have:

• sampleTimefloat

  Simulation iteration time step in seconds.

• idint

  Vehicle ID (excludes self from group predictions).

• targetModel or Vehicle

  Leader vehicle. Must have:

  • etandarray, shape (6,)

    Last known position.

  • velocityndarray, shape (3,)

    Last known velocity.

  • nextEtandarray, shape (3,)

    Predicted next position from communication.

  • nextVelndarray, shape (3,)

    Predicted next velocity from communication.

  • delayLastMsgfloat

    Time elapsed since last message in seconds.

• grouplist of Model

  Other swarm members. Each must have:

  • idint

    Vehicle ID.

  • etandarray, shape (6,)

    Last known position.

  • velocityndarray, shape (3,)

    Last known velocity.

  • delayLastMsgfloat

    Time elapsed since message in seconds.

Returns:

None

Return type:

None

Notes

Side Effects:

Updates for target vehicle:

• target.eta : Propagated via constant acceleration model

• target.delayLastMsg : Incremented by sampleTime

• target.nextEta, target.nextVel : Reused for next prediction

Updates for group members:

• member.eta : Propagated via constant velocity model

• member.delayLastMsg : Incremented by sampleTime

Data on swarm group member states is not logged. Predictions overwrite previous held values.

Target Predictions:

Projects target.velocity and target.nextVel onto distance vector from target.eta to target.nextEta (d_l) and, assuming constant acceleration, estimates position and velocity along d_l.

x = x_0 + v_0*t + 0.5*a*t^2 v = v_0 + a*t

Group Predictions:

Updates position using simple assumption of constant velocity.

x = x_0 +v_0*t

See also

predictNextEtaVel

Generates predictive states for communication messages

updateWpt

Updates target waypoint position

velocitySubsumptionCascadeAPF

Uses group state for APF forces

communication.writeEtaVelLogs

Logs reported position and velocities

munetauvsim.guidance.missionTargetFeedForwardAPF(vehicle)

Compute feed-forward control velocity vector from leader velocity.

Returns the leader’s current velocity vector directly as the contol velocity command. Provides a feed-forward term that drives the vehicle to match the target’s instantaneous velocity, independent of relative position. Only takes the target vehicle into consideration. Designed to be assigned to a vehicle as the GuidLaw.mission callable attribute as part of a guidance system.

Parameters:

vehicle (Vehicle) –

Follower vehicle computing the control vector. Must have:

• targetModel or Vehicle

  Leader vehicle. Must have:

  • velocityndarray, shape (3,) or greater

    Leader velocity [vx, vy, vz, …] in END frame (m/s). Only the first three elements are used.

Returns:

v_c (ndarray, shape (3,)) – Control velocity vector [vx, vy, vz] in END frame (m/s). Equal to the leader’s current velocity vector.

Return type:

NPFltArr

Notes

Feed-Forward Control:

Returns a copy of the leader’s velocity vector directly:

v_c = target.velocity[0:3]

As the lowest-priority layer in the subsumption architecture, this vector is scaled by the residual speed budget remaining after collision avoidance and formation keeping. When higher-priority layers are inactive, this term drives the follower to match the leader’s velocity, providing smooth coordinated motion without relying solely on the reactive APF layers.

Role in Subsumption Architecture:

In a stable formation where collision avoidance and formation keeping produce little or no output, this feed-forward term becomes the dominant velocity command. This is consistent with the formation stability assumption: when the swarm is in formation, the follower should simply match the leader’s velocity to maintain relative position.

Modularity:

This function implements one specific control algorithm for the GuidLaw.mission role. Alternative algorithms can be assigned to GuidLaw.mission, provided they accept a vehicle argument and return an ndarray of shape (3,) in the END frame. Possible alternatives include waypoint-tracking control vectors, mission objective vectors, or operator-commanded velocity inputs.

See also

velocitySubsumptionCascadeAPF

Subsumption guidance law

survivalGroupNormPolyAPF

Collision avoidance from group vehicles

formationTargetNormPolyAPF

Formation keeping with leader vehicle

depthSafetyLimit

Depth safety constraint applied after control vector

munetauvsim.guidance.formationTargetNormPolyAPF(vehicle)

Compute formation keeping APF vector using a normalized polynomial function.

Generates a velocity vector that drives the vehicle to maintain a preferred following distance from the leader using zones of different behavior, with repulsion and attraction governed by a normalized polynomial APF. Produces repulsion when too close, attraction when too far, and a deadband neutral zone around the preferred following distance. Only computes APF between vehicle and target. Designed to be assigned to a vehicle as the GuidLaw.formation callable attribute as part of a guidance system.

Parameters:

vehicle (Vehicle) –

Follower vehicle computing formation keeping. Must have:

• etandarray, shape (6,)

  Own position [x, y, z, …] in END frame.

• u_maxfloat

  Maximum vehicle speed in m/s. Sets velocity ceiling.

• r_safefloat

  Safety radius in meters. Hard maximum repulsion applied inside this distance.

• r_innerfloat

  Inner formation boundary in meters. Outer boundary of the repulsion zone and inner boundary of the neutral deadband. Must satisfy r_safe < r_inner < r_follow.

• r_followfloat

  Preferred following distance in meters. Center of neutral zone.

• targetVehicle

  Leader vehicle. Must have:

  • etandarray, shape (6,)

    Leader position [x, y, z, …] in END frame.

Returns:

v_fk (ndarray, shape (3,)) – Formation keeping velocity vector in END frame (m/s). Directed along the radial vector between the vehicle and the leader. Magnitude ranges from -u_max (maximum repulsion) to +u_max (maximum attraction).

Return type:

NPFltArr

Notes

Zone Geometry:

Five zones are defined along the radial distance r from the vehicle to the leader. The zone structure is symmetric around r_follow: the attraction zone width equals the repulsion zone width. r_outer and r_max are derived quantities, not free parameters:

• r_outer = 2 * r_follow - r_inner # Outer neutral boundary

• r_max = 2 * r_follow - r_safe # Maximum attraction radius

Piecewise APF Function:

The velocity magnitude v(r) along r_hat (unit vector toward leader) is:

1. Safety Zone (r <= r_safe)

   • v(r) = -u_max

   • Hard maximum repulsion away from leader

   • Prevents collision when dangerously close

2. Repulsion Zone (r_safe < r < r_inner)

   • v(r) = -u_max * ((r_inner - r) / (r_inner - r_safe))^p

   • Polynomial decay from -u_max at r_safe to 0 at r_inner

   • Exponent p = 3

3. Neutral Zone / Deadband (r_inner <= r <= r_outer)

   • v(r) = 0

   • No force applied; formation keeping APF produces no velocity input

   • Wide neutral zone (aka Deadband) centered on r_follow prevents oscillation between repulsion and attraction

4. Attraction Zone (r_outer < r < r_max)

   • v(r) = +u_max * ((r - r_outer) / (r_max - r_outer))^p

   • Polynomial increase from 0 at r_outer to u_max at r_max

   • Exponent p = 3

5. Maximum Attraction Zone (r >= r_max)

   • v(r) = +u_max

   • Hard maximum attraction toward leader

   • Enables fast recovery from large separation distances

Symmetric Zone Design:

The repulsion zone width (r_inner - r_safe) equals the attraction zone width (r_max - r_outer), and both use the same polynomial exponent. This produces a symmetric potential well centered on r_follow.

Modularity:

This function implements one specific formation keeping algorithm for the GuidLaw.formation role. Alternative algorithms can be assigned to GuidLaw.formation, provided they accept a vehicle argument and return an ndarray of shape (3,) in the END frame.

Relationship to survivalGroupNormPolyAPF:

Both functions use the same normalized polynomial structure, but serve different roles in the subsumption hierarchy. This formation keeping function operates only on the leader and includes both repulsion and attraction zones. survivalGroupNormPolyAPF operates on all group members (excludes leader) and uses only repulsion.

See also

velocitySubsumptionCascadeAPF

Subsumption guidance law

survivalGroupNormPolyAPF

Collision avoidance from group vehicles

missionTargetFeedForwardAPF

Control vector from target velocity

depthSafetyLimit

Depth safety constraint applied after formation keeping

munetauvsim.guidance.survivalGroupNormPolyAPF(vehicle)

Compute collision avoidance APF vector with a normalized polynomial function

Generates a repulsive velocity vector away from swarm group members that are within the avoidance radius. Uses a normalized polynomial repulsion function with a maximum at the safety radius and zero at the avoidance radius. Only checks vehicles that are in the group list. Designed to be assigned to a vehicle as the GuidLaw.survival callable attribute as part of a guidance system.

Parameters:

vehicle (Vehicle) –

Vehicle computing collision avoidance. Must have:

• etandarray, shape (6,)

  Own position [x, y, z, …] in END frame.

• u_maxfloat

  Maximum vehicle speed in m/s. Sets repulsion ceiling.

• r_safefloat

  Safety radius in meters. Maximum repulsion applied inside this distance.

• r_avoidfloat

  Avoidance radius in meters. Outer boundary of repulsion influence. No repulsion applied beyond this distance.

• grouplist of Vehicle

  Swarm group members. Each must have:

  • etandarray, shape (6,)

    Member position [x, y, z, …] in END frame.

Returns:

v_ca (ndarray, shape (3,)) – Total collision avoidance velocity vector in END frame (m/s). Magnitude clamped to u_max if multiple neighbors cause the sum to exceed vehicle capability.

Return type:

NPFltArr

Notes

Polynomial Repulsion Function:

For each group member within the avoidance radius, a repulsion velocity is computed along the radial unit vector from the member toward the vehicle. The total collision avoidance vector is the sum over all contributing members:

v_ca = sum_i( v(r_i) * r_hat_i )

The per-member repulsion magnitude v(r) is a piecewise function of the radial distance r to each group member:

1. Safety Zone (r <= r_safe)

   • v(r) = u_max

   • Hard maximum repulsion applied regardless of distance

   • Provides strong barrier against collision

2. Repulsion Zone (r_safe < r < r_avoid)

   • v(r) = u_max * ((r_avoid - r) / (r_avoid - r_safe))^p_rep

   • Polynomial increase as distance decreases toward r_safe

   • Exponent p_rep = 3 produces a smooth cubic onset

3. Outside Avoidance Radius (r >= r_avoid)

   • v(r) = 0

   • No collision avoidance influence

Output Clamping:

When multiple neighbors contribute simultaneously, the summed vector can exceed u_max. The total is clamped to u_max while preserving direction:

if norm(v_ca) > u_max: v_ca = u_max * (v_ca / norm(v_ca))

Modularity:

This function implements one specific collision avoidance algorithm for the GuidLaw.survival role. Alternative algorithms can be assigned to GuidLaw.survival, provided they accept a vehicle argument and return an ndarray of shape (3,) in the END frame.

Relationship to formationTargetNormPolyAPF:

Both functions use the same normalized polynomial structure, but serve different roles in the subsumption hierarchy. This survival function operates on all group members and uses only repulsion. formationTargetNormPolyAPF operates only on the leader and includes both repulsion and attraction zones.

See also

velocitySubsumptionCascadeAPF

Subsumption guidance law

formationTargetNormPolyAPF

Formation keeping layer

missionTargetFeedForwardAPF

Control layer

depthSafetyLimit

Depth safety constraint applied after collision avoidance

munetauvsim.guidance.depthSafetyLimit(vehicle, vel)

Apply depth safety constraint.

Modifies desired velocity to enforce minimum distance from ocean floor (z=seabed_z - z_safe).

Parameters:

• vehicle (Vehicle) –

  Vehicle with z_max, seabed_z, z_safe, eta, and velocity attributes.

  • z_max: Maximum operating depth (m).

  • seabed_z: Sensed ocean floor depth (m).

  • z_safe: Safety distance from ocean floor (m).

  • eta : [x, y, z, phi, theta, psi], vehicle position / attitude vector

  • velocity : [vx, vy, vz], vehicle velocity vector (m/s)

  • z_safe: Safety Distance from maximum depth limit (m)

• vel (ndarray, shape (3,)) – Desired velocity vector [vx, vy, vz] in END.

Returns:

vel_constrained (ndarray, shape (3,)) – Modified velocity respecting depth limits.

Return type:

NPFltArr

Notes

• Attempts to prevent ground collision (z > seabed_z - z_safe).

When the vehicle is below the Safety Distance threshold from the Maximum Depth Limit, the depth component of the velocity command is cancelled out and any velocity the vehicle has in the downward direction is opposed in proportion to how far past the Safety Distance threshold the vehicle has travelled.

v_z = -velocity_command_z - (gamma * velocity_z), gamma = (z - safety_depth) / safety_distance

munetauvsim.guidance.velocitySubsumptionCascadeAPF(vehicle)

APF velocity guidance law with subsumption-based saturation cascade.

Combines three Artificial Potential Field (APF) behaviors – collision avoidance, formation keeping, and a control vector – into a single desired velocity vector. Behavioral priority follows the subsumption architecture of Brooks [1], where higher-priority layers inhibit lower-priority layers. Behaviors are fused by weighted vector summation following Arkin’s motor schema formulation [2]. The weight computation uses saturation ratios that cascade multiplicatively through the priority hierarchy, analogous to the nested null-space projections in the NSB approach of Antonelli et al. [3] but reduced to scalar velocity-budget residuals. This function is designed to be assigned to a vehicle as the GuidLaw callable attribute as part of a guidance system.

Parameters:

vehicle (Vehicle) –

Vehicle object. Must have:

• GuidLaw.survivalcallable

  Computes collision avoidance velocity vector. Highest priority layer.

• GuidLaw.formationcallable

  Computes formation keeping velocity vector. Middle priority layer.

• GuidLaw.missioncallable

  Computes control / misison velocity vector. Lowest priority layer.

• u_maxfloat

  Maximum vehicle speed in m/s. Defines total velocity budget.

• CommNetworkobject or None

  Communication network manager. If not None, swarm state is predicted via predictSwarmState() before guidance evaluation.

Returns:

v_d (ndarray, shape (3,)) – Desired velocity vector [vx, vy, vz] in END frame (m/s). Magnitude limited to u_max. Depth-constrained by depthSafetyLimit().

Return type:

NPFltArr

Notes

Subsumption Architecture:

Behaviors are organized into a strict priority hierarchy. Each layer can fully suppress all layers below it by consuming the shared velocity budget. The total available speed u_max is treated as the budget, and each layer draws down a fraction proportional to its output magnitude:

1. Collision Avoidance (highest layer)

   • Saturation ratio: s_ca = norm(v_ca) / u_max

   • Scaling weight: w_ca = 1.0 (always full weight)

   • Residual passed down: w_fk = 1 - (w_ca * s_ca)

   • When s_ca = 1: all lower layers are fully suppressed

2. Formation Keeping (middle layer)

   • Saturation ratio: s_fk = norm(v_fk) / u_max

   • Scaling weight: w_fk = 1 - (w_ca * s_ca)

   • Residual passed down: w_m = w_fk * (1 - s_fk)

   • Fully suppressed when collision avoidance saturates the budget

3. Mission (lowest layer)

   • Scaling weight: w_m = w_fk * (1 - s_fk)

   • Only active when neither collision nor formation consumes the budget

   • Fully suppressed when either higher layer saturates the budget

Total Velocity:

v_tot = (w_ca * v_ca) + (w_fk * v_fk) + (w_m * v_m)

Clamped to u_max if the total magnitude exceeds the vehicle limit, then passed through depthSafetyLimit() for depth safety enforcement.

The subsumption model allows higher-priority behaviors to dominate when needed: when collision avoidance is active it progressively suppresses formation keeping, and both suppress the mission vector. This prevents lower-priority behaviors from counteracting safety-critical responses.

The general N-layer pattern:

w[0] = alpha
w[1] = 1.0 - (w[0] * s[0])
w[2] = w[1] * (1.0 - s[1])
w[3] = w[2] * (1.0 - s[2])
...

Full Suppression:

When s_ca = 1 (collision avoidance saturates u_max), the formation keeping residual w_fk = 0, which in turn forces w_m = 0. Both lower layers are completely inhibited and the vehicle behaves purely as a collision avoidance agent, consistent with subsumption’s hard behavioral priority guarantee.

Multiplicative Cascade:

The mission weight w_m = w_fk * (1 - s_fk) means the mission layer is doubly suppressed: first by how much collision avoidance consumes the budget (via w_fk), then by how much formation keeping consumes what remains (via 1 - s_fk). This nested structure mirrors the cascaded null-space projections of the NSB framework, but operating on scalar budget residuals.

Communication:

If vehicle.CommNetwork is not None, predictSwarmState() is called at the start of each evaluation to propagate estimated member positions forward using a prediction model.

References

[1] R. Brooks, “A robust layered control system for a mobile robot,” in IEEE Journal on Robotics and Automation, vol. 2, no. 1, pp. 14-23, March 1986, doi: 10.1109/JRA.1986.1087032.

[2] R. C. Arkin, “Motor schema-based mobile robot navigation,” The International Journal of Robotics Research. 1989;8(4):92-112. doi:10.1177/027836498900800406

[3] G. Antonelli, F. Arrichiello and S. Chiaverini, “Experiments of Formation Control With Multirobot Systems Using the Null-Space-Based Behavioral Control,” in IEEE Transactions on Control Systems Technology, vol. 17, no. 5, pp. 1173-1182, Sept. 2009, doi: 10.1109/TCST.2008.2004447.

See also

survivalGroupNormPolyAPF

Collision avoidance from group vehicles

formationTargetNormPolyAPF

Formation keeping with leader vehicle

missionTargetFeedForwardAPF

Control/tracking vector from leader velocity

depthSafetyLimit

Depth safety constraint enforcement

predictSwarmState

Swarm state prediction during communication gaps

targetTrack

Target tracking guidance system

munetauvsim.guidance.targetTrack(vehicle)

Target tracking guidance system.

Implements a complete guidance system that coordinates velocity guidance into control commands via heading and depth autopilots. Converts high-level desired velocity into low-level control surface commands for path execution. This function is designed to be assigned to a vehicle as the GuidSystem callable attribute.

Parameters:

vehicle (Vehicle) –

Vehicle object. Must have the following assigned methods:

• GuidLawcallable

  Core guidance algorithm implementing specific mathematical approaches for path generation.

• HeadingAPcallable

  Heading autopilot implementing control commands for rudder defleciton.

• DepthAPcallable

  Depth autopilot implementing control commands for stern plane deflection.

• xferU2Ncallable

  Function to convert speed to propeller RPM

Returns:

u_control (ndarray, shape (3,)) – Control command vector [delta_r, delta_s, n] where:

• delta_rfloat

  Rudder angle in radians from HeadingAP output.

• delta_sfloat

  Stern plane angle in radians from DepthAP output.

• nfloat

  Propeller RPM command from xferU2N output.

Return type:

NPFltArr

Notes

Workflow:

The function implements a desired-velocity-to-control conversion pipeline with configurable guidance components:

1. Velocity Guidance Computation

   • Processes target state and environmental constraints

   • Generates desired velocity vector in END frame

   • Handles swarm coordination and target tracking

2. Velocity-to-Attitude Conversion

   • Computes desired course angle (chi) from horizontal velocity components

   • Computes desired pitch angle (theta) from vertical velocity component

   • Maps velocity commands to vehicle attitude references for autopilot control

3. Autopilot Control Coordinator

• Heading Channel:

  • Sets vehicle.psi_d = chi (course-to-heading mapping)

  • HeadingAP(): Converts heading error to rudder deflection command

• Depth/Pitch Channel:

  • Sets vehicle.theta_d = theta (pitch reference)

  • DepthAP(): Converts pitch/depth error to stern plane deflection command

• Speed Control:

  • xferU2N(): Converts desired speed magnitude to propeller RPM command

Each component (GuidLaw, HeadingAP, DepthAP, xferU2N) is assigned to the vehicle as a callable function, allowing different implementations to be plugged in without modifying this coordination workflow. This design supports various velocity guidance algorithms (APF-based, geometric, reactive), autopilot configurations (PID, LQR, sliding mode), and propulsion models through a common interface.

Key Design Feature:

Unlike waypoint-based pathFollow(), this system operates on instantaneous velocity commands, making it suitable for reactive behaviors like target tracking, obstacle avoidance, and formation control where desired trajectory changes dynamically based on real-time conditions.

Current Limitations:

• Direct velocity-to-attitude mapping assumes no environmental filtering

• Course angle (chi) used directly as heading reference without drift compensation

• Pitch angle computed geometrically without considering depth reference

• Future versions should incorporate state estimation and wave filtering

See also

vehicles.Remus100s.loadTargetTracking

velocitySubsumptionCascadeAPF

APF velocity guidance

velCB

Constant bearing velocity guidance

missionTargetFeedForwardAPF

Target velocity feed-forward

formationTargetNormPolyAPF

Attraction and repulsion to target vehicle

survivalGroupNormPolyAPF

Repulsion from all group vehicles

control.headingPID

Heading autopilot

control.depthPID

Depth autopilot