munetauvsim.navigation

Navigation functions and sensor classes for AUV state estimation.

Implements the Navigation block of GNC design for determining vehicle position, attitude, velocity, course, and distance traveled. Provides sensor abstractions, coordinate transformations, state observers, and filtering algorithms.

Classes

Sensor: Abstract base class for sensor implementations.
OceanCurrentSensor: Measures ocean current speed and direction.
OceanDepthSensor: Measures ocean floor depth at vehicle position.

Functions

Coordinate Transformations:

attitudeEuler(vehicle) : Integrate vehicle attitude using Euler angles.

Rzyx(phi, theta, psi) : Rotation matrix in SO(3) using zyx convention.

Tzyx(phi, theta) : Attitude transformation matrix using zyx convention.

State Computation:

statePT(vehicle, pt1, pt2) : Path-tangential angle and track errors.

stateSpeed(vehicle) : Vehicle speed magnitude.

Observers and Filters:

headingFilterLOS(vehicle, psi_ref) : LOS heading observer with yaw rate estimation.

depthFilter(vehicle, pt) : Exponential moving average depth filter.

maxDepthLimit(vehicle, z) : Enforce depth safety limits.

Notes

Navigation block inputs: Sensors, vehicle motion
Navigation block outputs: State vectors to Guidance and Control blocks

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

`Rzyx`(phi, theta, psi)	Compute the 3x3 Euler angle rotation matrix R in SO(3) using the zyx convention.
`Tzyx`(phi, theta)	Compute the 3x3 Euler angle attitude transformation matrix T using the zyx convention.
`abstractmethod`(funcobj)	A decorator indicating abstract methods.
`attitudeEuler`(vehicle)	Integrate the generalized position/Euler angles vector (eta[k+1]), and the velocity vector in END reference frame (p_dot).
`depthFilter`(vehicle, pt)	Update vehicle depth command using exponential moving average filter.
`headingFilterLOS`(vehicle, psi_ref)	Update the vehicle heading command and yaw rate using LOS observer.
`maxDepthLimit`(vehicle, z)	Enforce vehicle depth limits based on operating limit and ocean floor depth.
`statePT`(vehicle, pt1, pt2)	Compute path-tangential angle and track errors for line segment following.
`stateSpeed`(vehicle)	Compute vehicle speed magnitude in END frame.

Classes

`ABC`()	Helper class that provides a standard way to create an ABC using inheritance.
`NDArray`	alias of `ndarray`[`Any`, `dtype`[`ScalarType`]]
`NPFltArr`	alias of `ndarray`[`Any`, `dtype`[`float64`]]
`OceanCurrentSensor`()	Sensor for measuring ocean current speed and direction.
`OceanDepthSensor`()	Sensor for measuring ocean floor depth at vehicle position.
`Sensor`()	Abstract base class for sensor implementations.

class munetauvsim.navigation.Sensor[source]

Bases: ABC

Abstract base class for sensor implementations.

Defines interface for sensor objects that collect data from the simulation environment. Subclasses must implement collectData() method.

Notes

Sensors as objects supports ability to create a data log or to cache data on the sensor itself.
Consider writing an __init__ method for class-wide attributes, such as a ‘name’ string that provides a default for the AUV.sensor dictionary to use as a reference.

abstract collectData(**kwargs)[source]

Collect sensor measurement data.

Returns:: data (Any) – Sensor-specific measurement data.
Return type:: Any

class munetauvsim.navigation.OceanCurrentSensor[source]

Bases: Sensor

Sensor for measuring ocean current speed and direction.

Reads current data from Ocean object at specified simulation iteration.

collectData(i=None, ocean=None, **kwargs)[source]

Measure ocean current at simulation iteration i.

Parameters:

i (int) – Simulation iteration counter.
ocean (Ocean) – Ocean object with current.speed and current.angle arrays.
**kwargs – Unused. Required for AUV sensor interface compatibility.

Returns:

speed (float) – Current speed in m/s.
direction (float) – Current direction in radians.

Return type:

List[float]

Notes

Returns [-1.0, -1.0] on error with log message.

class munetauvsim.navigation.OceanDepthSensor[source]

Bases: Sensor

Sensor for measuring ocean floor depth at vehicle position.

Queries Ocean.floor() method at vehicle’s (x, y) coordinates.

collectData(ocean=None, eta=None, **kwargs)[source]

Measure ocean depth at vehicle position.

Parameters:

ocean (Ocean) – Ocean object with floor(x, y) method.
eta (ndarray, shape (6,)) – Vehicle position/attitude [x, y, z, phi, theta, psi].
**kwargs – Unused. Required for AUV sensor interface compatibility.

Returns:

depth (float) – Ocean floor depth in meters at position (eta[0], eta[1]).

Return type:

float

Notes

Returns -1.0 on error with log message. Returns np.inf when ocean.floor is None (no bathymetry modeled), signalling an unbounded depth below the vehicle.

munetauvsim.navigation.attitudeEuler(vehicle)[source]

Integrate the generalized position/Euler angles vector (eta[k+1]), and the velocity vector in END reference frame (p_dot).

Parameters:

vehicle (Vehicle) –

Vehicle with sampleTime, eta, nu attributes.

sampleTime: Simulation time step.
eta : [x, y, z, phi, theta, psi], vehicle position / attitude vector.
nu : [u, v, w, p, q, r], vehicle linear / angular velocity vector in BODY frame.

Returns:

eta (ndarray, shape (6,)) – Updated position/attitude [x, y, z, phi, theta, psi].
p_dot (ndarray, shape (3,)) – Velocity in END frame [x_dot, y_dot, z_dot].

Return type:

Tuple[NPFltArr, NPFltArr]

Notes

Uses forward Euler integration.
Position integrated from END velocities, attitude from body angular rates via transformation matrices.
Based on Fossens Python Vehicle Simulator.

References

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

munetauvsim.navigation.Rzyx(phi, theta, psi)[source]

Compute the 3x3 Euler angle rotation matrix R in SO(3) using the zyx convention.

Parameters:

phi (float) – Roll angle in radians.
theta (float) – Pitch angle in radians.
psi (float) – Yaw angle in radians.

Returns:

R (ndarray, shape (3, 3)) – Rotation matrix from BODY to END frame.

Return type:

ndarray[Any, dtype[float64]]

References

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

munetauvsim.navigation.Tzyx(phi, theta)[source]

Compute the 3x3 Euler angle attitude transformation matrix T using the zyx convention.

Parameters:

phi (float) – Roll angle in radians.
theta (float) – Pitch angle in radians.

Returns:

T (ndarray, shape (3, 3)) – Transformation matrix mapping body angular rates to Euler angle rates.

Return type:

ndarray[Any, dtype[float64]]

Notes

Singular at theta = +/-90 degrees. Logs error on singularity.

References

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

munetauvsim.navigation.statePT(vehicle, pt1, pt2)[source]

Compute path-tangential angle and track errors for line segment following.

Computes the path-tangential (azimuth) angle (pi_h) with respect to the East axis, and the along-track (x_e) and cross-track (y_e) errors of a vehicle on a path between two points.

Parameters:

vehicle (Vehicle) – Vehicle with eta attribute. eta : [x, y, z, phi, theta, psi], vehicle position/attitude vector
pt1 (list of float, [x, y, z]) – Start point in NED coordinates (m).
pt2 (list of float, [x, y, z]) – End point in NED coordinates (m).

Returns:

x_e (float) – Along-track error from start point pt1 (m).
y_e – Cross-track error from path (m).
pi_h – Path-tangential (azimuth) angle w.r.t. East axis (rad).

Return type:

List[float]

Notes

Based on a section in Fossens ALOSpsi.m function.

References

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

munetauvsim.navigation.stateSpeed(vehicle)[source]

Compute vehicle speed magnitude in END frame.

Parameters:: vehicle (Vehicle) – Vehicle with velocity attribute. velocity : [vx, vy, vz], vehicle linear velocity vector in END frame.
Returns:: speed (float) – Speed magnitude in m/s.
Return type:: float

munetauvsim.navigation.headingFilterLOS(vehicle, psi_ref)[source]

Update the vehicle heading command and yaw rate using LOS observer.

Propagates heading estimate with feedback from reference angle and estimates yaw rate via numerical differentiation.

Parameters:

vehicle (Vehicle) –
Vehicle with sampleTime, psi_d, r_d, K_f attributes.
- sampletime: Simulation time step (s).
- psi_d: Desired heading angle (rad).
- r_d: Desired yaw rate (rad/s).
- K_f: Observer gain for desired yaw angle (typically 0.1-0-5).
psi_ref (float) – Reference LOS angle (rad) computed from guidance system.

Return type:

None

Notes

Based on Fossen’s LOSobserver.m function. The observer propagates the estimate of the LOS angle according to

psi_d = psi_d + h * (r_d + K_f * ssa(psi_ref - psi_d))

where the yaw rate estimate (r_d) is computed by numerical differentiation

r_d = T_f * s / (T_f * s + 1) * psi_d

where T_f is the differentiator time constant, which can be determined by pole-placement and inspection of the closed-loop system

psi_d / psi_ref = w_n^2 * (T_f*s + 1) / (s^2 + 2*w_n*s + w_n^2)

If K_f > 0, it follows that T_f = 1 / (K_f + 2*sqrt(K_f) + 1) and that the natural frequency is w_n = K_f + sqrt(K_f). Exact discretization of the observer gives

r_d = psi_d - xi xi = exp(-h/T_f) * xi + (1 - exp(-h/T_f)) * psi_d

References

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

munetauvsim.navigation.depthFilter(vehicle, pt)[source]

Update vehicle depth command using exponential moving average filter.

Smooths desired depth command with an EMA low-pass filter to reduce control chattering and enforce safety limits.

Parameters:

vehicle (Vehicle) –
Vehicle with sampleTime, z_d, wn_d_z, z_max, z_bed, z_safe attributes.
- sampletime: Simulation time step (s).
- z_d: Desired depth command (m)
- wn_d_z: Desired natural frequency (Hz), depth
- z_max: Maximum operating depth (m).
- seabed_z: Sensed ocean floor depth (m).
- z_safe: Safety distance from ocean floor (m).
pt (list of float, [x, y, z]) – Target waypoint coordinates in END (m).

Return type:

None

Notes

Based on Fossens Remus100 autopilot function.
Filter:

z_d = alpha * z_d_prev + (1 - alpha) * z_target

where:

alpha = exp(-h * wn_d_z).
Calls maxDepthLimit() to enforce maximum depth limit before filtering.
Updates vehicle.z_d in place.

References

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

munetauvsim.navigation.maxDepthLimit(vehicle, z)[source]

Enforce vehicle depth limits based on operating limit and ocean floor depth.

Parameters:

vehicle (Vehicle) –
Vehicle with z_max, seabed_z, z_safe attributes.
- z_max: Maximum operating depth (m).
- seabed_z: Sensed ocean floor depth (m).
- z_safe: Safety distance from ocean floor (m).
z (float) – Intended depth in meters.

Returns:

z_limited (float) – Depth bounded by min(z_max, seabed_z - z_safe).

Return type:

float