"""
Process noise covariance matrices for coordinated turn models.
This module provides functions to construct process noise covariance matrices
(Q matrices) for coordinated turn motion models used in target tracking.
Coordinated turn models describe targets that maintain a constant turn rate,
such as aircraft executing banking maneuvers.
The process noise captures uncertainty in the target's motion, including:
- Linear acceleration uncertainty along the velocity direction
- Turn rate (angular velocity) uncertainty
Available functions:
- ``q_coord_turn_2d``: 2D coordinated turn (planar motion)
- ``q_coord_turn_3d``: 3D coordinated turn (spatial motion)
- ``q_coord_turn_polar``: Polar/heading-speed representation
These Q matrices are designed to work with the corresponding state transition
matrices in :mod:`pytcl.dynamic_models.discrete_time.coordinated_turn`.
Examples
--------
Create process noise for a 2D coordinated turn tracker:
>>> from pytcl.dynamic_models.process_noise import q_coord_turn_2d
>>> Q = q_coord_turn_2d(T=0.1, sigma_a=2.0) # 0.1s step, 2 m/s² accel noise
>>> Q.shape
(4, 4)
With turn rate state estimation:
>>> Q = q_coord_turn_2d(T=0.1, sigma_a=2.0, sigma_omega=0.01,
... state_type='position_velocity_omega')
>>> Q.shape
(5, 5)
See Also
--------
pytcl.dynamic_models.discrete_time.coordinated_turn : State transition matrices
pytcl.dynamic_models.process_noise.polynomial : CV/CA process noise
References
----------
- Bar-Shalom, Y., Li, X.R., and Kirubarajan, T., "Estimation with
Applications to Tracking and Navigation", Wiley, 2001.
- Blackman, S. and Popoli, R., "Design and Analysis of Modern
Tracking Systems", Artech House, 1999.
"""
import numpy as np
from numpy.typing import NDArray
[docs]
def q_coord_turn_2d(
T: float,
sigma_a: float,
sigma_omega: float = 0.0,
state_type: str = "position_velocity",
) -> NDArray[np.floating]:
"""
Create process noise covariance for 2D coordinated turn model.
Parameters
----------
T : float
Time step in seconds.
sigma_a : float
Standard deviation of acceleration noise [m/s²].
sigma_omega : float, optional
Standard deviation of turn rate noise [rad/s²].
Only used if state_type includes omega.
state_type : str, optional
State vector composition:
- 'position_velocity': [x, vx, y, vy] (default)
- 'position_velocity_omega': [x, vx, y, vy, omega]
Returns
-------
Q : ndarray
Process noise covariance matrix.
Examples
--------
>>> Q = q_coord_turn_2d(T=1.0, sigma_a=1.0)
>>> Q.shape
(4, 4)
Notes
-----
**This is not a port of MATLAB's ``QCoordTurn``.** It stacks an
independent discrete-white-noise-acceleration block per axis,
``sigma_a**2 * [[T**4/4, T**3/2], [T**3/2, T**2]]``, plus a diagonal
``sigma_omega**2 * T**2`` for omega. ``QCoordTurn`` instead builds
``Q = G * G.T`` from a single column vector
``G = [sigmaV*T**2/2; sigmaV*T**2/2; sigmaV*T; sigmaV*T; sigmaTurn*T]``
(state ``[x; y; xdot; ydot; omega]``), so its result is rank 1 and every
x/y/velocity/omega pair is correlated. Here there is no x-y, no
position-omega and no velocity-omega coupling.
The comparable MATLAB branch is the length-5 state (2D, no linear
acceleration) with ``sigmaV2 = sigma_a**2`` and
``sigmaTurn2 = sigma_omega**2``, which is this function with
``state_type='position_velocity_omega'`` after permuting this
function's ``[x, vx, y, vy, omega]`` to MATLAB's
``[x, y, vx, vy, omega]``. ``state_type='position_velocity'`` has no
MATLAB counterpart (``QCoordTurn`` rejects 4-element states); its
comparable object is the leading 4x4 block of that same length-5 result.
MATLAB's length-6 branch (adds a linear-acceleration state) has no
counterpart here.
Measured at ``T=1, sigma_a=2, sigma_omega=0.1``: the diagonals agree
exactly, but the largest elementwise difference from ``QCoordTurn`` is
4.0 for both state types. The ``'position_velocity'`` result has rank 2
(eigenvalues ``[0, 0, 5, 5]``) and the ``'position_velocity_omega'``
result has rank 3 (eigenvalues ``[0, 0, 0.01, 5, 5]``); ``QCoordTurn``
has rank 1 in both. Matching diagonal blocks is the only property the
two share, so filters tuned against one will not reproduce the other.
The state is interleaved, unlike MATLAB's derivative-major layout; see
:func:`pytcl.dynamic_models.q_poly_kal`.
See Also
--------
f_coord_turn_2d : State transition matrix for 2D coordinated turn.
"""
var_a = sigma_a**2
# Discrete white noise acceleration model for x and y
Q_pos_vel = var_a * np.array(
[
[T**4 / 4, T**3 / 2],
[T**3 / 2, T**2],
],
dtype=np.float64,
)
if state_type == "position_velocity_omega":
n = 5
Q = np.zeros((n, n), dtype=np.float64)
# x, vx block
Q[0:2, 0:2] = Q_pos_vel
# y, vy block
Q[2:4, 2:4] = Q_pos_vel
# omega (turn rate uncertainty)
Q[4, 4] = sigma_omega**2 * T**2
else:
n = 4
Q = np.zeros((n, n), dtype=np.float64)
# x, vx block
Q[0:2, 0:2] = Q_pos_vel
# y, vy block
Q[2:4, 2:4] = Q_pos_vel
return Q
[docs]
def q_coord_turn_3d(
T: float,
sigma_a: float,
sigma_omega: float = 0.0,
state_type: str = "position_velocity",
) -> NDArray[np.floating]:
"""
Create process noise covariance for 3D coordinated turn model.
Parameters
----------
T : float
Time step in seconds.
sigma_a : float
Standard deviation of acceleration noise [m/s²].
sigma_omega : float, optional
Standard deviation of turn rate noise [rad/s²].
state_type : str, optional
State vector composition.
Returns
-------
Q : ndarray
Process noise covariance matrix.
Notes
-----
Same construction as :func:`q_coord_turn_2d` (independent per-axis
blocks, interleaved state), verified in v2.11.1: at ``T=1,
sigma_a=2, sigma_omega=0.1`` the 6x6 result is exactly block-diagonal
over the three per-axis 2x2 blocks (largest off-block entry 0.0),
rank 3, and its first block equals ``q_coord_turn_2d``'s. MATLAB has
no 3-D counterpart to compare element-wise, so the divergence from the
rank-1 ``QCoordTurn`` is established through that shared construction
rather than measured directly; see the Notes on ``q_coord_turn_2d``.
See Also
--------
f_coord_turn_3d : State transition matrix for 3D coordinated turn.
"""
var_a = sigma_a**2
# Discrete white noise acceleration model
Q_pos_vel = var_a * np.array(
[
[T**4 / 4, T**3 / 2],
[T**3 / 2, T**2],
],
dtype=np.float64,
)
if state_type == "position_velocity_omega":
n = 7
Q = np.zeros((n, n), dtype=np.float64)
# x, vx block
Q[0:2, 0:2] = Q_pos_vel
# y, vy block
Q[2:4, 2:4] = Q_pos_vel
# z, vz block
Q[4:6, 4:6] = Q_pos_vel
# omega (turn rate uncertainty)
Q[6, 6] = sigma_omega**2 * T**2
else:
n = 6
Q = np.zeros((n, n), dtype=np.float64)
# x, vx block
Q[0:2, 0:2] = Q_pos_vel
# y, vy block
Q[2:4, 2:4] = Q_pos_vel
# z, vz block
Q[4:6, 4:6] = Q_pos_vel
return Q
[docs]
def q_coord_turn_polar(
T: float,
sigma_a: float,
sigma_omega_dot: float,
) -> NDArray[np.floating]:
"""
Create process noise covariance for coordinated turn in polar form.
State vector is [x, y, heading, speed, turn_rate].
Notes
-----
MATLAB parity was **not** verified for this function in v2.11.1. Its
sibling :func:`q_coord_turn_2d` was checked against ``QCoordTurn`` and
diverges structurally (see its Notes); whether this function shares
that divergence is unknown, not established either way. The module's
EXPERIMENTAL maturity reflects that gap as well as the confirmed one.
Parameters
----------
T : float
Time step in seconds.
sigma_a : float
Standard deviation of tangential acceleration [m/s²].
sigma_omega_dot : float
Standard deviation of turn rate derivative [rad/s²].
Returns
-------
Q : ndarray
Process noise covariance matrix of shape (5, 5).
"""
Q = np.zeros((5, 5), dtype=np.float64)
# Position noise (from speed uncertainty)
# This is a simplified model
Q[0, 0] = sigma_a**2 * T**4 / 4
Q[1, 1] = sigma_a**2 * T**4 / 4
# Heading noise (from turn rate uncertainty)
Q[2, 2] = sigma_omega_dot**2 * T**4 / 4
Q[2, 4] = sigma_omega_dot**2 * T**3 / 2
Q[4, 2] = Q[2, 4]
# Speed noise
Q[3, 3] = sigma_a**2 * T**2
# Turn rate noise
Q[4, 4] = sigma_omega_dot**2 * T**2
return Q
__all__ = [
"q_coord_turn_2d",
"q_coord_turn_3d",
"q_coord_turn_polar",
]