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

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

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

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

• 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.

Variables:

• position (ndarray) – Position in geodetic coordinates [latitude (rad), longitude (rad), altitude (m)].

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

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

• time (float) – Time associated with this state (seconds).

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.

Variables:

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

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

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

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.

Variables:

• position_error (ndarray) – Position error [delta_lat, delta_lon, delta_alt] (rad, rad, m).

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

• attitude_error (ndarray) – Attitude error angles [phi_N, phi_E, phi_D] (rad).

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

• gyro_bias (ndarray) – Gyroscope bias [bwx, bwy, bwz] (rad/s).

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 INCREMENT [dtheta_x, dtheta_y, dtheta_z] (rad), not an angular rate – integrate the gyro over the interval first (gyro_rate * dt).

• gyro_curr (array_like) – Current angular increment (rad), same convention as gyro_prev.

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.

Both arguments are angular increments, not rates: the formula is (1/12) * (theta_prev x theta_curr), so passing rates in rad/s returns rad^2/s^2 and is wrong by a factor of 1/dt^2. compensate_imu_data() takes rates and does the integration for you; prefer it unless you already hold increments.

References

• 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 velocity INCREMENT [dv_x, dv_y, dv_z] (m/s), not a specific force – integrate the accelerometer over the interval first (accel * dt).

• accel_curr (array_like) – Current velocity increment (m/s), same convention as accel_prev.

• gyro_prev (array_like) – Previous angular INCREMENT (rad), not an angular rate.

• gyro_curr (array_like) – Current angular increment (rad).

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).

All four arguments are increments, not rates: passing m/s^2 and rad/s yields m*rad/s^3 and is wrong by 1/dt^2. The formula (1/12) * (theta_prev x dv_curr + dv_prev x theta_curr) is symmetric under swapping the two role pairs, so calling with the velocity and angular arguments transposed happens to give the same answer – do not rely on that. compensate_imu_data() takes rates and integrates for you.

References

• 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. Accepted for signature stability; the DCM enters the computation only as R_b_n @ R_b_n.T, which is the identity for any rotation matrix, so the state cannot change Q.

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

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

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

• 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.

Variables:

• position (ndarray, optional) – GNSS position [lat, lon, alt] in (rad, rad, m).

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

• position_cov (ndarray, optional) – Position covariance (3x3) in geodetic frame, units [rad^2, rad^2, m^2] on the diagonal – matching position’s (rad, rad, m). It is used directly as the measurement covariance against a (rad, rad, m) innovation, so supplying metres^2 for the latitude/longitude entries (how GNSS accuracy is usually quoted) inflates R by ~1e13; convert with position_std_to_error_state_units(). This is the v2.0.0 breaking change documented in the migration guide.

• velocity_cov (ndarray, optional) – Velocity covariance (3x3) in NED frame.

• time (float) – GPS time of measurement (seconds).

• valid (bool) – Whether the measurement is valid.

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.

Variables:

• prn (int) – Satellite PRN number.

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

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

• pseudorange (float) – Measured pseudorange (m).

• doppler (float, optional) – Measured Doppler shift (Hz).

• cn0 (float, optional) – Carrier-to-noise ratio (dB-Hz).

• elevation (float, optional) – Satellite elevation angle (rad).

• azimuth (float, optional) – Satellite azimuth angle (rad).

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.

Variables:

• ins_state (INSState) – Current INS navigation state.

• error_state (ndarray) – Error state vector, 15 elements. (Previously documented “15 or 17”; no code path produces 17 – tight_coupled_update builds a 16-element extended vector internally and writes back zeros(15).)

• error_cov (ndarray) – Error state covariance matrix.

• clock_bias (float) – Receiver clock bias (m).

• clock_drift (float) – Receiver clock drift (m/s).

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.

Variables:

• state (INSGNSSState) – Updated navigation state.

• innovation (ndarray) – Measurement innovation (residual).

• innovation_cov (ndarray) – Innovation covariance.

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.

Variables:

• state (INSGNSSState) – Updated navigation state.

• innovations (ndarray) – Pseudorange/Doppler innovations.

• dop (tuple) – Dilution of precision (GDOP, PDOP, HDOP, VDOP).

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, user_lla=None)

Compute Dilution of Precision from a geometry matrix.

HDOP and VDOP are only horizontal and vertical with respect to some local frame. They are read off the first two and the third diagonal entries of inv(H^T H), so they mean “horizontal” only if the first two position columns of H span the local horizontal plane. An ECEF geometry matrix does not: its x and y axes point at the equator regardless of where the user is, so HDOP computed from one is horizontal only at the poles (gh-19).

Pass user_lla and the position columns are rotated into the local ENU frame first, which is what makes the split meaningful. Without it the matrix is used as given, which is correct when H is already local.

GDOP and PDOP are invariant under rotation – they are traces – so they were correct either way and are unaffected by this argument.

Parameters:

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

• user_lla (array_like, optional) – User position [lat, lon, alt] in (rad, rad, m). When given, the three position columns of H are rotated from ECEF into ENU before the DOP terms are read off. Omit when H is already expressed in a local frame.

Returns:

• GDOP (float) – Geometric DOP. Rotation-invariant.

• PDOP (float) – Position DOP. Rotation-invariant.

• HDOP (float) – Horizontal DOP, in the local frame if user_lla was supplied.

• VDOP (float) – Vertical DOP, in the local frame if user_lla was supplied.

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 a 17-column error layout (15 INS + clock bias + clock drift) to pseudorange measurements. Only the clock-BIAS column (index 15) is ever populated; the drift column is identically zero, and tight_coupled_update slices the matrix to its first 16 columns before use.

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.

Variables:

• a (float) – Semi-major axis (equatorial radius) in meters.

• f (float) – Flattening (a-b)/a.

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)
>>> round(x / 1e6, 4)  # ~1.27 million meters
1.2663
>>> # 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), float(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)
>>> round(np.degrees(lat2), 6), round(np.degrees(lon2), 6), round(float(alt2), 3)
(45.0, 90.0, 1000.0)

Notes

Uses Bowring’s iterative algorithm for robust conversion.

References

• 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)
>>> round(e, 1)  # East displacement in meters
853.9
>>> 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)
>>> round(e, 3), round(n, 3), round(u, 3)
(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)
>>> round(np.degrees(lat2), 4), round(np.degrees(lon2), 4)  # Destination
(46.7148, -64.7496)

References

• 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)
>>> round(dist / 1000, 1)  # Geodesic distance in km
5585.2
>>> np.degrees(az1)  # Initial heading from NYC
51.2...

Notes

May fail to converge for nearly antipodal points.

References

• 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().

class pytcl.navigation.geodesy.OsculatingSphere(radius, center)

Bases: NamedTuple

A local spherical approximation to the reference ellipsoid.

Variables:

• radius (float) – Radius of the osculating sphere in meters.

• center (ndarray) – Offset of the sphere’s center from the ellipsoid’s center in ECEF coordinates, shape (3,). Subtract this from ECEF positions to work in the sphere-centered frame.

radius: float

Alias for field number 0

center: ndarray[tuple[Any, ...], dtype[float64]]

Alias for field number 1

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

Osculating sphere of the reference ellipsoid at a surface point.

The sphere has the Gaussian radius of curvature sqrt(M * N) (the geometric mean of the meridian and prime-vertical radii of curvature) and is positioned to touch the ellipsoid at the given latitude and longitude. Used as the local spherical-Earth approximation in the standard-exponential-model refraction routines.

Port of osculatingSpher4LatLon.m (the default 3D, no-geoid path).

Parameters:

• lat (float) – Geodetic latitude in radians.

• lon (float) – Longitude in radians.

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

Returns:

sphere – The radius and the ECEF offset of the sphere’s center.

Return type:

OsculatingSphere

Examples

>>> sph = osculating_sphere(0.7, 0.1)
>>> print(f"{sph.radius:.3f}")
6374462.341

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.

Variables:

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

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

• azimuth2 (float) – Final azimuth in radians.

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.

Variables:

• lat (float) – Latitude of waypoint in radians.

• lon (float) – Longitude of waypoint in radians.

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.

Variables:

• lat1 (float) – Latitude of first intersection in radians.

• lon1 (float) – Longitude of first intersection in radians.

• lat2 (float) – Latitude of second intersection (antipodal) in radians.

• lon2 (float) – Longitude of second intersection (antipodal) in radians.

• valid (bool) – True if intersection exists.

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.

Variables:

• cross_track (float) – Cross-track distance in meters (positive = right of path).

• along_track (float) – Along-track distance from start in meters.

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")
Distance: 5570 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}°")
Initial bearing: 51.2°

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)
>>> round(np.degrees(angle), 2)  # about 50 degrees
50.12

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}°")
Midpoint: 52.37°, -41.29°

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}°")
Destination: 6.35°, 6.39°

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)
>>> round(np.degrees(dest.lon), 6)  # 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) – t1 - t2, the arrival time at receiver 1 minus the arrival time at receiver 2 (seconds). Positive therefore means the signal reached receiver 2 first, since receiver 1 is the farther one. (This read “Positive means signal arrived at receiver 1 first”, which is backwards: the solver drives d1 - d2 toward tdoa12 * speed / radius, so a positive value places the emitter farther from receiver 1.)

• tdoa13 (float) – t1 - t3, same convention as tdoa12.

• 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 – Up to two possible emitter locations, or None where one was not found.

Warning

loc2 is rarely found: it was returned in 2 of 30 random three-receiver configurations, and 12 of those 30 produced no solution at all. Two receiver pairs give two hyperbolic loci that generically cross at two points, both consistent with the measurements, and the emitter may be either – so a single returned location is one of two candidates, not the emitter. In that sample the true emitter was among the returned locations 7 times in 30.

The second-solution search descends from loc1’s antipode, which has no geometric basis; seeding it from the grid instead was tried and did not help. Finding the second root reliably needs a different method than the 5-degree grid plus gradient descent used here. What is guaranteed is that any location returned satisfies the residual gate: the worst residual over that sample is 2.7e-07 against a 1e-6 threshold.

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.

A 5-degree grid search seeds a normalized-gradient descent with backtracking – gradient descent, despite an older comment naming it Newton-Raphson: no second derivative is formed anywhere.

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
>>> from pytcl.navigation.great_circle import clear_great_circle_cache, get_cache_info
>>> # 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)
>>> # Clear all cached values
>>> clear_great_circle_cache()
>>> # Cache is now empty
>>> get_cache_info()['distance']['currsize']
0

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
>>> from pytcl.navigation.great_circle import 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.

Variables:

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

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

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.

Variables:

• lat (float) – Destination latitude in radians.

• lon (float) – Destination longitude in radians.

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.

Variables:

• lat (float) – Intersection latitude in radians.

• lon (float) – Intersection longitude in radians.

• valid (bool) – True if intersection exists.

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}°")
Bearing: 78.0°

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

Notes

The meridional radius is evaluated once at the midpoint latitude rather than integrated along the leg, which is exact only for a leg short enough that the radius is effectively constant (gh-25).

Quantified: about 0.05% on legs spanning roughly 45 degrees of latitude – 4.5 km over 9,800 km – and up to about 400 m on a 3,000 km due-north leg. Sub-meter on typical legs. Round trips through direct_rhumb are self-consistent to under a millimeter because the same approximation appears in both directions and cancels; that consistency is therefore not evidence of accuracy.

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