Navigation

Inertial navigation, geodesy, and INS/GNSS integration functions.

Navigation utilities for target tracking.

This module provides geodetic and navigation calculations commonly needed in tracking applications, including: - Geodetic coordinate conversions - Inertial Navigation System (INS) mechanization - Alignment algorithms - INS/GNSS integration (loosely and tightly coupled) - Great circle navigation - Rhumb line navigation

class pytcl.navigation.Ellipsoid(a, f)

Bases: NamedTuple

Earth ellipsoid parameters.

a

Semi-major axis (equatorial radius) in meters.

Type:

float

f

Flattening (a-b)/a.

Type:

float

a: float

Alias for field number 0

f: float

Alias for field number 1

property b: float

Semi-minor axis (polar radius) in meters.

property e2: float

First eccentricity squared.

property ep2: float

Second eccentricity squared.

pytcl.navigation.geodetic_to_ecef(lat, lon, alt, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert geodetic coordinates to ECEF (Earth-Centered, Earth-Fixed).

Parameters:

• lat (array_like) – Geodetic latitude in radians.

• lon (array_like) – Geodetic longitude in radians.

• alt (array_like) – Altitude above ellipsoid in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Philadelphia (40°N, 75°W) at 100m altitude
>>> lat, lon, alt = np.radians(40.0), np.radians(-75.0), 100.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> x / 1e6  # ~1.2 million meters
1.24...
>>> # Equator at prime meridian
>>> x, y, z = geodetic_to_ecef(0.0, 0.0, 0.0)
>>> x  # Semi-major axis (equatorial radius)
6378137.0
>>> y, z
(0.0, 0.0)

pytcl.navigation.ecef_to_geodetic(x, y, z, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF coordinates to geodetic.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• lat (ndarray) – Geodetic latitude in radians.

• lon (ndarray) – Geodetic longitude in radians.

• alt (ndarray) – Altitude above ellipsoid in meters.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[float64]], ndarray[tuple[Any, …], dtype[float64]], ndarray[tuple[Any, …], dtype[float64]]]

Examples

>>> import numpy as np
>>> # Point on equator at prime meridian
>>> lat, lon, alt = ecef_to_geodetic(6378137.0, 0.0, 0.0)
>>> np.degrees(lat), np.degrees(lon), alt
(0.0, 0.0, 0.0)
>>> # Round-trip conversion
>>> x, y, z = geodetic_to_ecef(np.radians(45.0), np.radians(90.0), 1000.0)
>>> lat2, lon2, alt2 = ecef_to_geodetic(x, y, z)
>>> np.degrees(lat2), np.degrees(lon2), alt2
(45.0..., 90.0..., 1000.0...)

Notes

Uses Bowring’s iterative algorithm for robust conversion.

References

[1]

Bowring, B.R., “Transformation from spatial to geographical coordinates”, Survey Review, 1976.

pytcl.navigation.ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF to local ENU (East-North-Up) coordinates.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

east, north, up – ENU coordinates in meters relative to reference point.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point: Philadelphia
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target point slightly east
>>> lat, lon, alt = np.radians(40.0), np.radians(-74.99), 0.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> e  # East displacement in meters
850...
>>> abs(n) < 10  # North displacement should be ~0
True
>>> abs(u) < 10  # Up displacement should be ~0
True

pytcl.navigation.enu_to_ecef(east, north, up, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert local ENU to ECEF coordinates.

Parameters:

• east (array_like) – ENU coordinates in meters relative to reference point.

• north (array_like) – ENU coordinates in meters relative to reference point.

• up (array_like) – ENU coordinates in meters relative to reference point.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 1 km east, 500 m north, 100 m up
>>> x, y, z = enu_to_ecef(1000.0, 500.0, 100.0, lat_ref, lon_ref, alt_ref)
>>> # Convert back to verify
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> e, n, u
(1000.0..., 500.0..., 100.0...)

pytcl.navigation.ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF to local NED (North-East-Down) coordinates.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

north, east, down – NED coordinates in meters relative to reference point.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target above reference
>>> x, y, z = geodetic_to_ecef(lat_ref, lon_ref, 1000.0)  # 1km above
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> abs(n) < 1, abs(e) < 1, d  # Should be ~0, ~0, -1000
(True, True, -1000.0...)

pytcl.navigation.ned_to_ecef(north, east, down, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert local NED to ECEF coordinates.

Parameters:

• north (array_like) – NED coordinates in meters relative to reference point.

• east (array_like) – NED coordinates in meters relative to reference point.

• down (array_like) – NED coordinates in meters relative to reference point.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 100m north, 50m east, 10m down
>>> x, y, z = ned_to_ecef(100.0, 50.0, 10.0, lat_ref, lon_ref, alt_ref)
>>> # Verify round-trip
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> n, e, d
(100.0..., 50.0..., 10.0...)

pytcl.navigation.direct_geodetic(lat1, lon1, azimuth, distance, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the direct geodetic problem (Vincenty).

Given a starting point, azimuth, and distance, find the destination point. Results are cached for repeated queries with the same parameters.

Parameters:

• lat1 (float) – Starting latitude in radians.

• lon1 (float) – Starting longitude in radians.

• azimuth (float) – Forward azimuth in radians (from north, clockwise).

• distance (float) – Distance in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• lat2 (float) – Destination latitude in radians.

• lon2 (float) – Destination longitude in radians.

• azimuth2 (float) – Back azimuth at destination in radians.

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> # From New York, travel 1000 km northeast
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
>>> azimuth = np.radians(45)  # Northeast
>>> distance = 1_000_000  # 1000 km
>>> lat2, lon2, az2 = direct_geodetic(lat1, lon1, azimuth, distance)
>>> np.degrees(lat2), np.degrees(lon2)  # Destination
(47.0..., -62.6...)

References

[1]

Vincenty, T., “Direct and Inverse Solutions of Geodesics on the Ellipsoid with Application of Nested Equations”, Survey Review, 1975.

pytcl.navigation.inverse_geodetic(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the inverse geodetic problem (Vincenty).

Given two points, find the distance and azimuths between them. Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – Starting latitude in radians.

• lon1 (float) – Starting longitude in radians.

• lat2 (float) – Destination latitude in radians.

• lon2 (float) – Destination longitude in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• distance (float) – Geodesic distance in meters.

• azimuth1 (float) – Forward azimuth at start in radians.

• azimuth2 (float) – Back azimuth at destination in radians.

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> # Distance from New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)  # NYC
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)   # London
>>> dist, az1, az2 = inverse_geodetic(lat1, lon1, lat2, lon2)
>>> dist / 1000  # Distance in km
5570...
>>> np.degrees(az1)  # Initial heading from NYC
51.2...

Notes

May fail to converge for nearly antipodal points.

References

[1]

Vincenty, T., “Direct and Inverse Solutions of Geodesics on the Ellipsoid with Application of Nested Equations”, Survey Review, 1975.

pytcl.navigation.haversine_distance(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute great-circle distance using the haversine formula.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• radius (float, optional) – Earth radius in meters (default: 6371 km).

Returns:

Great-circle distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> # Distance from equator to 45°N along prime meridian
>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.radians(45.0), 0.0
>>> dist = haversine_distance(lat1, lon1, lat2, lon2)
>>> dist / 1000  # ~5000 km
5003...
>>> # Same point -> 0 distance
>>> haversine_distance(0.0, 0.0, 0.0, 0.0)
0.0

Notes

This is a spherical approximation. For higher accuracy on an ellipsoid, use inverse_geodetic().

class pytcl.navigation.INSState(position, velocity, quaternion, time)

Bases: NamedTuple

Inertial Navigation System state.

position

Position in geodetic coordinates [latitude (rad), longitude (rad), altitude (m)].

Type:

ndarray

velocity

Velocity in NED frame [vN, vE, vD] (m/s).

Type:

ndarray

quaternion

Attitude quaternion [qw, qx, qy, qz] from body to NED frame (scalar-first).

Type:

ndarray

time

Time associated with this state (seconds).

Type:

float

position: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

velocity: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

quaternion: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

time: float

Alias for field number 3

property latitude: float

Geodetic latitude in radians.

property longitude: float

Geodetic longitude in radians.

property altitude: float

Altitude above ellipsoid in meters.

property velocity_north: float

North velocity in m/s.

property velocity_east: float

East velocity in m/s.

property velocity_down: float

Down velocity in m/s.

property dcm: ndarray[tuple[Any, ...], dtype[floating]]

Direction cosine matrix from body to NED frame.

euler_angles()

Get Euler angles (roll, pitch, yaw) in radians.

Returns:

euler – [roll, pitch, yaw] in radians, ZYX convention.

Return type:

ndarray

class pytcl.navigation.IMUData(accel, gyro, dt)

Bases: NamedTuple

IMU measurement data.

accel

Specific force measurements [ax, ay, az] in body frame (m/s^2).

Type:

ndarray

gyro

Angular rate measurements [wx, wy, wz] in body frame (rad/s).

Type:

ndarray

dt

Time interval between measurements (seconds).

Type:

float

accel: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

gyro: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

dt: float

Alias for field number 2

class pytcl.navigation.INSErrorState(position_error, velocity_error, attitude_error, accel_bias, gyro_bias)

Bases: NamedTuple

INS error state for Kalman filtering.

position_error

Position error [delta_lat, delta_lon, delta_alt] (rad, rad, m).

Type:

ndarray

velocity_error

Velocity error in NED [delta_vN, delta_vE, delta_vD] (m/s).

Type:

ndarray

attitude_error

Attitude error angles [phi_N, phi_E, phi_D] (rad).

Type:

ndarray

accel_bias

Accelerometer bias [bax, bay, baz] (m/s^2).

Type:

ndarray

gyro_bias

Gyroscope bias [bwx, bwy, bwz] (rad/s).

Type:

ndarray

position_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

velocity_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

attitude_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

accel_bias: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 3

gyro_bias: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

static zeros()

Create zero error state.

to_vector()

Convert to 15-element state vector.

static from_vector(x)

Create from 15-element state vector.

pytcl.navigation.normal_gravity(lat, alt=0.0)

Compute normal gravity using Somigliana’s formula with free-air correction.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float, optional) – Altitude above ellipsoid in meters (default: 0).

Returns:

g – Normal gravity magnitude in m/s^2.

Return type:

float

Notes

Uses the WGS84 gravity formula with first-order altitude correction.

pytcl.navigation.gravity_ned(lat, alt=0.0)

Compute gravity vector in NED frame.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float, optional) – Altitude above ellipsoid in meters.

Returns:

g_ned – Gravity vector [gN, gE, gD] in m/s^2.

Return type:

ndarray

pytcl.navigation.earth_rate_ned(lat)

Compute Earth rotation rate in NED frame.

Parameters:

lat (float) – Geodetic latitude in radians.

Returns:

omega_ie_n – Earth rotation rate in NED frame [wN, wE, wD] (rad/s).

Return type:

ndarray

pytcl.navigation.transport_rate_ned(lat, alt, vN, vE, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute transport rate (navigation frame rotation) in NED frame.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float) – Altitude above ellipsoid in meters.

• vN (float) – North velocity in m/s.

• vE (float) – East velocity in m/s.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

omega_en_n – Transport rate in NED frame [wN, wE, wD] (rad/s).

Return type:

ndarray

Notes

The transport rate accounts for navigation frame rotation as the vehicle moves over the curved Earth surface.

pytcl.navigation.radii_of_curvature(lat, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute principal radii of curvature.

Parameters:

• lat (float) – Geodetic latitude in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• RN (float) – Meridian radius of curvature (m).

• RE (float) – Transverse radius of curvature (prime vertical) (m).

Return type:

Tuple[float, float]

pytcl.navigation.coning_correction(gyro_prev, gyro_curr)

Compute first-order coning correction for angular increment.

Parameters:

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

Returns:

delta_theta_coning – Coning correction to angular increment (rad).

Return type:

ndarray

Notes

Coning error occurs when the rotation axis itself rotates (coning motion). This first-order correction assumes trapezoidal integration of gyro data.

References

[1]

Savage, P.G., “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms”, AIAA Journal of Guidance, 1998.

pytcl.navigation.sculling_correction(accel_prev, accel_curr, gyro_prev, gyro_curr)

Compute first-order sculling correction for velocity increment.

Parameters:

• accel_prev (array_like) – Previous specific force [ax, ay, az] (m/s^2).

• accel_curr (array_like) – Current specific force [ax, ay, az] (m/s^2).

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

Returns:

delta_v_sculling – Sculling correction to velocity increment (m/s).

Return type:

ndarray

Notes

Sculling error occurs when linear vibration and angular vibration are correlated (e.g., on a flexible structure).

References

[1]

Savage, P.G., “Strapdown Inertial Navigation Integration Algorithm Design Part 2: Velocity and Position Algorithms”, AIAA, 1998.

pytcl.navigation.compensate_imu_data(accel_prev, accel_curr, gyro_prev, gyro_curr, dt)

Compute compensated angular and velocity increments with coning/sculling corrections.

Parameters:

• accel_prev (array_like) – Previous specific force [ax, ay, az] (m/s^2).

• accel_curr (array_like) – Current specific force [ax, ay, az] (m/s^2).

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

• dt (float) – Time interval (seconds).

Returns:

• delta_theta (ndarray) – Compensated angular increment (rad).

• delta_v (ndarray) – Compensated velocity increment (m/s).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

pytcl.navigation.skew_symmetric(v)

Create skew-symmetric matrix from vector.

Parameters:

v (array_like) – 3-element vector.

Returns:

S – 3x3 skew-symmetric matrix.

Return type:

ndarray

pytcl.navigation.update_quaternion(q, delta_theta)

Update quaternion using angular increment.

Parameters:

• q (array_like) – Current quaternion [qw, qx, qy, qz].

• delta_theta (array_like) – Angular increment in body frame (rad).

Returns:

q_new – Updated quaternion (normalized).

Return type:

ndarray

Notes

Uses first-order quaternion update: q_new = q * delta_q where delta_q represents the incremental rotation.

pytcl.navigation.update_attitude_ned(q_b_n, omega_ib_b, omega_in_n, dt)

Update attitude quaternion (body to NED) for one time step.

Parameters:

• q_b_n (array_like) – Current quaternion from body to NED frame.

• omega_ib_b (array_like) – Angular rate of body w.r.t. inertial, in body frame (rad/s).

• omega_in_n (array_like) – Angular rate of NED w.r.t. inertial, in NED frame (rad/s).

• dt (float) – Time step (seconds).

Returns:

q_new – Updated quaternion (body to NED).

Return type:

ndarray

Notes

The attitude update accounts for both body rotation and navigation frame rotation. omega_in_n = omega_ie_n + omega_en_n (Earth rate + transport rate)

pytcl.navigation.mechanize_ins_ned(state, imu, ellipsoid=(6378137.0, 0.0033528106647474805), accel_prev=None, gyro_prev=None)

Perform one step of strapdown INS mechanization in NED frame.

Parameters:

• state (INSState) – Current INS state.

• imu (IMUData) – IMU measurements for this time step.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

• accel_prev (array_like, optional) – Previous accelerometer reading for coning/sculling corrections.

• gyro_prev (array_like, optional) – Previous gyroscope reading for coning/sculling corrections.

Returns:

new_state – Updated INS state after mechanization.

Return type:

INSState

Notes

This implements the standard NED-frame strapdown mechanization: 1. Attitude update (quaternion integration) 2. Specific force transformation to NED 3. Velocity update (with gravity and Coriolis) 4. Position update

The algorithm follows Groves (2013), Chapter 5.

pytcl.navigation.initialize_ins_state(lat, lon, alt, vN=0.0, vE=0.0, vD=0.0, roll=0.0, pitch=0.0, yaw=0.0, time=0.0)

Initialize INS state from position, velocity, and attitude.

Parameters:

• lat (float) – Geodetic latitude in radians.

• lon (float) – Geodetic longitude in radians.

• alt (float) – Altitude above ellipsoid in meters.

• vN (float, optional) – North velocity in m/s (default: 0).

• vE (float, optional) – East velocity in m/s (default: 0).

• vD (float, optional) – Down velocity in m/s (default: 0).

• roll (float, optional) – Roll angle in radians (default: 0).

• pitch (float, optional) – Pitch angle in radians (default: 0).

• yaw (float, optional) – Yaw/heading angle in radians (default: 0).

• time (float, optional) – Initial time in seconds (default: 0).

Returns:

state – Initialized INS state.

Return type:

INSState

pytcl.navigation.coarse_alignment(accel, lat)

Perform coarse leveling alignment from accelerometer readings.

Parameters:

• accel (array_like) – Averaged accelerometer readings [ax, ay, az] in body frame (m/s^2). Vehicle should be stationary. For a level vehicle, accel = [0, 0, -g].

• lat (float) – Approximate latitude in radians.

Returns:

• roll (float) – Estimated roll angle in radians.

• pitch (float) – Estimated pitch angle in radians.

Return type:

Tuple[float, float]

Notes

This assumes the vehicle is stationary so the accelerometer measures only the reaction to gravity (specific force = -g when level). Does not estimate heading (yaw).

For a level vehicle pointing north: - Accelerometer reads [0, 0, -g] (z-axis up measures negative gravity) - Roll = 0, Pitch = 0

Sign convention: positive roll is right wing down, positive pitch is nose up.

pytcl.navigation.gyrocompass_alignment(gyro, roll, pitch, lat)

Perform gyrocompass alignment to estimate heading.

Parameters:

• gyro (array_like) – Averaged gyroscope readings [wx, wy, wz] in body frame (rad/s). Vehicle should be stationary.

• roll (float) – Known roll angle in radians.

• pitch (float) – Known pitch angle in radians.

• lat (float) – Latitude in radians.

Returns:

yaw – Estimated heading (yaw) angle in radians.

Return type:

float

Notes

Uses the horizontal component of Earth’s rotation rate to determine heading. Requires accurate roll and pitch (from coarse_alignment).

pytcl.navigation.ins_error_state_matrix(state, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute the INS error state transition matrix (continuous-time F matrix).

Parameters:

• state (INSState) – Current INS state.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

F – 15x15 error state transition matrix.

Return type:

ndarray

Notes

The 15-state error model includes: - Position errors (3): delta_lat, delta_lon, delta_alt - Velocity errors (3): delta_vN, delta_vE, delta_vD - Attitude errors (3): phi_N, phi_E, phi_D - Accelerometer biases (3): bax, bay, baz - Gyroscope biases (3): bwx, bwy, bwz

pytcl.navigation.ins_process_noise_matrix(accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std, state)

Compute the INS process noise covariance matrix (continuous-time Q matrix).

Parameters:

• accel_noise_std (float) – Accelerometer white noise standard deviation (m/s^2).

• gyro_noise_std (float) – Gyroscope white noise standard deviation (rad/s).

• accel_bias_std (float) – Accelerometer bias random walk standard deviation (m/s^2/sqrt(s)).

• gyro_bias_std (float) – Gyroscope bias random walk standard deviation (rad/s/sqrt(s)).

• state (INSState) – Current INS state (for DCM).

Returns:

Q – 15x15 process noise covariance matrix.

Return type:

ndarray

class pytcl.navigation.GNSSMeasurement(position, velocity, position_cov, velocity_cov, time, valid=True)

Bases: NamedTuple

GNSS measurement data.

position

GNSS position [lat, lon, alt] in (rad, rad, m).

Type:

ndarray, optional

velocity

GNSS velocity in NED frame [vN, vE, vD] (m/s).

Type:

ndarray, optional

position_cov

Position covariance (3x3) in geodetic frame.

Type:

ndarray, optional

velocity_cov

Velocity covariance (3x3) in NED frame.

Type:

ndarray, optional

time

GPS time of measurement (seconds).

Type:

float

valid

Whether the measurement is valid.

Type:

bool

position: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 0

velocity: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 1

position_cov: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 2

velocity_cov: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 3

time: float

Alias for field number 4

valid: bool

Alias for field number 5

class pytcl.navigation.SatelliteInfo(prn, position, velocity, pseudorange, doppler=None, cn0=None, elevation=None, azimuth=None)

Bases: NamedTuple

Satellite information for tightly-coupled integration.

prn

Satellite PRN number.

Type:

int

position

Satellite ECEF position [x, y, z] (m).

Type:

ndarray

velocity

Satellite ECEF velocity [vx, vy, vz] (m/s).

Type:

ndarray

pseudorange

Measured pseudorange (m).

Type:

float

doppler

Measured Doppler shift (Hz).

Type:

float, optional

cn0

Carrier-to-noise ratio (dB-Hz).

Type:

float, optional

elevation

Satellite elevation angle (rad).

Type:

float, optional

azimuth

Satellite azimuth angle (rad).

Type:

float, optional

prn: int

Alias for field number 0

position: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

velocity: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

pseudorange: float

Alias for field number 3

doppler: float | None

Alias for field number 4

cn0: float | None

Alias for field number 5

elevation: float | None

Alias for field number 6

azimuth: float | None

Alias for field number 7

class pytcl.navigation.INSGNSSState(ins_state, error_state, error_cov, clock_bias=0.0, clock_drift=0.0)

Bases: NamedTuple

Combined INS/GNSS navigation state.

ins_state

Current INS navigation state.

Type:

INSState

error_state

Error state vector (15 or 17 elements).

Type:

ndarray

error_cov

Error state covariance matrix.

Type:

ndarray

clock_bias

Receiver clock bias (m).

Type:

float

clock_drift

Receiver clock drift (m/s).

Type:

float

ins_state: INSState

Alias for field number 0

error_state: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

error_cov: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

clock_bias: float

Alias for field number 3

clock_drift: float

Alias for field number 4

class pytcl.navigation.LooseCoupledResult(state, innovation, innovation_cov)

Bases: NamedTuple

Result from loosely-coupled INS/GNSS update.

state

Updated navigation state.

Type:

INSGNSSState

innovation

Measurement innovation (residual).

Type:

ndarray

innovation_cov

Innovation covariance.

Type:

ndarray

state: INSGNSSState

Alias for field number 0

innovation: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

innovation_cov: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

class pytcl.navigation.TightCoupledResult(state, innovations, dop)

Bases: NamedTuple

Result from tightly-coupled INS/GNSS update.

state

Updated navigation state.

Type:

INSGNSSState

innovations

Pseudorange/Doppler innovations.

Type:

ndarray

dop

Dilution of precision (GDOP, PDOP, HDOP, VDOP).

Type:

tuple

state: INSGNSSState

Alias for field number 0

innovations: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

dop: Tuple[float, float, float, float]

Alias for field number 2

pytcl.navigation.position_measurement_matrix()

Create measurement matrix for position-only GNSS update.

Returns:

H – 3x15 measurement matrix mapping error state to position measurement.

Return type:

ndarray

pytcl.navigation.velocity_measurement_matrix()

Create measurement matrix for velocity-only GNSS update.

Returns:

H – 3x15 measurement matrix mapping error state to velocity measurement.

Return type:

ndarray

pytcl.navigation.position_velocity_measurement_matrix()

Create measurement matrix for position+velocity GNSS update.

Returns:

H – 6x15 measurement matrix.

Return type:

ndarray

pytcl.navigation.compute_line_of_sight(user_pos, sat_pos)

Compute line-of-sight unit vector and range from user to satellite.

Parameters:

• user_pos (array_like) – User ECEF position [x, y, z] (m).

• sat_pos (array_like) – Satellite ECEF position [x, y, z] (m).

Returns:

• los (ndarray) – Line-of-sight unit vector from user to satellite.

• range (float) – Geometric range (m).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], float]

pytcl.navigation.pseudorange_measurement_matrix(user_pos, satellites, include_clock=True)

Create measurement matrix for pseudorange observations.

Parameters:

• user_pos (array_like) – User ECEF position [x, y, z] (m).

• satellites (list of SatelliteInfo) – List of satellite information.

• include_clock (bool, optional) – Whether to include clock bias column (default: True).

Returns:

H – (n_sats x 4) or (n_sats x 3) geometry matrix. Columns are [dx, dy, dz, clock_bias] or [dx, dy, dz].

Return type:

ndarray

pytcl.navigation.compute_dop(H)

Compute Dilution of Precision from geometry matrix.

Parameters:

H (array_like) – Geometry matrix (n_sats x 4) with columns [dx, dy, dz, clock].

Returns:

• GDOP (float) – Geometric DOP.

• PDOP (float) – Position DOP.

• HDOP (float) – Horizontal DOP.

• VDOP (float) – Vertical DOP.

Return type:

Tuple[float, float, float, float]

pytcl.navigation.satellite_elevation_azimuth(user_lla, sat_ecef, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute satellite elevation and azimuth angles from user position.

Parameters:

• user_lla (array_like) – User position [lat, lon, alt] in (rad, rad, m).

• sat_ecef (array_like) – Satellite ECEF position [x, y, z] (m).

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• elevation (float) – Elevation angle in radians.

• azimuth (float) – Azimuth angle in radians (from north, clockwise).

Return type:

Tuple[float, float]

pytcl.navigation.initialize_ins_gnss(ins_state, position_std=10.0, velocity_std=1.0, attitude_std=0.1, accel_bias_std=0.1, gyro_bias_std=0.01)

Initialize INS/GNSS integration state.

Parameters:

• ins_state (INSState) – Initial INS navigation state.

• position_std (float, optional) – Initial position uncertainty (m). Default: 10.0.

• velocity_std (float, optional) – Initial velocity uncertainty (m/s). Default: 1.0.

• attitude_std (float, optional) – Initial attitude uncertainty (rad). Default: 0.1.

• accel_bias_std (float, optional) – Initial accelerometer bias uncertainty (m/s^2). Default: 0.1.

• gyro_bias_std (float, optional) – Initial gyroscope bias uncertainty (rad/s). Default: 0.01.

Returns:

state – Initialized INS/GNSS state.

Return type:

INSGNSSState

pytcl.navigation.loose_coupled_predict(state, imu, accel_noise_std=0.01, gyro_noise_std=0.0001, accel_bias_std=1e-05, gyro_bias_std=1e-07, accel_prev=None, gyro_prev=None)

Perform prediction step for loosely-coupled INS/GNSS.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• imu (IMUData) – IMU measurements for this time step.

• accel_noise_std (float, optional) – Accelerometer noise std (m/s^2). Default: 0.01.

• gyro_noise_std (float, optional) – Gyroscope noise std (rad/s). Default: 1e-4.

• accel_bias_std (float, optional) – Accelerometer bias random walk std. Default: 1e-5.

• gyro_bias_std (float, optional) – Gyroscope bias random walk std. Default: 1e-7.

• accel_prev (array_like, optional) – Previous accelerometer reading for coning/sculling.

• gyro_prev (array_like, optional) – Previous gyroscope reading for coning/sculling.

Returns:

state – Predicted state.

Return type:

INSGNSSState

pytcl.navigation.loose_coupled_update_position(state, gnss)

Update INS/GNSS state with GNSS position measurement.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS position measurement.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.loose_coupled_update_velocity(state, gnss)

Update INS/GNSS state with GNSS velocity measurement.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS velocity measurement.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.loose_coupled_update(state, gnss)

Update INS/GNSS state with GNSS position and velocity measurements.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS measurement with position and/or velocity.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.tight_coupled_pseudorange_innovation(state, satellites, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute pseudorange innovations for tightly-coupled update.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

• innovations (ndarray) – Pseudorange innovations (measured - predicted).

• predicted (ndarray) – Predicted pseudoranges.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

pytcl.navigation.tight_coupled_measurement_matrix(state, satellites, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute measurement matrix for tightly-coupled pseudorange update.

Maps 17-state error (15 INS + 2 clock) to pseudorange measurements.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

H – (n_sats x 17) measurement matrix.

Return type:

ndarray

pytcl.navigation.tight_coupled_update(state, satellites, pseudorange_std=3.0, ellipsoid=(6378137.0, 0.0033528106647474805))

Perform tightly-coupled INS/GNSS update using pseudoranges.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations with pseudoranges.

• pseudorange_std (float, optional) – Pseudorange measurement noise std (m). Default: 3.0.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

result – Updated state and DOP values.

Return type:

TightCoupledResult

pytcl.navigation.gnss_outage_detection(innovations, innovation_cov, threshold=5.991)

Detect potential GNSS measurement faults using chi-squared test.

Parameters:

• innovations (array_like) – Measurement innovations.

• innovation_cov (array_like) – Innovation covariance matrix.

• threshold (float, optional) – Chi-squared threshold (default: 5.991 for 2 DOF, 95% confidence).

Returns:

fault_detected – True if measurement appears faulty.

Return type:

bool

class pytcl.navigation.GreatCircleResult(distance, azimuth1, azimuth2)

Bases: NamedTuple

Result of great circle computation.

distance

Great circle distance in meters.

Type:

float

azimuth1

Initial azimuth in radians (from north, clockwise).

Type:

float

azimuth2

Final azimuth in radians.

Type:

float

distance: float

Alias for field number 0

azimuth1: float

Alias for field number 1

azimuth2: float

Alias for field number 2

class pytcl.navigation.WaypointResult(lat, lon)

Bases: NamedTuple

Result of waypoint computation.

lat

Latitude of waypoint in radians.

Type:

float

lon

Longitude of waypoint in radians.

Type:

float

lat: float

Alias for field number 0

lon: float

Alias for field number 1

class pytcl.navigation.IntersectionResult(lat1, lon1, lat2, lon2, valid)

Bases: NamedTuple

Result of great circle intersection.

lat1

Latitude of first intersection in radians.

Type:

float

lon1

Longitude of first intersection in radians.

Type:

float

lat2

Latitude of second intersection (antipodal) in radians.

Type:

float

lon2

Longitude of second intersection (antipodal) in radians.

Type:

float

valid

True if intersection exists.

Type:

bool

lat1: float

Alias for field number 0

lon1: float

Alias for field number 1

lat2: float

Alias for field number 2

lon2: float

Alias for field number 3

valid: bool

Alias for field number 4

class pytcl.navigation.CrossTrackResult(cross_track, along_track)

Bases: NamedTuple

Result of cross-track distance computation.

cross_track

Cross-track distance in meters (positive = right of path).

Type:

float

along_track

Along-track distance from start in meters.

Type:

float

cross_track: float

Alias for field number 0

along_track: float

Alias for field number 1

pytcl.navigation.great_circle_distance(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute great circle distance between two points.

Uses the haversine formula for numerical stability at small distances. Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Great circle distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> # New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> dist = great_circle_distance(lat1, lon1, lat2, lon2)
>>> print(f"Distance: {dist/1000:.0f} km")

pytcl.navigation.great_circle_azimuth(lat1, lon1, lat2, lon2)

Compute initial azimuth (bearing) from point 1 to point 2.

Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Initial azimuth in radians (from north, clockwise, 0 to 2π).

Return type:

float

Examples

>>> import numpy as np
>>> # Bearing from New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> az = great_circle_azimuth(lat1, lon1, lat2, lon2)
>>> print(f"Initial bearing: {np.degrees(az):.1f}°")

pytcl.navigation.great_circle_inverse(lat1, lon1, lat2, lon2, radius=6371000.0)

Solve the inverse great circle problem.

Given two points, compute distance and azimuths.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Distance and azimuths.

Return type:

GreatCircleResult

Examples

>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)  # NYC
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)   # London
>>> result = great_circle_inverse(lat1, lon1, lat2, lon2)
>>> result.distance > 5000000  # Over 5000 km
True

pytcl.navigation.great_circle_waypoint(lat1, lon1, lat2, lon2, fraction)

Compute intermediate waypoint along a great circle.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• fraction (float) – Fraction of distance (0 = start, 1 = end).

Returns:

Latitude and longitude of waypoint.

Return type:

WaypointResult

Examples

>>> import numpy as np
>>> # Midpoint between New York and London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> mid = great_circle_waypoint(lat1, lon1, lat2, lon2, 0.5)
>>> print(f"Midpoint: {np.degrees(mid.lat):.2f}°, {np.degrees(mid.lon):.2f}°")

pytcl.navigation.great_circle_waypoints(lat1, lon1, lat2, lon2, n_points)

Compute multiple waypoints along a great circle.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• n_points (int) – Number of waypoints (including start and end).

Returns:

lats, lons – Arrays of waypoint latitudes and longitudes in radians.

Return type:

ndarray

Examples

>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.pi/4, np.pi/4
>>> lats, lons = great_circle_waypoints(lat1, lon1, lat2, lon2, 5)
>>> len(lats)
5

pytcl.navigation.great_circle_direct(lat1, lon1, azimuth, distance, radius=6371000.0)

Solve the direct great circle problem.

Given starting point, azimuth, and distance, find destination.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• azimuth (float) – Initial azimuth in radians (from north, clockwise).

• distance (float) – Distance in meters.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Destination latitude and longitude.

Return type:

WaypointResult

Examples

>>> import numpy as np
>>> # 1000 km northeast from origin
>>> lat, lon = 0.0, 0.0
>>> az = np.radians(45)  # Northeast
>>> dest = great_circle_direct(lat, lon, az, 1000000)
>>> print(f"Destination: {np.degrees(dest.lat):.2f}°, {np.degrees(dest.lon):.2f}°")

pytcl.navigation.cross_track_distance(lat_point, lon_point, lat1, lon1, lat2, lon2, radius=6371000.0)

Compute cross-track and along-track distances.

The cross-track distance is the perpendicular distance from a point to the great circle path between two other points.

Parameters:

• lat_point (float) – Point coordinates in radians.

• lon_point (float) – Point coordinates in radians.

• lat1 (float) – Path start coordinates in radians.

• lon1 (float) – Path start coordinates in radians.

• lat2 (float) – Path end coordinates in radians.

• lon2 (float) – Path end coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Cross-track distance (positive = right of path) and along-track distance.

Return type:

CrossTrackResult

Notes

Positive cross-track means the point is to the right of the path (when traveling from start to end).

Examples

>>> # Point near a path from origin to northeast
>>> lat_pt, lon_pt = np.radians(5), np.radians(2)
>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.radians(10), np.radians(10)
>>> result = cross_track_distance(lat_pt, lon_pt, lat1, lon1, lat2, lon2)
>>> abs(result.cross_track) < 500000  # Within 500 km
True

pytcl.navigation.great_circle_intersect(lat1, lon1, azimuth1, lat2, lon2, azimuth2)

Find intersection of two great circles.

Given two points with initial bearings, find where the great circles defined by those bearings intersect.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• azimuth1 (float) – Bearing from first point in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• azimuth2 (float) – Bearing from second point in radians.

Returns:

Two intersection points (antipodal) and validity flag.

Return type:

IntersectionResult

Notes

Great circles always intersect at two antipodal points (unless they are identical or parallel). The returned points are the intersections closest to the given points.

Examples

>>> lat1, lon1 = 0.0, 0.0
>>> az1 = np.radians(45)  # Northeast
>>> lat2, lon2 = 0.0, np.radians(10)
>>> az2 = np.radians(315)  # Northwest
>>> result = great_circle_intersect(lat1, lon1, az1, lat2, lon2, az2)
>>> result.valid
True

pytcl.navigation.great_circle_path_intersect(lat1a, lon1a, lat2a, lon2a, lat1b, lon1b, lat2b, lon2b)

Find intersection of two great circle paths (defined by endpoints).

Parameters:

• lat1a (float) – Start of first path in radians.

• lon1a (float) – Start of first path in radians.

• lat2a (float) – End of first path in radians.

• lon2a (float) – End of first path in radians.

• lat1b (float) – Start of second path in radians.

• lon1b (float) – Start of second path in radians.

• lat2b (float) – End of second path in radians.

• lon2b (float) – End of second path in radians.

Returns:

Intersection points and validity.

Return type:

IntersectionResult

Examples

>>> # Two crossing paths
>>> result = great_circle_path_intersect(
...     0.0, 0.0, np.radians(10), np.radians(10),  # Path A
...     0.0, np.radians(10), np.radians(10), 0.0    # Path B
... )
>>> result.valid
True

pytcl.navigation.great_circle_tdoa_loc(lat1, lon1, lat2, lon2, lat3, lon3, tdoa12, tdoa13, speed=299792458.0, radius=6371000.0)

TDOA localization on a sphere.

Given three receivers and two time-difference-of-arrival measurements, estimate the emitter location.

Parameters:

• lat1 (float) – First receiver coordinates in radians.

• lon1 (float) – First receiver coordinates in radians.

• lat2 (float) – Second receiver coordinates in radians.

• lon2 (float) – Second receiver coordinates in radians.

• lat3 (float) – Third receiver coordinates in radians.

• lon3 (float) – Third receiver coordinates in radians.

• tdoa12 (float) – Time difference of arrival between receivers 1 and 2 (seconds). Positive means signal arrived at receiver 1 first.

• tdoa13 (float) – Time difference of arrival between receivers 1 and 3 (seconds).

• speed (float, optional) – Signal propagation speed in m/s (default: speed of light).

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

loc1, loc2 – Two possible emitter locations, or None if no solution.

Return type:

Optional[WaypointResult]

Notes

TDOA localization finds the intersection of two hyperbolas. On a sphere, these are represented by the locus of points where the difference in distance to two receivers is constant.

This implementation uses a grid search followed by Newton-Raphson refinement for robustness.

pytcl.navigation.angular_distance(lat1, lon1, lat2, lon2)

Compute angular distance (central angle) between two points.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

Returns:

Angular distance in radians.

Return type:

float

Examples

Compute angular distance between New York and London:

>>> import numpy as np
>>> # NYC: 40.7°N, 74.0°W; London: 51.5°N, 0.1°W
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
>>> lat2, lon2 = np.radians(51.5), np.radians(-0.1)
>>> angle = angular_distance(lat1, lon1, lat2, lon2)
>>> np.degrees(angle)  # about 50 degrees
49.9...

See also

great_circle_distance

Compute distance on sphere with given radius.

pytcl.navigation.destination_point(lat, lon, bearing, angular_distance)

Compute destination point given start, bearing, and angular distance.

Parameters:

• lat (float) – Starting point coordinates in radians.

• lon (float) – Starting point coordinates in radians.

• bearing (float) – Initial bearing in radians.

• angular_distance (float) – Angular distance in radians (distance/radius).

Returns:

Destination coordinates.

Return type:

WaypointResult

Examples

>>> lat, lon = 0.0, 0.0
>>> bearing = np.radians(90)  # Due East
>>> ang_dist = np.radians(10)  # 10 degrees
>>> dest = destination_point(lat, lon, bearing, ang_dist)
>>> np.degrees(dest.lon)  # Should be ~10 degrees East
10.0

class pytcl.navigation.RhumbResult(distance, bearing)

Bases: NamedTuple

Result of rhumb line computation.

distance

Rhumb line distance in meters.

Type:

float

bearing

Constant bearing in radians (from north, clockwise).

Type:

float

distance: float

Alias for field number 0

bearing: float

Alias for field number 1

class pytcl.navigation.RhumbDirectResult(lat, lon)

Bases: NamedTuple

Result of direct rhumb problem.

lat

Destination latitude in radians.

Type:

float

lon

Destination longitude in radians.

Type:

float

lat: float

Alias for field number 0

lon: float

Alias for field number 1

class pytcl.navigation.RhumbIntersectionResult(lat, lon, valid)

Bases: NamedTuple

Result of rhumb line intersection.

lat

Intersection latitude in radians.

Type:

float

lon

Intersection longitude in radians.

Type:

float

valid

True if intersection exists.

Type:

bool

lat: float

Alias for field number 0

lon: float

Alias for field number 1

valid: bool

Alias for field number 2

pytcl.navigation.rhumb_distance_spherical(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute rhumb line distance on a sphere.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Rhumb line distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> dist = rhumb_distance_spherical(lat1, lon1, lat2, lon2)

pytcl.navigation.rhumb_bearing(lat1, lon1, lat2, lon2)

Compute rhumb line bearing from point 1 to point 2.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Constant bearing in radians (from north, clockwise, 0 to 2π).

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> bearing = rhumb_bearing(lat1, lon1, lat2, lon2)
>>> print(f"Bearing: {np.degrees(bearing):.1f}°")

pytcl.navigation.indirect_rhumb_spherical(lat1, lon1, lat2, lon2, radius=6371000.0)

Solve the indirect rhumb problem on a sphere.

Given two points, find the rhumb line distance and bearing.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Distance and constant bearing.

Return type:

RhumbResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # New York
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> result = indirect_rhumb_spherical(lat1, lon1, lat2, lon2)
>>> result.distance > 5000000  # Over 5000 km
True

pytcl.navigation.direct_rhumb_spherical(lat1, lon1, bearing, distance, radius=6371000.0)

Solve the direct rhumb problem on a sphere.

Given starting point, bearing, and distance, find destination.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• bearing (float) – Constant bearing in radians (from north, clockwise).

• distance (float) – Distance in meters.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Destination latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat, lon = np.radians(40), np.radians(-74)
>>> bearing = np.radians(90)  # Due east
>>> dest = direct_rhumb_spherical(lat, lon, bearing, 1000000)

pytcl.navigation.rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute rhumb line distance on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Rhumb line distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> dist = rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2)
>>> dist > 5000000  # Over 5000 km
True

pytcl.navigation.indirect_rhumb(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the indirect rhumb problem on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Distance and constant bearing.

Return type:

RhumbResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # New York
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> result = indirect_rhumb(lat1, lon1, lat2, lon2)
>>> 0 < result.bearing < np.pi  # Eastward bearing
True

pytcl.navigation.direct_rhumb(lat1, lon1, bearing, distance, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the direct rhumb problem on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• bearing (float) – Constant bearing in radians (from north, clockwise).

• distance (float) – Distance in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Destination latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat, lon = np.radians(40), np.radians(-74)
>>> bearing = np.radians(90)  # Due east
>>> dest = direct_rhumb(lat, lon, bearing, 100000)  # 100 km
>>> np.degrees(dest.lon) > -74  # Moved east
True

pytcl.navigation.rhumb_intersect(lat1, lon1, bearing1, lat2, lon2, bearing2)

Find intersection of two rhumb lines.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• bearing1 (float) – Bearing from first point in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• bearing2 (float) – Bearing from second point in radians.

Returns:

Intersection point and validity flag.

Return type:

RhumbIntersectionResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> bearing1 = np.radians(45)
>>> bearing2 = np.radians(270)
>>> result = rhumb_intersect(lat1, lon1, bearing1, lat2, lon2, bearing2)
>>> result.valid  # May or may not intersect
True

Notes

Unlike great circles, two rhumb lines may not intersect (if bearings are parallel or if the lines diverge before meeting).

pytcl.navigation.rhumb_midpoint(lat1, lon1, lat2, lon2)

Compute midpoint along a rhumb line.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Midpoint latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(0), np.radians(0)
>>> lat2, lon2 = np.radians(10), np.radians(10)
>>> mid = rhumb_midpoint(lat1, lon1, lat2, lon2)
>>> np.isclose(np.degrees(mid.lat), 5, atol=0.1)
True

pytcl.navigation.rhumb_waypoints(lat1, lon1, lat2, lon2, n_points, radius=6371000.0)

Compute multiple waypoints along a rhumb line.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• n_points (int) – Number of waypoints (including start and end).

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

lats, lons – Arrays of waypoint latitudes and longitudes in radians.

Return type:

ndarray

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> lats, lons = rhumb_waypoints(lat1, lon1, lat2, lon2, 5)
>>> len(lats)
5

pytcl.navigation.compare_great_circle_rhumb(lat1, lon1, lat2, lon2, radius=6371000.0)

Compare great circle and rhumb line paths.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

• gc_distance (float) – Great circle distance in meters.

• rhumb_distance (float) – Rhumb line distance in meters.

• difference_percent (float) – Percentage difference (rhumb is longer).

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # NYC
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> gc, rhumb, diff = compare_great_circle_rhumb(lat1, lon1, lat2, lon2)
>>> rhumb > gc  # Rhumb is always longer
True

INS Mechanization

Strapdown inertial navigation system mechanization and error modeling.

Inertial Navigation System (INS) mechanization.

This module provides strapdown INS mechanization equations for navigation, including: - INS state representation (position, velocity, attitude) - Strapdown mechanization equations (NED and ECEF frames) - Coning and sculling corrections for IMU data - Gravity and transport rate computations - Attitude integration using quaternions

References

[1]

D. Titterton and J. Weston, “Strapdown Inertial Navigation Technology”, 2nd ed., IEE, 2004.

[2]

P. Groves, “Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems”, 2nd ed., Artech House, 2013.

[3]

10. Farrell, “Aided Navigation: GPS with High Rate Sensors”, McGraw-Hill, 2008.

class pytcl.navigation.ins.INSState(position, velocity, quaternion, time)

Bases: NamedTuple

Inertial Navigation System state.

position

Position in geodetic coordinates [latitude (rad), longitude (rad), altitude (m)].

Type:

ndarray

velocity

Velocity in NED frame [vN, vE, vD] (m/s).

Type:

ndarray

quaternion

Attitude quaternion [qw, qx, qy, qz] from body to NED frame (scalar-first).

Type:

ndarray

time

Time associated with this state (seconds).

Type:

float

position: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

velocity: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

quaternion: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

time: float

Alias for field number 3

property latitude: float

Geodetic latitude in radians.

property longitude: float

Geodetic longitude in radians.

property altitude: float

Altitude above ellipsoid in meters.

property velocity_north: float

North velocity in m/s.

property velocity_east: float

East velocity in m/s.

property velocity_down: float

Down velocity in m/s.

property dcm: ndarray[tuple[Any, ...], dtype[floating]]

Direction cosine matrix from body to NED frame.

euler_angles()

Get Euler angles (roll, pitch, yaw) in radians.

Returns:

euler – [roll, pitch, yaw] in radians, ZYX convention.

Return type:

ndarray

class pytcl.navigation.ins.IMUData(accel, gyro, dt)

Bases: NamedTuple

IMU measurement data.

accel

Specific force measurements [ax, ay, az] in body frame (m/s^2).

Type:

ndarray

gyro

Angular rate measurements [wx, wy, wz] in body frame (rad/s).

Type:

ndarray

dt

Time interval between measurements (seconds).

Type:

float

accel: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

gyro: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

dt: float

Alias for field number 2

class pytcl.navigation.ins.INSErrorState(position_error, velocity_error, attitude_error, accel_bias, gyro_bias)

Bases: NamedTuple

INS error state for Kalman filtering.

position_error

Position error [delta_lat, delta_lon, delta_alt] (rad, rad, m).

Type:

ndarray

velocity_error

Velocity error in NED [delta_vN, delta_vE, delta_vD] (m/s).

Type:

ndarray

attitude_error

Attitude error angles [phi_N, phi_E, phi_D] (rad).

Type:

ndarray

accel_bias

Accelerometer bias [bax, bay, baz] (m/s^2).

Type:

ndarray

gyro_bias

Gyroscope bias [bwx, bwy, bwz] (rad/s).

Type:

ndarray

position_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 0

velocity_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

attitude_error: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

accel_bias: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 3

gyro_bias: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 4

static zeros()

Create zero error state.

to_vector()

Convert to 15-element state vector.

static from_vector(x)

Create from 15-element state vector.

pytcl.navigation.ins.normal_gravity(lat, alt=0.0)

Compute normal gravity using Somigliana’s formula with free-air correction.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float, optional) – Altitude above ellipsoid in meters (default: 0).

Returns:

g – Normal gravity magnitude in m/s^2.

Return type:

float

Notes

Uses the WGS84 gravity formula with first-order altitude correction.

pytcl.navigation.ins.gravity_ned(lat, alt=0.0)

Compute gravity vector in NED frame.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float, optional) – Altitude above ellipsoid in meters.

Returns:

g_ned – Gravity vector [gN, gE, gD] in m/s^2.

Return type:

ndarray

pytcl.navigation.ins.earth_rate_ned(lat)

Compute Earth rotation rate in NED frame.

Parameters:

lat (float) – Geodetic latitude in radians.

Returns:

omega_ie_n – Earth rotation rate in NED frame [wN, wE, wD] (rad/s).

Return type:

ndarray

pytcl.navigation.ins.transport_rate_ned(lat, alt, vN, vE, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute transport rate (navigation frame rotation) in NED frame.

Parameters:

• lat (float) – Geodetic latitude in radians.

• alt (float) – Altitude above ellipsoid in meters.

• vN (float) – North velocity in m/s.

• vE (float) – East velocity in m/s.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

omega_en_n – Transport rate in NED frame [wN, wE, wD] (rad/s).

Return type:

ndarray

Notes

The transport rate accounts for navigation frame rotation as the vehicle moves over the curved Earth surface.

pytcl.navigation.ins.radii_of_curvature(lat, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute principal radii of curvature.

Parameters:

• lat (float) – Geodetic latitude in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• RN (float) – Meridian radius of curvature (m).

• RE (float) – Transverse radius of curvature (prime vertical) (m).

Return type:

Tuple[float, float]

pytcl.navigation.ins.coning_correction(gyro_prev, gyro_curr)

Compute first-order coning correction for angular increment.

Parameters:

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

Returns:

delta_theta_coning – Coning correction to angular increment (rad).

Return type:

ndarray

Notes

Coning error occurs when the rotation axis itself rotates (coning motion). This first-order correction assumes trapezoidal integration of gyro data.

References

[1]

Savage, P.G., “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms”, AIAA Journal of Guidance, 1998.

pytcl.navigation.ins.sculling_correction(accel_prev, accel_curr, gyro_prev, gyro_curr)

Compute first-order sculling correction for velocity increment.

Parameters:

• accel_prev (array_like) – Previous specific force [ax, ay, az] (m/s^2).

• accel_curr (array_like) – Current specific force [ax, ay, az] (m/s^2).

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

Returns:

delta_v_sculling – Sculling correction to velocity increment (m/s).

Return type:

ndarray

Notes

Sculling error occurs when linear vibration and angular vibration are correlated (e.g., on a flexible structure).

References

[1]

Savage, P.G., “Strapdown Inertial Navigation Integration Algorithm Design Part 2: Velocity and Position Algorithms”, AIAA, 1998.

pytcl.navigation.ins.compensate_imu_data(accel_prev, accel_curr, gyro_prev, gyro_curr, dt)

Compute compensated angular and velocity increments with coning/sculling corrections.

Parameters:

• accel_prev (array_like) – Previous specific force [ax, ay, az] (m/s^2).

• accel_curr (array_like) – Current specific force [ax, ay, az] (m/s^2).

• gyro_prev (array_like) – Previous angular rate [wx, wy, wz] (rad/s).

• gyro_curr (array_like) – Current angular rate [wx, wy, wz] (rad/s).

• dt (float) – Time interval (seconds).

Returns:

• delta_theta (ndarray) – Compensated angular increment (rad).

• delta_v (ndarray) – Compensated velocity increment (m/s).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

pytcl.navigation.ins.skew_symmetric(v)

Create skew-symmetric matrix from vector.

Parameters:

v (array_like) – 3-element vector.

Returns:

S – 3x3 skew-symmetric matrix.

Return type:

ndarray

pytcl.navigation.ins.update_quaternion(q, delta_theta)

Update quaternion using angular increment.

Parameters:

• q (array_like) – Current quaternion [qw, qx, qy, qz].

• delta_theta (array_like) – Angular increment in body frame (rad).

Returns:

q_new – Updated quaternion (normalized).

Return type:

ndarray

Notes

Uses first-order quaternion update: q_new = q * delta_q where delta_q represents the incremental rotation.

pytcl.navigation.ins.update_attitude_ned(q_b_n, omega_ib_b, omega_in_n, dt)

Update attitude quaternion (body to NED) for one time step.

Parameters:

• q_b_n (array_like) – Current quaternion from body to NED frame.

• omega_ib_b (array_like) – Angular rate of body w.r.t. inertial, in body frame (rad/s).

• omega_in_n (array_like) – Angular rate of NED w.r.t. inertial, in NED frame (rad/s).

• dt (float) – Time step (seconds).

Returns:

q_new – Updated quaternion (body to NED).

Return type:

ndarray

Notes

The attitude update accounts for both body rotation and navigation frame rotation. omega_in_n = omega_ie_n + omega_en_n (Earth rate + transport rate)

pytcl.navigation.ins.mechanize_ins_ned(state, imu, ellipsoid=(6378137.0, 0.0033528106647474805), accel_prev=None, gyro_prev=None)

Perform one step of strapdown INS mechanization in NED frame.

Parameters:

• state (INSState) – Current INS state.

• imu (IMUData) – IMU measurements for this time step.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

• accel_prev (array_like, optional) – Previous accelerometer reading for coning/sculling corrections.

• gyro_prev (array_like, optional) – Previous gyroscope reading for coning/sculling corrections.

Returns:

new_state – Updated INS state after mechanization.

Return type:

INSState

Notes

This implements the standard NED-frame strapdown mechanization: 1. Attitude update (quaternion integration) 2. Specific force transformation to NED 3. Velocity update (with gravity and Coriolis) 4. Position update

The algorithm follows Groves (2013), Chapter 5.

pytcl.navigation.ins.initialize_ins_state(lat, lon, alt, vN=0.0, vE=0.0, vD=0.0, roll=0.0, pitch=0.0, yaw=0.0, time=0.0)

Initialize INS state from position, velocity, and attitude.

Parameters:

• lat (float) – Geodetic latitude in radians.

• lon (float) – Geodetic longitude in radians.

• alt (float) – Altitude above ellipsoid in meters.

• vN (float, optional) – North velocity in m/s (default: 0).

• vE (float, optional) – East velocity in m/s (default: 0).

• vD (float, optional) – Down velocity in m/s (default: 0).

• roll (float, optional) – Roll angle in radians (default: 0).

• pitch (float, optional) – Pitch angle in radians (default: 0).

• yaw (float, optional) – Yaw/heading angle in radians (default: 0).

• time (float, optional) – Initial time in seconds (default: 0).

Returns:

state – Initialized INS state.

Return type:

INSState

pytcl.navigation.ins.coarse_alignment(accel, lat)

Perform coarse leveling alignment from accelerometer readings.

Parameters:

• accel (array_like) – Averaged accelerometer readings [ax, ay, az] in body frame (m/s^2). Vehicle should be stationary. For a level vehicle, accel = [0, 0, -g].

• lat (float) – Approximate latitude in radians.

Returns:

• roll (float) – Estimated roll angle in radians.

• pitch (float) – Estimated pitch angle in radians.

Return type:

Tuple[float, float]

Notes

This assumes the vehicle is stationary so the accelerometer measures only the reaction to gravity (specific force = -g when level). Does not estimate heading (yaw).

For a level vehicle pointing north: - Accelerometer reads [0, 0, -g] (z-axis up measures negative gravity) - Roll = 0, Pitch = 0

Sign convention: positive roll is right wing down, positive pitch is nose up.

pytcl.navigation.ins.gyrocompass_alignment(gyro, roll, pitch, lat)

Perform gyrocompass alignment to estimate heading.

Parameters:

• gyro (array_like) – Averaged gyroscope readings [wx, wy, wz] in body frame (rad/s). Vehicle should be stationary.

• roll (float) – Known roll angle in radians.

• pitch (float) – Known pitch angle in radians.

• lat (float) – Latitude in radians.

Returns:

yaw – Estimated heading (yaw) angle in radians.

Return type:

float

Notes

Uses the horizontal component of Earth’s rotation rate to determine heading. Requires accurate roll and pitch (from coarse_alignment).

pytcl.navigation.ins.ins_error_state_matrix(state, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute the INS error state transition matrix (continuous-time F matrix).

Parameters:

• state (INSState) – Current INS state.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

F – 15x15 error state transition matrix.

Return type:

ndarray

Notes

The 15-state error model includes: - Position errors (3): delta_lat, delta_lon, delta_alt - Velocity errors (3): delta_vN, delta_vE, delta_vD - Attitude errors (3): phi_N, phi_E, phi_D - Accelerometer biases (3): bax, bay, baz - Gyroscope biases (3): bwx, bwy, bwz

pytcl.navigation.ins.ins_process_noise_matrix(accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std, state)

Compute the INS process noise covariance matrix (continuous-time Q matrix).

Parameters:

• accel_noise_std (float) – Accelerometer white noise standard deviation (m/s^2).

• gyro_noise_std (float) – Gyroscope white noise standard deviation (rad/s).

• accel_bias_std (float) – Accelerometer bias random walk standard deviation (m/s^2/sqrt(s)).

• gyro_bias_std (float) – Gyroscope bias random walk standard deviation (rad/s/sqrt(s)).

• state (INSState) – Current INS state (for DCM).

Returns:

Q – 15x15 process noise covariance matrix.

Return type:

ndarray

INS/GNSS Integration

Loosely-coupled and tightly-coupled INS/GNSS integration algorithms.

INS/GNSS Integration algorithms.

This module provides integrated navigation solutions combining Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS): - GNSS measurement models and state representation - Loosely-coupled INS/GNSS integration (position/velocity aiding) - Tightly-coupled INS/GNSS integration (pseudorange/Doppler aiding) - Error state Kalman filter for INS/GNSS fusion

References

[1]

P. Groves, “Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems”, 2nd ed., Artech House, 2013.

[2]

10. Farrell, “Aided Navigation: GPS with High Rate Sensors”, McGraw-Hill, 2008.

[3]

R. Brown and P. Hwang, “Introduction to Random Signals and Applied Kalman Filtering”, 4th ed., Wiley, 2012.

class pytcl.navigation.ins_gnss.GNSSMeasurement(position, velocity, position_cov, velocity_cov, time, valid=True)

Bases: NamedTuple

GNSS measurement data.

position

GNSS position [lat, lon, alt] in (rad, rad, m).

Type:

ndarray, optional

velocity

GNSS velocity in NED frame [vN, vE, vD] (m/s).

Type:

ndarray, optional

position_cov

Position covariance (3x3) in geodetic frame.

Type:

ndarray, optional

velocity_cov

Velocity covariance (3x3) in NED frame.

Type:

ndarray, optional

time

GPS time of measurement (seconds).

Type:

float

valid

Whether the measurement is valid.

Type:

bool

position: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 0

velocity: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 1

position_cov: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 2

velocity_cov: ndarray[tuple[Any, ...], dtype[floating]] | None

Alias for field number 3

time: float

Alias for field number 4

valid: bool

Alias for field number 5

class pytcl.navigation.ins_gnss.SatelliteInfo(prn, position, velocity, pseudorange, doppler=None, cn0=None, elevation=None, azimuth=None)

Bases: NamedTuple

Satellite information for tightly-coupled integration.

prn

Satellite PRN number.

Type:

int

position

Satellite ECEF position [x, y, z] (m).

Type:

ndarray

velocity

Satellite ECEF velocity [vx, vy, vz] (m/s).

Type:

ndarray

pseudorange

Measured pseudorange (m).

Type:

float

doppler

Measured Doppler shift (Hz).

Type:

float, optional

cn0

Carrier-to-noise ratio (dB-Hz).

Type:

float, optional

elevation

Satellite elevation angle (rad).

Type:

float, optional

azimuth

Satellite azimuth angle (rad).

Type:

float, optional

prn: int

Alias for field number 0

position: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

velocity: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

pseudorange: float

Alias for field number 3

doppler: float | None

Alias for field number 4

cn0: float | None

Alias for field number 5

elevation: float | None

Alias for field number 6

azimuth: float | None

Alias for field number 7

class pytcl.navigation.ins_gnss.INSGNSSState(ins_state, error_state, error_cov, clock_bias=0.0, clock_drift=0.0)

Bases: NamedTuple

Combined INS/GNSS navigation state.

ins_state

Current INS navigation state.

Type:

INSState

error_state

Error state vector (15 or 17 elements).

Type:

ndarray

error_cov

Error state covariance matrix.

Type:

ndarray

clock_bias

Receiver clock bias (m).

Type:

float

clock_drift

Receiver clock drift (m/s).

Type:

float

ins_state: INSState

Alias for field number 0

error_state: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

error_cov: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

clock_bias: float

Alias for field number 3

clock_drift: float

Alias for field number 4

class pytcl.navigation.ins_gnss.LooseCoupledResult(state, innovation, innovation_cov)

Bases: NamedTuple

Result from loosely-coupled INS/GNSS update.

state

Updated navigation state.

Type:

INSGNSSState

innovation

Measurement innovation (residual).

Type:

ndarray

innovation_cov

Innovation covariance.

Type:

ndarray

state: INSGNSSState

Alias for field number 0

innovation: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

innovation_cov: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 2

class pytcl.navigation.ins_gnss.TightCoupledResult(state, innovations, dop)

Bases: NamedTuple

Result from tightly-coupled INS/GNSS update.

state

Updated navigation state.

Type:

INSGNSSState

innovations

Pseudorange/Doppler innovations.

Type:

ndarray

dop

Dilution of precision (GDOP, PDOP, HDOP, VDOP).

Type:

tuple

state: INSGNSSState

Alias for field number 0

innovations: ndarray[tuple[Any, ...], dtype[floating]]

Alias for field number 1

dop: Tuple[float, float, float, float]

Alias for field number 2

pytcl.navigation.ins_gnss.position_measurement_matrix()

Create measurement matrix for position-only GNSS update.

Returns:

H – 3x15 measurement matrix mapping error state to position measurement.

Return type:

ndarray

pytcl.navigation.ins_gnss.velocity_measurement_matrix()

Create measurement matrix for velocity-only GNSS update.

Returns:

H – 3x15 measurement matrix mapping error state to velocity measurement.

Return type:

ndarray

pytcl.navigation.ins_gnss.position_velocity_measurement_matrix()

Create measurement matrix for position+velocity GNSS update.

Returns:

H – 6x15 measurement matrix.

Return type:

ndarray

pytcl.navigation.ins_gnss.compute_line_of_sight(user_pos, sat_pos)

Compute line-of-sight unit vector and range from user to satellite.

Parameters:

• user_pos (array_like) – User ECEF position [x, y, z] (m).

• sat_pos (array_like) – Satellite ECEF position [x, y, z] (m).

Returns:

• los (ndarray) – Line-of-sight unit vector from user to satellite.

• range (float) – Geometric range (m).

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], float]

pytcl.navigation.ins_gnss.pseudorange_measurement_matrix(user_pos, satellites, include_clock=True)

Create measurement matrix for pseudorange observations.

Parameters:

• user_pos (array_like) – User ECEF position [x, y, z] (m).

• satellites (list of SatelliteInfo) – List of satellite information.

• include_clock (bool, optional) – Whether to include clock bias column (default: True).

Returns:

H – (n_sats x 4) or (n_sats x 3) geometry matrix. Columns are [dx, dy, dz, clock_bias] or [dx, dy, dz].

Return type:

ndarray

pytcl.navigation.ins_gnss.compute_dop(H)

Compute Dilution of Precision from geometry matrix.

Parameters:

H (array_like) – Geometry matrix (n_sats x 4) with columns [dx, dy, dz, clock].

Returns:

• GDOP (float) – Geometric DOP.

• PDOP (float) – Position DOP.

• HDOP (float) – Horizontal DOP.

• VDOP (float) – Vertical DOP.

Return type:

Tuple[float, float, float, float]

pytcl.navigation.ins_gnss.satellite_elevation_azimuth(user_lla, sat_ecef, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute satellite elevation and azimuth angles from user position.

Parameters:

• user_lla (array_like) – User position [lat, lon, alt] in (rad, rad, m).

• sat_ecef (array_like) – Satellite ECEF position [x, y, z] (m).

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• elevation (float) – Elevation angle in radians.

• azimuth (float) – Azimuth angle in radians (from north, clockwise).

Return type:

Tuple[float, float]

pytcl.navigation.ins_gnss.initialize_ins_gnss(ins_state, position_std=10.0, velocity_std=1.0, attitude_std=0.1, accel_bias_std=0.1, gyro_bias_std=0.01)

Initialize INS/GNSS integration state.

Parameters:

• ins_state (INSState) – Initial INS navigation state.

• position_std (float, optional) – Initial position uncertainty (m). Default: 10.0.

• velocity_std (float, optional) – Initial velocity uncertainty (m/s). Default: 1.0.

• attitude_std (float, optional) – Initial attitude uncertainty (rad). Default: 0.1.

• accel_bias_std (float, optional) – Initial accelerometer bias uncertainty (m/s^2). Default: 0.1.

• gyro_bias_std (float, optional) – Initial gyroscope bias uncertainty (rad/s). Default: 0.01.

Returns:

state – Initialized INS/GNSS state.

Return type:

INSGNSSState

pytcl.navigation.ins_gnss.loose_coupled_predict(state, imu, accel_noise_std=0.01, gyro_noise_std=0.0001, accel_bias_std=1e-05, gyro_bias_std=1e-07, accel_prev=None, gyro_prev=None)

Perform prediction step for loosely-coupled INS/GNSS.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• imu (IMUData) – IMU measurements for this time step.

• accel_noise_std (float, optional) – Accelerometer noise std (m/s^2). Default: 0.01.

• gyro_noise_std (float, optional) – Gyroscope noise std (rad/s). Default: 1e-4.

• accel_bias_std (float, optional) – Accelerometer bias random walk std. Default: 1e-5.

• gyro_bias_std (float, optional) – Gyroscope bias random walk std. Default: 1e-7.

• accel_prev (array_like, optional) – Previous accelerometer reading for coning/sculling.

• gyro_prev (array_like, optional) – Previous gyroscope reading for coning/sculling.

Returns:

state – Predicted state.

Return type:

INSGNSSState

pytcl.navigation.ins_gnss.loose_coupled_update_position(state, gnss)

Update INS/GNSS state with GNSS position measurement.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS position measurement.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.ins_gnss.loose_coupled_update_velocity(state, gnss)

Update INS/GNSS state with GNSS velocity measurement.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS velocity measurement.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.ins_gnss.loose_coupled_update(state, gnss)

Update INS/GNSS state with GNSS position and velocity measurements.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• gnss (GNSSMeasurement) – GNSS measurement with position and/or velocity.

Returns:

result – Updated state and innovation statistics.

Return type:

LooseCoupledResult

pytcl.navigation.ins_gnss.tight_coupled_pseudorange_innovation(state, satellites, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute pseudorange innovations for tightly-coupled update.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

• innovations (ndarray) – Pseudorange innovations (measured - predicted).

• predicted (ndarray) – Predicted pseudoranges.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[floating]], ndarray[tuple[Any, …], dtype[floating]]]

pytcl.navigation.ins_gnss.tight_coupled_measurement_matrix(state, satellites, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute measurement matrix for tightly-coupled pseudorange update.

Maps 17-state error (15 INS + 2 clock) to pseudorange measurements.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

H – (n_sats x 17) measurement matrix.

Return type:

ndarray

pytcl.navigation.ins_gnss.tight_coupled_update(state, satellites, pseudorange_std=3.0, ellipsoid=(6378137.0, 0.0033528106647474805))

Perform tightly-coupled INS/GNSS update using pseudoranges.

Parameters:

• state (INSGNSSState) – Current INS/GNSS state.

• satellites (list of SatelliteInfo) – Satellite observations with pseudoranges.

• pseudorange_std (float, optional) – Pseudorange measurement noise std (m). Default: 3.0.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid.

Returns:

result – Updated state and DOP values.

Return type:

TightCoupledResult

pytcl.navigation.ins_gnss.gnss_outage_detection(innovations, innovation_cov, threshold=5.991)

Detect potential GNSS measurement faults using chi-squared test.

Parameters:

• innovations (array_like) – Measurement innovations.

• innovation_cov (array_like) – Innovation covariance matrix.

• threshold (float, optional) – Chi-squared threshold (default: 5.991 for 2 DOF, 95% confidence).

Returns:

fault_detected – True if measurement appears faulty.

Return type:

bool

Geodesy

Geodetic computations and reference ellipsoid functions.

Geodetic calculations for navigation and tracking.

This module provides geodetic utilities including: - Geodetic to ECEF conversions - Direct and inverse geodetic problems - Local tangent plane (ENU/NED) conversions - Earth ellipsoid parameters

class pytcl.navigation.geodesy.Ellipsoid(a, f)

Bases: NamedTuple

Earth ellipsoid parameters.

a

Semi-major axis (equatorial radius) in meters.

Type:

float

f

Flattening (a-b)/a.

Type:

float

a: float

Alias for field number 0

f: float

Alias for field number 1

property b: float

Semi-minor axis (polar radius) in meters.

property e2: float

First eccentricity squared.

property ep2: float

Second eccentricity squared.

pytcl.navigation.geodesy.geodetic_to_ecef(lat, lon, alt, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert geodetic coordinates to ECEF (Earth-Centered, Earth-Fixed).

Parameters:

• lat (array_like) – Geodetic latitude in radians.

• lon (array_like) – Geodetic longitude in radians.

• alt (array_like) – Altitude above ellipsoid in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Philadelphia (40°N, 75°W) at 100m altitude
>>> lat, lon, alt = np.radians(40.0), np.radians(-75.0), 100.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> x / 1e6  # ~1.2 million meters
1.24...
>>> # Equator at prime meridian
>>> x, y, z = geodetic_to_ecef(0.0, 0.0, 0.0)
>>> x  # Semi-major axis (equatorial radius)
6378137.0
>>> y, z
(0.0, 0.0)

pytcl.navigation.geodesy.ecef_to_geodetic(x, y, z, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF coordinates to geodetic.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• lat (ndarray) – Geodetic latitude in radians.

• lon (ndarray) – Geodetic longitude in radians.

• alt (ndarray) – Altitude above ellipsoid in meters.

Return type:

Tuple[ndarray[tuple[Any, …], dtype[float64]], ndarray[tuple[Any, …], dtype[float64]], ndarray[tuple[Any, …], dtype[float64]]]

Examples

>>> import numpy as np
>>> # Point on equator at prime meridian
>>> lat, lon, alt = ecef_to_geodetic(6378137.0, 0.0, 0.0)
>>> np.degrees(lat), np.degrees(lon), alt
(0.0, 0.0, 0.0)
>>> # Round-trip conversion
>>> x, y, z = geodetic_to_ecef(np.radians(45.0), np.radians(90.0), 1000.0)
>>> lat2, lon2, alt2 = ecef_to_geodetic(x, y, z)
>>> np.degrees(lat2), np.degrees(lon2), alt2
(45.0..., 90.0..., 1000.0...)

Notes

Uses Bowring’s iterative algorithm for robust conversion.

References

[1]

Bowring, B.R., “Transformation from spatial to geographical coordinates”, Survey Review, 1976.

pytcl.navigation.geodesy.ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF to local ENU (East-North-Up) coordinates.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

east, north, up – ENU coordinates in meters relative to reference point.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point: Philadelphia
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target point slightly east
>>> lat, lon, alt = np.radians(40.0), np.radians(-74.99), 0.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> e  # East displacement in meters
850...
>>> abs(n) < 10  # North displacement should be ~0
True
>>> abs(u) < 10  # Up displacement should be ~0
True

pytcl.navigation.geodesy.enu_to_ecef(east, north, up, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert local ENU to ECEF coordinates.

Parameters:

• east (array_like) – ENU coordinates in meters relative to reference point.

• north (array_like) – ENU coordinates in meters relative to reference point.

• up (array_like) – ENU coordinates in meters relative to reference point.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 1 km east, 500 m north, 100 m up
>>> x, y, z = enu_to_ecef(1000.0, 500.0, 100.0, lat_ref, lon_ref, alt_ref)
>>> # Convert back to verify
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> e, n, u
(1000.0..., 500.0..., 100.0...)

pytcl.navigation.geodesy.ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert ECEF to local NED (North-East-Down) coordinates.

Parameters:

• x (array_like) – ECEF coordinates in meters.

• y (array_like) – ECEF coordinates in meters.

• z (array_like) – ECEF coordinates in meters.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

north, east, down – NED coordinates in meters relative to reference point.

Return type:

ndarray

Examples

>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target above reference
>>> x, y, z = geodetic_to_ecef(lat_ref, lon_ref, 1000.0)  # 1km above
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> abs(n) < 1, abs(e) < 1, d  # Should be ~0, ~0, -1000
(True, True, -1000.0...)

pytcl.navigation.geodesy.ned_to_ecef(north, east, down, lat_ref, lon_ref, alt_ref, ellipsoid=(6378137.0, 0.0033528106647474805))

Convert local NED to ECEF coordinates.

Parameters:

• north (array_like) – NED coordinates in meters relative to reference point.

• east (array_like) – NED coordinates in meters relative to reference point.

• down (array_like) – NED coordinates in meters relative to reference point.

• lat_ref (float) – Reference latitude in radians.

• lon_ref (float) – Reference longitude in radians.

• alt_ref (float) – Reference altitude in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

x, y, z – ECEF coordinates in meters.

Return type:

ndarray

Examples

>>> import numpy as np
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 100m north, 50m east, 10m down
>>> x, y, z = ned_to_ecef(100.0, 50.0, 10.0, lat_ref, lon_ref, alt_ref)
>>> # Verify round-trip
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> n, e, d
(100.0..., 50.0..., 10.0...)

pytcl.navigation.geodesy.direct_geodetic(lat1, lon1, azimuth, distance, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the direct geodetic problem (Vincenty).

Given a starting point, azimuth, and distance, find the destination point. Results are cached for repeated queries with the same parameters.

Parameters:

• lat1 (float) – Starting latitude in radians.

• lon1 (float) – Starting longitude in radians.

• azimuth (float) – Forward azimuth in radians (from north, clockwise).

• distance (float) – Distance in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• lat2 (float) – Destination latitude in radians.

• lon2 (float) – Destination longitude in radians.

• azimuth2 (float) – Back azimuth at destination in radians.

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> # From New York, travel 1000 km northeast
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
>>> azimuth = np.radians(45)  # Northeast
>>> distance = 1_000_000  # 1000 km
>>> lat2, lon2, az2 = direct_geodetic(lat1, lon1, azimuth, distance)
>>> np.degrees(lat2), np.degrees(lon2)  # Destination
(47.0..., -62.6...)

References

[1]

Vincenty, T., “Direct and Inverse Solutions of Geodesics on the Ellipsoid with Application of Nested Equations”, Survey Review, 1975.

pytcl.navigation.geodesy.inverse_geodetic(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the inverse geodetic problem (Vincenty).

Given two points, find the distance and azimuths between them. Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – Starting latitude in radians.

• lon1 (float) – Starting longitude in radians.

• lat2 (float) – Destination latitude in radians.

• lon2 (float) – Destination longitude in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

• distance (float) – Geodesic distance in meters.

• azimuth1 (float) – Forward azimuth at start in radians.

• azimuth2 (float) – Back azimuth at destination in radians.

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> # Distance from New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)  # NYC
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)   # London
>>> dist, az1, az2 = inverse_geodetic(lat1, lon1, lat2, lon2)
>>> dist / 1000  # Distance in km
5570...
>>> np.degrees(az1)  # Initial heading from NYC
51.2...

Notes

May fail to converge for nearly antipodal points.

References

[1]

Vincenty, T., “Direct and Inverse Solutions of Geodesics on the Ellipsoid with Application of Nested Equations”, Survey Review, 1975.

pytcl.navigation.geodesy.haversine_distance(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute great-circle distance using the haversine formula.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• radius (float, optional) – Earth radius in meters (default: 6371 km).

Returns:

Great-circle distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> # Distance from equator to 45°N along prime meridian
>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.radians(45.0), 0.0
>>> dist = haversine_distance(lat1, lon1, lat2, lon2)
>>> dist / 1000  # ~5000 km
5003...
>>> # Same point -> 0 distance
>>> haversine_distance(0.0, 0.0, 0.0, 0.0)
0.0

Notes

This is a spherical approximation. For higher accuracy on an ellipsoid, use inverse_geodetic().

pytcl.navigation.geodesy.clear_geodesy_cache()

Clear all geodesy computation caches.

This can be useful to free memory after processing large datasets or when cache statistics are being monitored.

pytcl.navigation.geodesy.get_geodesy_cache_info()

Get cache statistics for geodesy computations.

Returns:

Dictionary with cache statistics for inverse and direct geodetic caches.

Return type:

dict[str, Any]

Great Circle Navigation

Shortest-path (orthodrome) navigation on a spherical Earth.

Great circle navigation algorithms.

This module provides great circle (orthodrome) navigation functions for computing the shortest path on a sphere, including: - Great circle distance and initial azimuth - Intermediate waypoint computation - Great circle intersection - Cross-track and along-track distances - TDOA localization on a sphere

class pytcl.navigation.great_circle.GreatCircleResult(distance, azimuth1, azimuth2)

Bases: NamedTuple

Result of great circle computation.

distance

Great circle distance in meters.

Type:

float

azimuth1

Initial azimuth in radians (from north, clockwise).

Type:

float

azimuth2

Final azimuth in radians.

Type:

float

distance: float

Alias for field number 0

azimuth1: float

Alias for field number 1

azimuth2: float

Alias for field number 2

class pytcl.navigation.great_circle.WaypointResult(lat, lon)

Bases: NamedTuple

Result of waypoint computation.

lat

Latitude of waypoint in radians.

Type:

float

lon

Longitude of waypoint in radians.

Type:

float

lat: float

Alias for field number 0

lon: float

Alias for field number 1

class pytcl.navigation.great_circle.IntersectionResult(lat1, lon1, lat2, lon2, valid)

Bases: NamedTuple

Result of great circle intersection.

lat1

Latitude of first intersection in radians.

Type:

float

lon1

Longitude of first intersection in radians.

Type:

float

lat2

Latitude of second intersection (antipodal) in radians.

Type:

float

lon2

Longitude of second intersection (antipodal) in radians.

Type:

float

valid

True if intersection exists.

Type:

bool

lat1: float

Alias for field number 0

lon1: float

Alias for field number 1

lat2: float

Alias for field number 2

lon2: float

Alias for field number 3

valid: bool

Alias for field number 4

class pytcl.navigation.great_circle.CrossTrackResult(cross_track, along_track)

Bases: NamedTuple

Result of cross-track distance computation.

cross_track

Cross-track distance in meters (positive = right of path).

Type:

float

along_track

Along-track distance from start in meters.

Type:

float

cross_track: float

Alias for field number 0

along_track: float

Alias for field number 1

pytcl.navigation.great_circle.great_circle_distance(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute great circle distance between two points.

Uses the haversine formula for numerical stability at small distances. Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Great circle distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> # New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> dist = great_circle_distance(lat1, lon1, lat2, lon2)
>>> print(f"Distance: {dist/1000:.0f} km")

pytcl.navigation.great_circle.great_circle_azimuth(lat1, lon1, lat2, lon2)

Compute initial azimuth (bearing) from point 1 to point 2.

Results are cached for repeated queries with the same coordinates.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Initial azimuth in radians (from north, clockwise, 0 to 2π).

Return type:

float

Examples

>>> import numpy as np
>>> # Bearing from New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> az = great_circle_azimuth(lat1, lon1, lat2, lon2)
>>> print(f"Initial bearing: {np.degrees(az):.1f}°")

pytcl.navigation.great_circle.great_circle_inverse(lat1, lon1, lat2, lon2, radius=6371000.0)

Solve the inverse great circle problem.

Given two points, compute distance and azimuths.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Distance and azimuths.

Return type:

GreatCircleResult

Examples

>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)  # NYC
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)   # London
>>> result = great_circle_inverse(lat1, lon1, lat2, lon2)
>>> result.distance > 5000000  # Over 5000 km
True

pytcl.navigation.great_circle.angular_distance(lat1, lon1, lat2, lon2)

Compute angular distance (central angle) between two points.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

Returns:

Angular distance in radians.

Return type:

float

Examples

Compute angular distance between New York and London:

>>> import numpy as np
>>> # NYC: 40.7°N, 74.0°W; London: 51.5°N, 0.1°W
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
>>> lat2, lon2 = np.radians(51.5), np.radians(-0.1)
>>> angle = angular_distance(lat1, lon1, lat2, lon2)
>>> np.degrees(angle)  # about 50 degrees
49.9...

See also

great_circle_distance

Compute distance on sphere with given radius.

pytcl.navigation.great_circle.great_circle_waypoint(lat1, lon1, lat2, lon2, fraction)

Compute intermediate waypoint along a great circle.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• fraction (float) – Fraction of distance (0 = start, 1 = end).

Returns:

Latitude and longitude of waypoint.

Return type:

WaypointResult

Examples

>>> import numpy as np
>>> # Midpoint between New York and London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060)
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278)
>>> mid = great_circle_waypoint(lat1, lon1, lat2, lon2, 0.5)
>>> print(f"Midpoint: {np.degrees(mid.lat):.2f}°, {np.degrees(mid.lon):.2f}°")

pytcl.navigation.great_circle.great_circle_waypoints(lat1, lon1, lat2, lon2, n_points)

Compute multiple waypoints along a great circle.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• n_points (int) – Number of waypoints (including start and end).

Returns:

lats, lons – Arrays of waypoint latitudes and longitudes in radians.

Return type:

ndarray

Examples

>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.pi/4, np.pi/4
>>> lats, lons = great_circle_waypoints(lat1, lon1, lat2, lon2, 5)
>>> len(lats)
5

pytcl.navigation.great_circle.great_circle_direct(lat1, lon1, azimuth, distance, radius=6371000.0)

Solve the direct great circle problem.

Given starting point, azimuth, and distance, find destination.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• azimuth (float) – Initial azimuth in radians (from north, clockwise).

• distance (float) – Distance in meters.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Destination latitude and longitude.

Return type:

WaypointResult

Examples

>>> import numpy as np
>>> # 1000 km northeast from origin
>>> lat, lon = 0.0, 0.0
>>> az = np.radians(45)  # Northeast
>>> dest = great_circle_direct(lat, lon, az, 1000000)
>>> print(f"Destination: {np.degrees(dest.lat):.2f}°, {np.degrees(dest.lon):.2f}°")

pytcl.navigation.great_circle.destination_point(lat, lon, bearing, angular_distance)

Compute destination point given start, bearing, and angular distance.

Parameters:

• lat (float) – Starting point coordinates in radians.

• lon (float) – Starting point coordinates in radians.

• bearing (float) – Initial bearing in radians.

• angular_distance (float) – Angular distance in radians (distance/radius).

Returns:

Destination coordinates.

Return type:

WaypointResult

Examples

>>> lat, lon = 0.0, 0.0
>>> bearing = np.radians(90)  # Due East
>>> ang_dist = np.radians(10)  # 10 degrees
>>> dest = destination_point(lat, lon, bearing, ang_dist)
>>> np.degrees(dest.lon)  # Should be ~10 degrees East
10.0

pytcl.navigation.great_circle.cross_track_distance(lat_point, lon_point, lat1, lon1, lat2, lon2, radius=6371000.0)

Compute cross-track and along-track distances.

The cross-track distance is the perpendicular distance from a point to the great circle path between two other points.

Parameters:

• lat_point (float) – Point coordinates in radians.

• lon_point (float) – Point coordinates in radians.

• lat1 (float) – Path start coordinates in radians.

• lon1 (float) – Path start coordinates in radians.

• lat2 (float) – Path end coordinates in radians.

• lon2 (float) – Path end coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Cross-track distance (positive = right of path) and along-track distance.

Return type:

CrossTrackResult

Notes

Positive cross-track means the point is to the right of the path (when traveling from start to end).

Examples

>>> # Point near a path from origin to northeast
>>> lat_pt, lon_pt = np.radians(5), np.radians(2)
>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.radians(10), np.radians(10)
>>> result = cross_track_distance(lat_pt, lon_pt, lat1, lon1, lat2, lon2)
>>> abs(result.cross_track) < 500000  # Within 500 km
True

pytcl.navigation.great_circle.great_circle_intersect(lat1, lon1, azimuth1, lat2, lon2, azimuth2)

Find intersection of two great circles.

Given two points with initial bearings, find where the great circles defined by those bearings intersect.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• azimuth1 (float) – Bearing from first point in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• azimuth2 (float) – Bearing from second point in radians.

Returns:

Two intersection points (antipodal) and validity flag.

Return type:

IntersectionResult

Notes

Great circles always intersect at two antipodal points (unless they are identical or parallel). The returned points are the intersections closest to the given points.

Examples

>>> lat1, lon1 = 0.0, 0.0
>>> az1 = np.radians(45)  # Northeast
>>> lat2, lon2 = 0.0, np.radians(10)
>>> az2 = np.radians(315)  # Northwest
>>> result = great_circle_intersect(lat1, lon1, az1, lat2, lon2, az2)
>>> result.valid
True

pytcl.navigation.great_circle.great_circle_path_intersect(lat1a, lon1a, lat2a, lon2a, lat1b, lon1b, lat2b, lon2b)

Find intersection of two great circle paths (defined by endpoints).

Parameters:

• lat1a (float) – Start of first path in radians.

• lon1a (float) – Start of first path in radians.

• lat2a (float) – End of first path in radians.

• lon2a (float) – End of first path in radians.

• lat1b (float) – Start of second path in radians.

• lon1b (float) – Start of second path in radians.

• lat2b (float) – End of second path in radians.

• lon2b (float) – End of second path in radians.

Returns:

Intersection points and validity.

Return type:

IntersectionResult

Examples

>>> # Two crossing paths
>>> result = great_circle_path_intersect(
...     0.0, 0.0, np.radians(10), np.radians(10),  # Path A
...     0.0, np.radians(10), np.radians(10), 0.0    # Path B
... )
>>> result.valid
True

pytcl.navigation.great_circle.great_circle_tdoa_loc(lat1, lon1, lat2, lon2, lat3, lon3, tdoa12, tdoa13, speed=299792458.0, radius=6371000.0)

TDOA localization on a sphere.

Given three receivers and two time-difference-of-arrival measurements, estimate the emitter location.

Parameters:

• lat1 (float) – First receiver coordinates in radians.

• lon1 (float) – First receiver coordinates in radians.

• lat2 (float) – Second receiver coordinates in radians.

• lon2 (float) – Second receiver coordinates in radians.

• lat3 (float) – Third receiver coordinates in radians.

• lon3 (float) – Third receiver coordinates in radians.

• tdoa12 (float) – Time difference of arrival between receivers 1 and 2 (seconds). Positive means signal arrived at receiver 1 first.

• tdoa13 (float) – Time difference of arrival between receivers 1 and 3 (seconds).

• speed (float, optional) – Signal propagation speed in m/s (default: speed of light).

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

loc1, loc2 – Two possible emitter locations, or None if no solution.

Return type:

Optional[WaypointResult]

Notes

TDOA localization finds the intersection of two hyperbolas. On a sphere, these are represented by the locus of points where the difference in distance to two receivers is constant.

This implementation uses a grid search followed by Newton-Raphson refinement for robustness.

pytcl.navigation.great_circle.clear_great_circle_cache()

Clear all great circle computation caches.

This can be useful to free memory after processing large datasets or when cache statistics are being monitored.

Examples

>>> import numpy as np
>>> from pytcl.navigation import great_circle_distance, clear_great_circle_cache
>>> # Compute a few distances (cached)
>>> d1 = great_circle_distance(0, 0, np.radians(1), 0)
>>> d2 = great_circle_distance(0, 0, np.radians(2), 0)
>>> # Check cache state before clear
>>> cache_before = great_circle_distance.__wrapped__.__self__.cache_info()
>>> # Clear all cached values
>>> clear_great_circle_cache()
>>> # Cache is now empty

pytcl.navigation.great_circle.get_cache_info()

Get cache statistics for great circle computations.

Returns:

Dictionary with cache statistics for distance and azimuth caches.

Return type:

dict[str, Any]

Examples

>>> import numpy as np
>>> from pytcl.navigation import great_circle_distance, get_cache_info, clear_great_circle_cache
>>> # Clear cache first
>>> clear_great_circle_cache()
>>> # Compute some distances (multiple calls to test cache hits)
>>> d1 = great_circle_distance(0, 0, np.radians(1), 0)
>>> d1_again = great_circle_distance(0, 0, np.radians(1), 0)  # Cache hit
>>> # Get cache statistics
>>> info = get_cache_info()
>>> info['distance']['hits'] > 0  # Should have at least one hit
True
>>> info['distance']['currsize'] > 0  # Cache is not empty
True

See also

clear_great_circle_cache

Clear all cached values.

Rhumb Line Navigation

Constant-bearing (loxodrome) navigation on spherical and ellipsoidal Earth.

Rhumb line (loxodrome) navigation algorithms.

This module provides rhumb line navigation functions for computing constant-bearing paths on a sphere and ellipsoid, including: - Rhumb line distance and bearing - Direct and inverse rhumb problems - Rhumb line intersection - Spherical and ellipsoidal formulations

class pytcl.navigation.rhumb.RhumbResult(distance, bearing)

Bases: NamedTuple

Result of rhumb line computation.

distance

Rhumb line distance in meters.

Type:

float

bearing

Constant bearing in radians (from north, clockwise).

Type:

float

distance: float

Alias for field number 0

bearing: float

Alias for field number 1

class pytcl.navigation.rhumb.RhumbDirectResult(lat, lon)

Bases: NamedTuple

Result of direct rhumb problem.

lat

Destination latitude in radians.

Type:

float

lon

Destination longitude in radians.

Type:

float

lat: float

Alias for field number 0

lon: float

Alias for field number 1

class pytcl.navigation.rhumb.RhumbIntersectionResult(lat, lon, valid)

Bases: NamedTuple

Result of rhumb line intersection.

lat

Intersection latitude in radians.

Type:

float

lon

Intersection longitude in radians.

Type:

float

valid

True if intersection exists.

Type:

bool

lat: float

Alias for field number 0

lon: float

Alias for field number 1

valid: bool

Alias for field number 2

pytcl.navigation.rhumb.rhumb_distance_spherical(lat1, lon1, lat2, lon2, radius=6371000.0)

Compute rhumb line distance on a sphere.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Rhumb line distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> dist = rhumb_distance_spherical(lat1, lon1, lat2, lon2)

pytcl.navigation.rhumb.rhumb_bearing(lat1, lon1, lat2, lon2)

Compute rhumb line bearing from point 1 to point 2.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Constant bearing in radians (from north, clockwise, 0 to 2π).

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> bearing = rhumb_bearing(lat1, lon1, lat2, lon2)
>>> print(f"Bearing: {np.degrees(bearing):.1f}°")

pytcl.navigation.rhumb.indirect_rhumb_spherical(lat1, lon1, lat2, lon2, radius=6371000.0)

Solve the indirect rhumb problem on a sphere.

Given two points, find the rhumb line distance and bearing.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Distance and constant bearing.

Return type:

RhumbResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # New York
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> result = indirect_rhumb_spherical(lat1, lon1, lat2, lon2)
>>> result.distance > 5000000  # Over 5000 km
True

pytcl.navigation.rhumb.direct_rhumb_spherical(lat1, lon1, bearing, distance, radius=6371000.0)

Solve the direct rhumb problem on a sphere.

Given starting point, bearing, and distance, find destination.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• bearing (float) – Constant bearing in radians (from north, clockwise).

• distance (float) – Distance in meters.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

Destination latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat, lon = np.radians(40), np.radians(-74)
>>> bearing = np.radians(90)  # Due east
>>> dest = direct_rhumb_spherical(lat, lon, bearing, 1000000)

pytcl.navigation.rhumb.rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Compute rhumb line distance on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Rhumb line distance in meters.

Return type:

float

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> dist = rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2)
>>> dist > 5000000  # Over 5000 km
True

pytcl.navigation.rhumb.indirect_rhumb(lat1, lon1, lat2, lon2, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the indirect rhumb problem on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Distance and constant bearing.

Return type:

RhumbResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # New York
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> result = indirect_rhumb(lat1, lon1, lat2, lon2)
>>> 0 < result.bearing < np.pi  # Eastward bearing
True

pytcl.navigation.rhumb.direct_rhumb(lat1, lon1, bearing, distance, ellipsoid=(6378137.0, 0.0033528106647474805))

Solve the direct rhumb problem on an ellipsoid.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• bearing (float) – Constant bearing in radians (from north, clockwise).

• distance (float) – Distance in meters.

• ellipsoid (Ellipsoid, optional) – Reference ellipsoid (default: WGS84).

Returns:

Destination latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat, lon = np.radians(40), np.radians(-74)
>>> bearing = np.radians(90)  # Due east
>>> dest = direct_rhumb(lat, lon, bearing, 100000)  # 100 km
>>> np.degrees(dest.lon) > -74  # Moved east
True

pytcl.navigation.rhumb.rhumb_intersect(lat1, lon1, bearing1, lat2, lon2, bearing2)

Find intersection of two rhumb lines.

Parameters:

• lat1 (float) – First point coordinates in radians.

• lon1 (float) – First point coordinates in radians.

• bearing1 (float) – Bearing from first point in radians.

• lat2 (float) – Second point coordinates in radians.

• lon2 (float) – Second point coordinates in radians.

• bearing2 (float) – Bearing from second point in radians.

Returns:

Intersection point and validity flag.

Return type:

RhumbIntersectionResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> bearing1 = np.radians(45)
>>> bearing2 = np.radians(270)
>>> result = rhumb_intersect(lat1, lon1, bearing1, lat2, lon2, bearing2)
>>> result.valid  # May or may not intersect
True

Notes

Unlike great circles, two rhumb lines may not intersect (if bearings are parallel or if the lines diverge before meeting).

pytcl.navigation.rhumb.rhumb_midpoint(lat1, lon1, lat2, lon2)

Compute midpoint along a rhumb line.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

Returns:

Midpoint latitude and longitude.

Return type:

RhumbDirectResult

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(0), np.radians(0)
>>> lat2, lon2 = np.radians(10), np.radians(10)
>>> mid = rhumb_midpoint(lat1, lon1, lat2, lon2)
>>> np.isclose(np.degrees(mid.lat), 5, atol=0.1)
True

pytcl.navigation.rhumb.rhumb_waypoints(lat1, lon1, lat2, lon2, n_points, radius=6371000.0)

Compute multiple waypoints along a rhumb line.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• n_points (int) – Number of waypoints (including start and end).

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

lats, lons – Arrays of waypoint latitudes and longitudes in radians.

Return type:

ndarray

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)
>>> lat2, lon2 = np.radians(51), np.radians(0)
>>> lats, lons = rhumb_waypoints(lat1, lon1, lat2, lon2, 5)
>>> len(lats)
5

pytcl.navigation.rhumb.compare_great_circle_rhumb(lat1, lon1, lat2, lon2, radius=6371000.0)

Compare great circle and rhumb line paths.

Parameters:

• lat1 (float) – Starting point coordinates in radians.

• lon1 (float) – Starting point coordinates in radians.

• lat2 (float) – Destination point coordinates in radians.

• lon2 (float) – Destination point coordinates in radians.

• radius (float, optional) – Sphere radius in meters (default: 6371 km).

Returns:

• gc_distance (float) – Great circle distance in meters.

• rhumb_distance (float) – Rhumb line distance in meters.

• difference_percent (float) – Percentage difference (rhumb is longer).

Return type:

Tuple[float, float, float]

Examples

>>> import numpy as np
>>> lat1, lon1 = np.radians(40), np.radians(-74)  # NYC
>>> lat2, lon2 = np.radians(51), np.radians(0)   # London
>>> gc, rhumb, diff = compare_great_circle_rhumb(lat1, lon1, lat2, lon2)
>>> rhumb > gc  # Rhumb is always longer
True