Kalman Filter Module Documentation

This module provides functions to work with Kalman filters.

Functions:

Name	Description
kf_lti_discretize	Discretize a Linear Time-Invariant (LTI) system using the matrix fraction decomposition.
kf_predict	Prediction step of the Kalman filter.
kf_update	Update step of the Kalman filter.
kf_smooth	Rauch-Tung-Striebel (RTS) smoother for a linear (or locally linearized) Kalman filter.

Classes:

Name	Description
UUV_EKF	Extended Kalman Filter (EKF) class for uncrewed underwater vehicles.

UUV_EKF(bottom_height=False, covariance_dvl_velocity=0.01, covariance_dvl_bottom_height=0.01, covariance_ahrs_quaternion=0.1, covariance_ahrs_angular_rate=0.1, covariance_depth=0.001, covariance_position=10.0, process_noise_velocity=0.001)

Extended Kalman Filter (EKF) class for uncrewed underwater vehicles. This class implements the Extended Kalman Filter (EKF) algorithm for state estimation of an uncrewed underwater vehicle using AHRS, DVL, Depth sensors, and USBL asynchronous measurements.

State Vector Structure

[t, x, y, z, qx, qy, qz, qw, vx, vy, vz, wx, wy, wz, bh (optional)] - t: Timestamp - x, y, z: Position in the world frame - qx, qy, qz, qw: Quaternion representing orientation - vx, vy, vz: Linear velocity in the world frame - wx, wy, wz: Angular velocity in the body frame - bh: Bottom height (optional, if enabled)

Extended Kalman Filter (EKF) class.

This class implements the Extended Kalman Filter (EKF) algorithm for state estimation of an uncrewed underwater vehicle using AHRS, DVL and Depth sensor asynchronous measurements.

Parameters:

Name	Type	Description	Default
bottom_height	bool	If True, the EKF will estimate the bottom height. If False, the EKF will not estimate the bottom height.	False
covariance_dvl_velocity	Union[float, ndarray, List[float]]	Covariance for DVL velocity measurements. It must be a float or a (3, ) array. If it is a float, it will be used as a scaling factor for the identity matrix. If it is a (3, ) array, it will be used as the diagonal of the covariance matrix.	0.01
covariance_dvl_bottom_height	float	Covariance for DVL bottom height measurements.	0.01
covariance_ahrs_quaternion	float	Covariance for AHRS quaternion measurements. It must be a (3, ) array. If it is a float, it will be used as a scaling factor for the identity matrix. If it is a (3, ) array, it will be used as the diagonal of the covariance matrix. It uses the small angle approximation, where the covariance for qx, qy, qz is approximated with the covariance for roll, pitch and yaw angles.	0.1
covariance_ahrs_angular_rate	Union[float, ndarray, List[float]]	Covariance for AHRS angular rate measurements. It must be a float or a (3, ) array. If it is a float, it will be used as a scaling factor for the identity matrix. If it is a (3, ) array, it will be used as the diagonal of the covariance matrix.	0.1
covariance_depth	float	Covariance for depth measurements.	0.001
covariance_position	Union[float, ndarray, List[float]]	Covariance for position measurements. It must be a float or a (2, ) array. If it is a float, it will be used as a scaling factor for the identity matrix. If it is a (2, ) array, it will be used as the diagonal of the covariance matrix.	10.0
process_noise_velocity	float	Process noise covariance for velocity. This value will scale an identity 3x3 matrix. The default value is 0.001. Guidance to tune this value is in the note below.	0.001

Note

The process noise covariance for velocity is a scaling factor for the identity matrix. This value is used to propagate the velocity, position and bottom height (if enabled) states. An initial guess for this value for a DVL with a typical standard deviation of >=1%. The magnitude for this value can be computed based on a magnitude computation based on the following equation:

\[ Q_{v}^0 \propto \frac{y_{dvl}^2}{n \cdot dt} \]

Where, \(y_{dvl}\) is the average innovation for the DVL, \(n\) is the number predictions between correction steps and \(dt\) is the time step of the predictions.

Source code in navlib/nav/kalman_filter.py

def __init__(
    self,
    bottom_height: bool = False,
    covariance_dvl_velocity: Union[float, np.ndarray, List[float]] = 0.01,
    covariance_dvl_bottom_height: float = 0.01,
    covariance_ahrs_quaternion: Union[float, np.ndarray, List[float]] = 0.1,
    covariance_ahrs_angular_rate: Union[float, np.ndarray, List[float]] = 0.1,
    covariance_depth: float = 0.001,
    covariance_position: Union[float, np.ndarray, List[float]] = 10.0,
    process_noise_velocity: float = 0.001,
) -> None:
    """
    Extended Kalman Filter (EKF) class.

    This class implements the Extended Kalman Filter (EKF) algorithm for state estimation
    of an uncrewed underwater vehicle using AHRS, DVL and Depth sensor asynchronous measurements.

    Args:
        bottom_height (bool): If True, the EKF will estimate the bottom height.
            If False, the EKF will not estimate the bottom height.
        covariance_dvl_velocity (Union[float, np.ndarray, List[float]]): Covariance for DVL velocity measurements.
            It must be a float or a (3, ) array. If it is a float, it will be used as a scaling factor for the
            identity matrix. If it is a (3, ) array, it will be used as the diagonal of the covariance matrix.
        covariance_dvl_bottom_height (float): Covariance for DVL bottom height measurements.
        covariance_ahrs_quaternion (float): Covariance for AHRS quaternion measurements. It must be a (3, ) array.
            If it is a float, it will be used as a scaling factor for the identity matrix. If it is a (3, ) array,
            it will be used as the diagonal of the covariance matrix. It uses the small angle approximation, where
            the covariance for qx, qy, qz is approximated with the covariance for roll, pitch and yaw angles.
        covariance_ahrs_angular_rate (Union[float, np.ndarray, List[float]]): Covariance for AHRS angular rate measurements.
            It must be a float or a (3, ) array. If it is a float, it will be used as a scaling factor for the
            identity matrix. If it is a (3, ) array, it will be used as the diagonal of the covariance matrix.
        covariance_depth (float): Covariance for depth measurements.
        covariance_position (Union[float, np.ndarray, List[float]]): Covariance for position measurements.
            It must be a float or a (2, ) array. If it is a float, it will be used as a scaling factor for the
            identity matrix. If it is a (2, ) array, it will be used as the diagonal of the covariance matrix.
        process_noise_velocity (float): Process noise covariance for velocity. This value will scale an identity 3x3
            matrix. The default value is 0.001. Guidance to tune this value is in the note below.

    Note:
        The process noise covariance for velocity is a scaling factor for the identity matrix. This value is used to
        propagate the velocity, position and bottom height (if enabled) states. An initial guess for this value for
        a DVL with a typical standard deviation of >=1%. The magnitude for this value can be computed based on a magnitude
        computation based on the following equation:

        $$ Q_{v}^0 \\propto \\frac{y_{dvl}^2}{n \\cdot dt} $$

        Where, $y_{dvl}$ is the average innovation for the DVL, $n$ is the number predictions between correction steps
        and $dt$ is the time step of the predictions.
    """
    # Convert to a numpy arrays if they are lists
    if isinstance(covariance_dvl_velocity, list):
        covariance_dvl_velocity = np.array(covariance_dvl_velocity)
    if isinstance(covariance_ahrs_quaternion, list):
        covariance_ahrs_quaternion = np.array(covariance_ahrs_quaternion)
    if isinstance(covariance_ahrs_angular_rate, list):
        covariance_ahrs_angular_rate = np.array(covariance_ahrs_angular_rate)
    if isinstance(covariance_position, list):
        covariance_position = np.array(covariance_position)

    # Types checking
    if not isinstance(bottom_height, bool):
        raise TypeError("bottom_height must be a boolean")
    if not isinstance(covariance_dvl_velocity, (float, np.ndarray, list)):
        raise TypeError("covariance_dvl_velocity must be a float or a (3, ) array")
    if not isinstance(covariance_dvl_velocity, float):
        covariance_dvl_velocity = covariance_dvl_velocity.squeeze()
        if covariance_dvl_velocity.ndim != 1:
            raise ValueError("covariance_dvl_velocity must be a (3, ) array")
        if covariance_dvl_velocity.shape[0] != 3:
            raise ValueError("covariance_dvl_velocity must be a (3, ) array")
    if not isinstance(covariance_dvl_bottom_height, float):
        raise TypeError("covariance_dvl_bottom_height must be a float")
    if not isinstance(covariance_ahrs_quaternion, (float, np.ndarray)):
        raise TypeError("covariance_ahrs_quaternion must be a float or a (3, ) array")
    if not isinstance(covariance_ahrs_angular_rate, (float, np.ndarray)):
        raise TypeError("covariance_ahrs_angular_rate must be a float or a (3, ) array")
    if not isinstance(covariance_ahrs_angular_rate, float):
        covariance_ahrs_angular_rate = covariance_ahrs_angular_rate.squeeze()
        if covariance_ahrs_angular_rate.ndim != 1:
            raise ValueError("covariance_ahrs_angular_rate must be a (3, ) array")
        if covariance_ahrs_angular_rate.shape[0] != 3:
            raise ValueError("covariance_ahrs_angular_rate must be a (3, ) array")
    if not isinstance(covariance_ahrs_quaternion, float):
        covariance_ahrs_quaternion = covariance_ahrs_quaternion.squeeze()
        if covariance_ahrs_quaternion.ndim != 1:
            raise ValueError("covariance_ahrs_quaternion must be a (3, ) array")
        if covariance_ahrs_quaternion.shape[0] != 3:
            raise ValueError("covariance_ahrs_quaternion must be a (3, ) array")
    if not isinstance(covariance_depth, float):
        raise TypeError("covariance_depth must be a float")
    if not isinstance(covariance_position, (float, np.ndarray)):
        raise TypeError("covariance_position must be a float or a (2, ) array")
    if not isinstance(covariance_position, float):
        covariance_position = covariance_position.squeeze()
        if covariance_position.ndim != 1:
            raise ValueError("covariance_position must be a (2, ) array")
        if covariance_position.shape[0] != 2:
            raise ValueError("covariance_position must be a (2, ) array")

    # State Vector: [t, x, y, z, qx, qy, qz, qw, vx, vy, vz, wx, wy, wz, bh (optional)]
    self._current_state = np.r_[np.zeros(7), np.ones(1), np.zeros(6)]
    if bottom_height:
        self._current_state = np.r_[self._current_state, np.zeros(1)]
    self._state_dim = self._current_state.shape[0] - 1  # state dim doesn't include time

    # Process noise covariance
    self._q0_velocity = process_noise_velocity * np.eye(3, dtype=np.float64)

    # Measurements
    # XY position - Measurement model matrix
    H_position = np.zeros((2, self.state_dim))
    H_position[:, :2] = np.eye(2, dtype=np.float64)
    self._H_position = H_position
    # XY position - Measurement noise covariance
    if isinstance(covariance_position, float):
        self._R_position = np.eye(2, dtype=np.float64) * covariance_position
    else:
        self._R_position = np.diag(covariance_position)
    # Depth - Measurement model matrix
    H_depth = np.zeros((1, self.state_dim))
    H_depth[0, 2] = 1
    self._H_depth = H_depth
    # Depth - Measurement noise covariance
    self._R_depth = np.eye(1, dtype=np.float64) * covariance_depth
    # Attitude - Measurement model matrix
    H_attitude = np.zeros((3, self.state_dim))
    H_attitude[:, 3:6] = np.eye(3, dtype=np.float64)
    self._H_attitude = H_attitude
    # Attitude - Measurement noise covariance
    # Quaternion covariance: we will use the small angle approximation;
    # therefore, we will use the covariance of roll, pitch and yaw angles
    # and leave the scalar component as 0.0.
    self._R_attitude = np.zeros((3, 3), dtype=np.float64)
    if isinstance(covariance_ahrs_quaternion, float):
        self._R_attitude = np.eye(3, dtype=np.float64) * covariance_ahrs_quaternion
    else:
        self._R_attitude = np.diag(covariance_ahrs_quaternion)
    # Angular rates - Measurement model matrix
    H_angrate = np.zeros((3, self.state_dim))
    H_angrate[:, 10:13] = np.eye(3, dtype=np.float64)
    self._H_angrate = H_angrate
    # Angular rate - Measurement noise covariance
    self._R_angrate = np.zeros((3, 3), dtype=np.float64)
    if isinstance(covariance_ahrs_angular_rate, float):
        self._R_angrate = np.eye(3, dtype=np.float64) * covariance_ahrs_angular_rate
    else:
        self._R_angrate = np.diag(covariance_ahrs_angular_rate)
    # Linear velocity - Measurement model matrix
    H_vel = np.zeros((3, self.state_dim))
    H_vel[:, 7:10] = np.eye(3, dtype=np.float64)
    self._H_vel = H_vel
    # Linear velocity - Measurement noise covariance
    if isinstance(covariance_dvl_velocity, float):
        self._R_vel = np.eye(3, dtype=np.float64) * covariance_dvl_velocity
    else:
        self._R_vel = np.diag(covariance_dvl_velocity)
    # Bottom height - Measurement model matrix
    H_bottom_height = np.zeros((1, self.state_dim))
    H_bottom_height[0, -1] = 1
    self._H_bottom_height = H_bottom_height
    # Bottom height - Measurement noise covariance
    self._R_bottom_height = np.eye(1, dtype=np.float64) * covariance_dvl_bottom_height

    # Velocity Filtering
    self._velocity_d_squared_acummulted = 0.0
    self._velocity_d_squared_count = 0
    self._velocity_q_tuner = _QTuner()

    # Maximum time step considered stable for EKF prediction
    self._max_stable_time_step = 5.0

    # Since initialize_state() copies every component directly from one raw
    # sensor sample, the principled prior for each is that sensor's own
    # measurement covariance R.
    self._current_covariance = self._initial_state_covariance()

H_angrate property

Get the measurement model matrix for angular rates.

Returns:

Name	Type	Description
H_angrate	ndarray	Measurement model matrix for angular rates.

H_attitude property

Get the measurement model matrix for attitude.

Returns:

Name	Type	Description
H_attitude	ndarray	Measurement model matrix for attitude.

H_bottom_height property

Get the measurement model matrix for bottom height.

Returns:

Name	Type	Description
H_bottom_height	ndarray	Measurement model matrix for bottom height.

H_depth property

Get the measurement model matrix for depth.

Returns:

Name	Type	Description
H_depth	ndarray	Measurement model matrix for depth.

H_position property

Get the measurement model matrix for cartesian position.

Returns:

Name	Type	Description
H_position	ndarray	Measurement model matrix for cartesian position.

H_vel property

Get the measurement model matrix for linear velocity.

Returns:

Name	Type	Description
H_vel	ndarray	Measurement model matrix for linear velocity.

R_angrate property

Get the measurement noise covariance for angular rates.

Returns:

Name	Type	Description
R_angrate	ndarray	Measurement noise covariance for angular rates.

R_attitude property

Get the measurement noise covariance for attitude.

Returns:

Name	Type	Description
R_attitude	ndarray	Measurement noise covariance for attitude.

R_bottom_height property

Get the measurement noise covariance for bottom height.

Returns:

Name	Type	Description
R_bottom_height	ndarray	Measurement noise covariance for bottom height.

R_depth property

Get the measurement noise covariance for depth.

Returns:

Name	Type	Description
R_depth	ndarray	Measurement noise covariance for depth.

R_position property

Get the measurement noise covariance for cartesian position.

Returns:

Name	Type	Description
R_position	ndarray	Measurement noise covariance for cartesian position.

R_vel property

Get the measurement noise covariance for linear velocity.

Returns:

Name	Type	Description
R_vel	ndarray	Measurement noise covariance for linear velocity.

current_covariance property

Get the current covariance (\(P_t\)) of the EKF.

Returns:

Name	Type	Description
current_covariance	ndarray	Current covariance matrix.

current_state property

Get the current state (\(x_t\)) of the EKF.

Returns:

Name	Type	Description
current_state	ndarray	Current state vector.

state_dim property

Get the dimension of the state vector.

Returns:

Name	Type	Description
state_dim	int	Dimension of the state vector.

correction_angular_rate(angular_velocity, measurement_covariance=None)

Correct the state of the EKF using angular velocity measurements.

Parameters:

Name	Type	Description	Default
angular_velocity	Union[List[Union[int, float]], ndarray]	Angular velocity measurement.	required
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Source code in navlib/nav/kalman_filter.py

def correction_angular_rate(
    self,
    angular_velocity: Union[List[Union[int, float]], np.ndarray],
    measurement_covariance: np.ndarray = None,
) -> None:
    """
    Correct the state of the EKF using angular velocity measurements.

    Args:
        angular_velocity (Union[List[Union[int, float]], np.ndarray]): Angular velocity measurement.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.
    """
    # Convert to numpy array
    if isinstance(angular_velocity, list):
        angular_velocity = np.array(angular_velocity)

    # Check inputs type
    if not isinstance(angular_velocity, np.ndarray):
        raise TypeError("Angular velocity must be a numpy array")
    angular_velocity = angular_velocity.squeeze()
    if angular_velocity.ndim != 1:
        raise ValueError("Angular velocity must be a 1D array")
    if angular_velocity.shape[0] != 3:
        raise ValueError("Angular velocity must have 3 elements")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Measurement (z, shape=(3, 1))
    z = np.vstack(angular_velocity)

    # Innovation or residual (y, shape=(3, 1))
    y = z - self._H_angrate @ np.vstack(self._current_state[1:])

    # Measurement noise covariance (R, shape=(3, 3))
    if measurement_covariance is None:
        R = self._R_angrate
    else:
        if measurement_covariance.shape[0] != z.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # Measurement prediction covariance (S, shape=(3, 3))
    S = self._H_angrate @ self._current_covariance @ self._H_angrate.T + R
    S_inv = np.linalg.inv(S)

    # Kalman gain (K, shape=(n, 3))
    K = self._current_covariance @ self._H_angrate.T @ S_inv

    # Update state estimate (x, shape=(n, 1))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (
        np.eye(self._state_dim, dtype=np.float64) - K @ self._H_angrate
    ) @ self._current_covariance
    self._symmetrize_covariance()

correction_attitude(quaternion, measurement_covariance=None)

Correct the state of the EKF using orientation measurements.

Parameters:

Name	Type	Description	Default
quaternion	Union[List[Union[int, float]], ndarray]	Quaternion measurement. Order: (x, y, z, w).	required
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Source code in navlib/nav/kalman_filter.py

def correction_attitude(
    self,
    quaternion: Union[List[Union[int, float]], np.ndarray],
    measurement_covariance: np.ndarray = None,
) -> None:
    """
    Correct the state of the EKF using orientation measurements.

    Args:
        quaternion (Union[List[Union[int, float]], np.ndarray]): Quaternion measurement. Order: (x, y, z, w).
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.
    """
    # Convert to numpy array
    if isinstance(quaternion, list):
        quaternion = np.array(quaternion)

    # Check inputs type
    if not isinstance(quaternion, np.ndarray):
        raise TypeError("Quaternion must be a numpy array")
    quaternion = quaternion.squeeze()
    if quaternion.ndim != 1:
        raise ValueError("Quaternion must be a 1D array")
    if quaternion.shape[0] != 4:
        raise ValueError("Quaternion must have 4 elements")
    if np.abs(norm(quaternion) - 1.0) > 1e-6:
        raise ValueError("Quaternion must be a unit quaternion")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Force the input quaternion to be unit and with a positive scalar part
    quaternion /= norm(quaternion)
    if quaternion[3] < 0:
        quaternion *= -1

    # Use relative quaternion error and extract small angle error
    q_est = self._current_state[4:8]
    q_meas = quaternion

    # Quaternion residual
    delta_q = left_qmatrix(quat_conj(q_est)) @ q_meas

    # delta_q[:3] ~= (phi/2)*axis for a small-angle rotation phi -- the same
    # half-angle scaling as the state's own qx,qy,qz slots (H_attitude = I
    # asserts these are 1:1 in units). Do not scale this to the full angle
    # (phi*axis): that would double every correction relative to what the
    # half-angle-scaled state slots should actually move by.

    # Innovation or residual (y, shape=(3, 1))
    y = np.vstack(delta_q[:3])

    # Measurement noise covariance (R, shape=(3, 3))
    if measurement_covariance is None:
        R = self._R_attitude
    else:
        if measurement_covariance.shape[0] != y.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the innovation")
        R = measurement_covariance

    # Measurement prediction covariance (S, shape=(4, 4))
    S = self._H_attitude @ self._current_covariance @ self._H_attitude.T + R
    S_inv = np.linalg.inv(S)

    # Kalman gain (K, shape=(n, 4))
    K = self._current_covariance @ self._H_attitude.T @ S_inv

    # Update state estimate (x, shape=(n, 1))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (
        np.eye(self._state_dim, dtype=np.float64) - K @ self._H_attitude
    ) @ self._current_covariance
    self._symmetrize_covariance()

correction_bottom_height(bottom_height, scaling_strength=1.0, threshold=0.99, measurement_covariance=None)

Correct the state of the EKF using bottom height measurements.

Parameters:

Name	Type	Description	Default
bottom_height	Union[float, int]	Bottom height measurement.	required
scaling_strength	Union[float, int]	Scaling strength for the adaptive measurement noise covariance. scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))	1.0
threshold	float	Threshold for the adaptive measurement noise covariance indicating the confidence level. The default value is 0.99, which corresponds to a 99% confidence level.	0.99
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Returns:

Name	Type	Description
d_squared	float	Mahalanobis distance squared.
success	bool	True if the correction was used, False if the correction was scaled.

Raises:

Type	Description
TypeError	If bottom_height, scaling_strength or threshold are not of the expected type.
ValueError	If threshold is not between 0.0 and 1.0.

Source code in navlib/nav/kalman_filter.py

def correction_bottom_height(
    self,
    bottom_height: Union[float, int],
    scaling_strength: Union[float, int] = 1.0,
    threshold: float = 0.99,
    measurement_covariance: np.ndarray = None,
) -> Tuple[float, bool]:
    """
    Correct the state of the EKF using bottom height measurements.

    Args:
        bottom_height (Union[float, int]): Bottom height measurement.
        scaling_strength (Union[float, int]): Scaling strength for the adaptive measurement noise covariance.
            scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))
        threshold (float): Threshold for the adaptive measurement noise covariance indicating the confidence level.
            The default value is 0.99, which corresponds to a 99% confidence level.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.

    Returns:
        d_squared (float): Mahalanobis distance squared.
        success (bool): True if the correction was used, False if the correction was scaled.

    Raises:
        TypeError: If bottom_height, scaling_strength or threshold are not of the expected type.
        ValueError: If threshold is not between 0.0 and 1.0.
    """
    # Check typing
    if not isinstance(bottom_height, (float, int)):
        raise TypeError("Bottom height must be a float or an int")
    if not isinstance(scaling_strength, (float, int)):
        raise TypeError("Scaling strength must be a float or an int")
    if not isinstance(threshold, (float, int)):
        raise TypeError("Threshold must be a float or an int")
    if threshold <= 0.0 or threshold >= 1.0:
        raise ValueError("Threshold must be between 0.0 and 1.0")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    if self._state_dim != 14:
        return np.nan, False

    # Measurement (z, shape=(1, 1))
    z = np.array([[bottom_height]])

    # Measurement covariance
    if measurement_covariance is None:
        R = self._R_bottom_height
    else:
        if measurement_covariance.shape[0] != z.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # H matrix
    H = self._H_bottom_height

    # Innovation or residual (y, shape=(1, 1))
    y = z - H @ np.vstack(self._current_state[1:])

    # Innovation covariance
    S = H @ self._current_covariance @ H.T + R

    # Mahalanobis distance-based adaptive covariance matrix scaling
    S_inv, d_squared, scaled = self._adaptive_covariance(y, S, R, threshold, scaling_strength)

    # Kalman gain (K, shape=(n, 1))
    K = self._current_covariance @ H.T @ S_inv

    # Update state estimate (x, shape=(n, 1) or (n, 4))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (np.eye(self._state_dim, dtype=np.float64) - K @ H) @ self._current_covariance
    self._symmetrize_covariance()

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    return d_squared, scaled

correction_depth(z, scaling_strength=1.0, threshold=0.99, measurement_covariance=None)

Correct the state of the EKF using depth measurements.

Parameters:

Name	Type	Description	Default
z	Union[float, int]	Depth measurement.	required
scaling_strength	Union[float, int]	Scaling strength for the adaptive measurement noise covariance. scale_factor (Union[float, int]) = np.exp(scaling_strength * (d_squared / threshold - 1.0))	1.0
threshold	float	Threshold for the adaptive measurement noise covariance indicating the confidence level. The default value is 0.99, which corresponds to a 99% confidence level.	0.99
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Returns:

Name	Type	Description
d_squared	float	Mahalanobis distance squared.
success	bool	True if the correction was used, False if the correction was scaled.

Raises:

Type	Description
TypeError	If timestamp, z, scaling_strength or threshold are not of the expected type.
ValueError	If threshold is not between 0.0 and 1.0.

Source code in navlib/nav/kalman_filter.py

def correction_depth(
    self,
    z: Union[float, int],
    scaling_strength: Union[float, int] = 1.0,
    threshold: float = 0.99,
    measurement_covariance: np.ndarray = None,
) -> Tuple[float, bool]:
    """
    Correct the state of the EKF using depth measurements.

    Args:
        z (Union[float, int]): Depth measurement.
        scaling_strength (Union[float, int]): Scaling strength for the adaptive measurement noise covariance.
            scale_factor (Union[float, int]) = np.exp(scaling_strength * (d_squared / threshold - 1.0))
        threshold (float): Threshold for the adaptive measurement noise covariance indicating the confidence level.
            The default value is 0.99, which corresponds to a 99% confidence level.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.

    Returns:
        d_squared (float): Mahalanobis distance squared.
        success (bool): True if the correction was used, False if the correction was scaled.

    Raises:
        TypeError: If timestamp, z, scaling_strength or threshold are not of the expected type.
        ValueError: If threshold is not between 0.0 and 1.0.
    """
    # Check typing
    if not isinstance(z, (float, int)):
        raise TypeError("Z position must be a float or an int")
    if not isinstance(scaling_strength, (float, int)):
        raise TypeError("Scaling strength must be a float or an int")
    if not isinstance(threshold, (float, int)):
        raise TypeError("Threshold must be a float or an int")
    if threshold <= 0.0 or threshold >= 1.0:
        raise ValueError("Threshold must be between 0.0 and 1.0")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Measurement (z, shape=(1, 1))
    z = np.array([[z]])

    # Measurement covariance
    if measurement_covariance is None:
        R = self._R_depth
    else:
        if measurement_covariance.shape[0] != z.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # H matrix
    H = self._H_depth

    # Innovation or residual (y, shape=(1, 1))
    y = z - H @ np.vstack(self._current_state[1:])

    # Innovation covariance
    S = H @ self._current_covariance @ H.T + R

    # Mahalanobis distance-based adaptive covariance matrix scaling
    S_inv, d_squared, scaled = self._adaptive_covariance(y, S, R, threshold, scaling_strength)

    # Kalman gain (K, shape=(n, 1))
    K = self._current_covariance @ H.T @ S_inv

    # Update state estimate (x, shape=(n, 1))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (np.eye(self._state_dim, dtype=np.float64) - K @ H) @ self._current_covariance
    self._symmetrize_covariance()

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    return d_squared, scaled

correction_position_xy(x, y, scaling_strength=1.0, threshold=0.99, measurement_covariance=None)

Correct the state of the EKF using position measurements.

Parameters:

Name	Type	Description	Default
x	Union[float, int]	X position measurement.	required
y	Union[float, int]	Y position measurement.	required
scaling_strength	Union[float, int]	Scaling strength for the adaptive measurement noise covariance. scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))	1.0
threshold	float	Threshold for the adaptive measurement noise covariance indicating the confidence level. The default value is 0.99, which corresponds to a 99% confidence level. This ensures that measurements falling within the 99% confidence region are trusted, while outliers are adaptively scaled.	0.99
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Returns:

Name	Type	Description
d_squared	float	Mahalanobis distance squared.
success	bool	True if the correction was used, False if the correction was scaled.

Raises:

Type	Description
TypeError	If timestamp, x, y, scaling_strength or threshold are not of the expected type.
ValueError	If threshold is not between 0.0 and 1.0.

Source code in navlib/nav/kalman_filter.py

def correction_position_xy(
    self,
    x: Union[float, int],
    y: Union[float, int],
    scaling_strength: Union[float, int] = 1.0,
    threshold: float = 0.99,
    measurement_covariance: np.ndarray = None,
) -> Tuple[float, bool]:
    """
    Correct the state of the EKF using position measurements.

    Args:
        x (Union[float, int]): X position measurement.
        y (Union[float, int]): Y position measurement.
        scaling_strength (Union[float, int]): Scaling strength for the adaptive measurement noise covariance.
            scale_factor =  np.exp(scaling_strength * (d_squared / threshold - 1.0))
        threshold (float): Threshold for the adaptive measurement noise covariance indicating the confidence level.
            The default value is 0.99, which corresponds to a 99% confidence level. This ensures that measurements
            falling within the 99% confidence region are trusted, while outliers are adaptively scaled.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.

    Returns:
        d_squared (float): Mahalanobis distance squared.
        success (bool): True if the correction was used, False if the correction was scaled.

    Raises:
        TypeError: If timestamp, x, y, scaling_strength or threshold are not of the expected type.
        ValueError: If threshold is not between 0.0 and 1.0.
    """
    # Check typing
    if not isinstance(x, (float, int)):
        raise TypeError("X position must be a float or an int")
    if not isinstance(y, (float, int)):
        raise TypeError("Y position must be a float or an int")
    if not isinstance(scaling_strength, (float, int)):
        raise TypeError("Scaling strength must be a float or an int")
    if not isinstance(threshold, (float, int)):
        raise TypeError("Threshold must be a float or an int")
    if threshold <= 0.0 or threshold >= 1.0:
        raise ValueError("Threshold must be between 0.0 and 1.0")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Measurement (z, shape=(2, 1))
    z = np.array([[x], [y]])

    # Measurement covariance
    if measurement_covariance is None:
        R = self._R_position
    else:
        if measurement_covariance.shape[0] != z.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # H matrix
    H = self._H_position

    # Innovation or residual (y, shape=(2, 1))
    y = z - H @ np.vstack(self._current_state[1:])
    if norm(y) > 100.0:
        warn("Position correction: large innovation (> 100 m), skipping correction")
        return np.nan, True

    # Innovation covariance
    S = H @ self._current_covariance @ H.T + R

    # Mahalanobis distance-based adaptive covariance matrix scaling
    S_inv, d_squared, scaled = self._adaptive_covariance(y, S, R, threshold, scaling_strength)

    # Kalman gain (K, shape=(n, 2))
    K = self._current_covariance @ H.T @ S_inv

    # Update state estimate (x, shape=(n, 2))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (np.eye(self._state_dim, dtype=np.float64) - K @ H) @ self._current_covariance
    self._symmetrize_covariance()

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    return d_squared, scaled

correction_position_xyz(x, y, z, scaling_strength=1.0, threshold=0.99, measurement_covariance=None)

Correct the state of the EKF using position measurements.

Parameters:

Name	Type	Description	Default
x	Union[float, int]	X position measurement.	required
y	Union[float, int]	Y position measurement.	required
z	Union[float, int]	Z position measurement.	required
scaling_strength	Union[float, int]	Scaling strength for the adaptive measurement noise covariance. scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))	1.0
threshold	float	Threshold for the adaptive measurement noise covariance indicating the confidence level. The default value is 0.99, which corresponds to a 99% confidence level. This ensures that measurements falling within the 99% confidence region are trusted, while outliers are adaptively scaled.	0.99
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Returns:

Name	Type	Description
d_squared	float	Mahalanobis distance squared.
success	bool	True if the correction was used, False if the correction was scaled.

Raises:

Type	Description
TypeError	If timestamp, x, y, z, scaling_strength or threshold are not of the expected type.
ValueError	If threshold is not between 0.0 and 1.0.

Source code in navlib/nav/kalman_filter.py

def correction_position_xyz(
    self,
    x: Union[float, int],
    y: Union[float, int],
    z: Union[float, int],
    scaling_strength: Union[float, int] = 1.0,
    threshold: float = 0.99,
    measurement_covariance: np.ndarray = None,
) -> Tuple[float, bool]:
    """
    Correct the state of the EKF using position measurements.

    Args:
        x (Union[float, int]): X position measurement.
        y (Union[float, int]): Y position measurement.
        z (Union[float, int]): Z position measurement.
        scaling_strength (Union[float, int]): Scaling strength for the adaptive measurement noise covariance.
            scale_factor =  np.exp(scaling_strength * (d_squared / threshold - 1.0))
        threshold (float): Threshold for the adaptive measurement noise covariance indicating the confidence level.
            The default value is 0.99, which corresponds to a 99% confidence level. This ensures that measurements
            falling within the 99% confidence region are trusted, while outliers are adaptively scaled.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.

    Returns:
        d_squared (float): Mahalanobis distance squared.
        success (bool): True if the correction was used, False if the correction was scaled.

    Raises:
        TypeError: If timestamp, x, y, z, scaling_strength or threshold are not of the expected type.
        ValueError: If threshold is not between 0.0 and 1.0.
    """
    # Check typing
    if not isinstance(x, (float, int)):
        raise TypeError("X position must be a float or an int")
    if not isinstance(y, (float, int)):
        raise TypeError("Y position must be a float or an int")
    if not isinstance(z, (float, int)):
        raise TypeError("Z position must be a float or an int")
    if not isinstance(scaling_strength, (float, int)):
        raise TypeError("Scaling strength must be a float or an int")
    if not isinstance(threshold, (float, int)):
        raise TypeError("Threshold must be a float or an int")
    if threshold <= 0.0 or threshold >= 1.0:
        raise ValueError("Threshold must be between 0.0 and 1.0")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Measurement (z, shape=(3, 1))
    z = np.array([[x], [y], [z]])

    # Measurement covariance
    if measurement_covariance is None:
        R = np.eye(3, dtype=np.float64)
        R[:2, :2] = self._R_position
        R[2, 2] = self._R_depth[0, 0]
    else:
        if measurement_covariance.shape[0] != z.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # H matrix
    H = np.concatenate([self._H_position, self._H_depth], axis=0)

    # Innovation or residual (y, shape=(3, 1))
    y = z - H @ np.vstack(self._current_state[1:])
    if norm(y) > 100.0:
        warn("Position correction: large innovation (> 100 m), skipping correction")
        return np.nan, True

    # Innovation covariance
    S = H @ self._current_covariance @ H.T + R

    # Mahalanobis distance-based adaptive covariance matrix scaling
    S_inv, d_squared, scaled = self._adaptive_covariance(y, S, R, threshold, scaling_strength)

    # Kalman gain (K, shape=(n, 3))
    K = self._current_covariance @ H.T @ S_inv

    # Update state estimate (x, shape=(n, 3))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (np.eye(self._state_dim, dtype=np.float64) - K @ H) @ self._current_covariance
    self._symmetrize_covariance()

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    return d_squared, scaled

correction_velocity(velocity, body_frame=True, scaling_strength=1.0, threshold=0.99, measurement_covariance=None)

Correct the state of the EKF using velocity measurements.

Parameters:

Name	Type	Description	Default
velocity	Union[List[Union[int, float]], ndarray]	Velocity measurement. Order: (vx, vy, vz).	required
body_frame	bool	If True, the velocity is in body frame, otherwise in world frame.	True
scaling_strength	Union[float, int]	Scaling strength for the adaptive measurement noise covariance. scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))	1.0
threshold	float	Threshold for the adaptive measurement noise covariance indicating the confidence level. The default value is 0.99, which corresponds to a 99% confidence level.	0.99
measurement_covariance	ndarray	Optional custom measurement covariance matrix. If this is provided, it will override the default measurement noise covariance provided in the constructor.	None

Returns:

Name	Type	Description
d_squared	float	Mahalanobis distance squared.
success	bool	True if the correction was used, False if the correction was scaled.

Raises:

Type	Description
TypeError	If timestamp, velocity, body_frame, scaling_strength or threshold are not of the expected type.
ValueError	If threshold is not between 0.0 and 1.0.

Source code in navlib/nav/kalman_filter.py

def correction_velocity(
    self,
    velocity: Union[List[Union[int, float]], np.ndarray],
    body_frame: bool = True,
    scaling_strength: Union[float, int] = 1.0,
    threshold: float = 0.99,
    measurement_covariance: np.ndarray = None,
) -> Tuple[float, bool]:
    """
    Correct the state of the EKF using velocity measurements.

    Args:
        velocity (Union[List[Union[int, float]], np.ndarray]): Velocity measurement. Order: (vx, vy, vz).
        body_frame (bool): If True, the velocity is in body frame, otherwise in world frame.
        scaling_strength (Union[float, int]): Scaling strength for the adaptive measurement noise covariance.
            scale_factor = np.exp(scaling_strength * (d_squared / threshold - 1.0))
        threshold (float): Threshold for the adaptive measurement noise covariance indicating the confidence level.
            The default value is 0.99, which corresponds to a 99% confidence level.
        measurement_covariance (np.ndarray): Optional custom measurement covariance matrix. If this is provided,
            it will override the default measurement noise covariance provided in the constructor.

    Returns:
        d_squared (float): Mahalanobis distance squared.
        success (bool): True if the correction was used, False if the correction was scaled.

    Raises:
        TypeError: If timestamp, velocity, body_frame, scaling_strength or threshold are not of the expected type.
        ValueError: If threshold is not between 0.0 and 1.0.
    """
    # Convert to numpy array
    if isinstance(velocity, list):
        velocity = np.array(velocity)

    # Check inputs type
    if not isinstance(velocity, np.ndarray):
        raise TypeError("Velocity must be a numpy array")
    velocity = velocity.squeeze()
    if velocity.ndim != 1:
        raise ValueError("Velocity must be a 1D array")
    if velocity.shape[0] != 3:
        raise ValueError("Velocity must have 3 elements")
    if not isinstance(body_frame, bool):
        raise TypeError("body_frame must be a boolean")
    if not isinstance(scaling_strength, (float, int)):
        raise TypeError("Scaling strength must be a float or an int")
    if not isinstance(threshold, (float, int)):
        raise TypeError("Threshold must be a float or an int")
    if threshold <= 0.0 or threshold >= 1.0:
        raise ValueError("Threshold must be between 0.0 and 1.0")
    if measurement_covariance is not None:
        if not isinstance(measurement_covariance, np.ndarray):
            raise TypeError("Measurement covariance must be a numpy array")
        if measurement_covariance.ndim != 2:
            raise ValueError("Measurement covariance must be a 2D matrix")
        if measurement_covariance.shape[0] != measurement_covariance.shape[1]:
            raise ValueError("Measurement covariance must be a square matrix")

    # Extract velocity from current state
    v_world = np.vstack(self._current_state[8:11])

    if body_frame:
        # Rotate into body frame to define the predicted DVL measurement
        q = self._current_state[4:8]
        rot_v_w = quat2rot(q).T
        z_pred = rot_v_w @ v_world

        # Innovation
        y = np.vstack(velocity) - z_pred

        # H matrix
        H = np.zeros((3, self._state_dim))
        H[:, 7:10] = rot_v_w
    else:
        # Innovation
        y = np.vstack(velocity) - v_world

        # H matrix
        H = np.zeros((3, self._state_dim))
        H[:, 7:10] = np.eye(3, dtype=np.float64)

    # Measurement noise covariance
    if measurement_covariance is None:
        R = self._R_vel
    else:
        if measurement_covariance.shape[0] != y.shape[0]:
            raise ValueError("Measurement covariance must have the same number of rows as the measurement")
        R = measurement_covariance

    # Innovation covariance
    S = H @ self._current_covariance @ H.T + R

    # Mahalanobis distance-based adaptive covariance matrix scaling
    S_inv, d_squared, scaled = self._adaptive_covariance(y, S, R, threshold, scaling_strength)

    # Adaptive Q tuner
    self._velocity_d_squared_acummulted += d_squared
    self._velocity_d_squared_count += 1
    self._velocity_q_tuner.update(self._velocity_d_squared_acummulted / self._velocity_d_squared_count)

    # Kalman gain (K, shape=(n, 3) or (n, 4))
    K = self._current_covariance @ H.T @ S_inv

    # Update state estimate (x, shape=(n, 1) or (n, 4))
    self._current_state[1:] = np.squeeze(np.vstack(self._current_state[1:]) + K @ y)

    # Update covariance estimate (P, shape=(n, n))
    self._current_covariance = (np.eye(self._state_dim, dtype=np.float64) - K @ H) @ self._current_covariance
    self._symmetrize_covariance()

    # Normalize quaternion and force it to have positive scalar part
    self._check_state_attitude()

    return d_squared, scaled

initialize_state(timestamp, position, quaternion, velocity, angular_velocity, bottom_height=0.0)

Initialize the state of the EKF.

Parameters:

Name	Type	Description	Default
timestamp	float	Timestamp of the measurement.	required
position	Union[List[Union[int, float]], ndarray]	Initial position of the vehicle.	required
quaternion	Union[List[Union[int, float]], ndarray]	Initial quaternion of the vehicle. Order: (x, y, z, w).	required
velocity	Union[List[Union[int, float]], ndarray]	Initial velocity of the vehicle.	required
angular_velocity	Union[List[Union[int, float]], ndarray]	Initial angular velocity of the vehicle.	required
bottom_height	Union[float, int]	Initial bottom height of the vehicle. Default is 0.0.	0.0

Source code in navlib/nav/kalman_filter.py

def initialize_state(
    self,
    timestamp: Union[float, int],
    position: Union[List[Union[int, float]], np.ndarray],
    quaternion: Union[List[Union[int, float]], np.ndarray],
    velocity: Union[List[Union[int, float]], np.ndarray],
    angular_velocity: Union[List[Union[int, float]], np.ndarray],
    bottom_height: Union[float, int] = 0.0,
) -> None:
    """
    Initialize the state of the EKF.

    Args:
        timestamp (float): Timestamp of the measurement.
        position (Union[List[Union[int, float]], np.ndarray]): Initial position of the vehicle.
        quaternion (Union[List[Union[int, float]], np.ndarray]): Initial quaternion of the vehicle. Order: (x, y, z, w).
        velocity (Union[List[Union[int, float]], np.ndarray]): Initial velocity of the vehicle.
        angular_velocity (Union[List[Union[int, float]], np.ndarray]): Initial angular velocity of the vehicle.
        bottom_height (Union[float, int]): Initial bottom height of the vehicle. Default is 0.0.
    """
    # Convert to numpy array
    if isinstance(position, list):
        position = np.array(position)
    if isinstance(quaternion, list):
        quaternion = np.array(quaternion)
    if isinstance(velocity, list):
        velocity = np.array(velocity)
    if isinstance(angular_velocity, list):
        angular_velocity = np.array(angular_velocity)

    # Check inputs type
    if not isinstance(timestamp, (float, int)):
        raise TypeError("Timestamp must be a float or an int")
    if not isinstance(position, np.ndarray):
        raise TypeError("Position must be a numpy array")
    position = position.squeeze()
    if position.ndim != 1:
        raise ValueError("Position must be a 1D array")
    if position.shape[0] != 3:
        raise ValueError("Position must have 3 elements")
    if not isinstance(quaternion, np.ndarray):
        raise TypeError("Quaternion must be a numpy array")
    quaternion = quaternion.squeeze()
    if quaternion.ndim != 1:
        raise ValueError("Quaternion must be a 1D array")
    if quaternion.shape[0] != 4:
        raise ValueError("Quaternion must have 4 elements")
    if np.abs(norm(quaternion) - 1.0) > 1e-6:
        raise ValueError("Quaternion must be a unit quaternion")
    if not isinstance(velocity, np.ndarray):
        raise TypeError("Velocity must be a numpy array")
    velocity = velocity.squeeze()
    if velocity.ndim != 1:
        raise ValueError("Velocity must be a 1D array")
    if velocity.shape[0] != 3:
        raise ValueError("Velocity must have 3 elements")
    if not isinstance(angular_velocity, np.ndarray):
        raise TypeError("Angular velocity must be a numpy array")
    angular_velocity = angular_velocity.squeeze()
    if angular_velocity.ndim != 1:
        raise ValueError("Angular velocity must be a 1D array")
    if angular_velocity.shape[0] != 3:
        raise ValueError("Angular velocity must have 3 elements")
    if not isinstance(bottom_height, (int, float)):
        raise TypeError("Bottom height must be a float or an int")

    current_state = np.concatenate([np.array([timestamp]), position, quaternion, velocity, angular_velocity])
    if self._state_dim == 14:
        current_state = np.concatenate([current_state, np.array([bottom_height])])
    self._current_state = current_state

predict(timestamp, angular_velocity)

Predict the next state of the EKF.

Parameters:

Name	Type	Description	Default
timestamp	float	Timestamp of the measurement.	required
angular_velocity	ndarray	Angular velocity in body frame.	required

Source code in navlib/nav/kalman_filter.py

def predict(self, timestamp: float, angular_velocity: np.ndarray) -> None:
    """
    Predict the next state of the EKF.

    Args:
        timestamp (float): Timestamp of the measurement.
        angular_velocity (np.ndarray): Angular velocity in body frame.
    """
    # Compute time step
    dt = timestamp - self._current_state[0]

    if dt < 0:
        warn(f"Negative time step in prediction: dt = {dt:.6f}, skipping prediction.")
        return
    elif dt > self._max_stable_time_step:
        warn(f"Large time step in prediction: dt = {dt:.2f} s, may destabilize EKF.")

    # State transition
    current_state = np.empty(self._state_dim + 1, dtype=np.float64)
    current_state[0] = timestamp

    # Position (p <- p + v * dt)
    current_state[1:4] = self._current_state[1:4] + self._current_state[8:11] * dt

    # Quaternion (q <- q * q{omega_body*dt})
    axis, theta = axis_ang3(angular_velocity)
    q_delta = np.array(
        [
            np.sin(dt * theta / 2) * axis[0],
            np.sin(dt * theta / 2) * axis[1],
            np.sin(dt * theta / 2) * axis[2],
            np.cos(dt * theta / 2),
        ]
    )
    current_state[4:8] = left_qmatrix(self._current_state[4:8]) @ q_delta
    current_state[4:8] /= norm(current_state[4:8])
    if current_state[7] < 0:
        current_state[4:8] *= -1

    # Linear velocity (v <- v)
    current_state[8:11] = self._current_state[8:11]
    # Angular rates (w <- omega_body)
    current_state[11:14] = angular_velocity
    # Bottom height (bh <- bh)
    if self._state_dim == 14:
        current_state[14] = self._current_state[14]

    # Update state
    self._current_state = current_state

    # Process noise covariance and state transition jacobian
    Q = self._process_noise_covariance(dt)
    J = self._state_transition_jacobian(dt, angular_velocity)

    # Predict covariance
    self._current_covariance = J @ self._current_covariance @ J.T + Q
    self._symmetrize_covariance()

recording()

Context manager that records every predict/correction_* call made on this instance during the with block, producing the input rts_smooth expects.

One entry is recorded per individual call, not per external measurement: a single IMU update, for example, typically drives predict followed by two correction calls, and each becomes its own entry. The original (unwrapped) methods are restored when the with block exits, including on an exception.

This is meant for replaying an already-recorded dataset (e.g. a log file) through the filter for offline post-processing. It has no place in a live/online estimation loop: rts_smooth needs the entire recorded sequence to smooth any part of it, which is why smoothing is fundamentally an offline operation.

Yields:

Type

Description

List[dict]

List[dict]: Growing list of {"timestamp", "x", "P", "F", "Q"} entries, one per predict/correction call, in call order. "x"/"P" are the state (without the leading timestamp element) and covariance after that call. "F"/"Q" are the state transition and process noise matrices used to reach it from the previous entry: the real _state_transition_jacobian/_process_noise_covariance for a predict call, or identity/ zero for a correction call, since a correction is a pure information update that does not advance time.

Examples:

>>> ekf = UUV_EKF()
>>> ekf.initialize_state(timestamp, position, quaternion, velocity, angular_velocity)
>>> with ekf.recording() as steps:
...     for timestamp, angular_velocity in imu_stream:
...         ekf.predict(timestamp, angular_velocity)
...         ekf.correction_attitude(quaternion_measurement)
...         # ... corrections from other sensors ...
>>> timestamps, Xs_smooth, Ps_smooth = ekf.rts_smooth(steps)

Source code in navlib/nav/kalman_filter.py

@contextmanager
def recording(self) -> Generator[List[dict], None, None]:
    """
    Context manager that records every `predict`/`correction_*` call made on this instance during the
    ``with`` block, producing the input `rts_smooth` expects.

    One entry is recorded per individual call, not per external measurement: a single IMU update, for
    example, typically drives `predict` followed by two correction calls, and each becomes its own
    entry. The original (unwrapped) methods are restored when the ``with`` block exits, including on
    an exception.

    This is meant for replaying an already-recorded dataset (e.g. a log file) through the filter for
    offline post-processing. It has no place in a live/online estimation loop: `rts_smooth` needs the
    entire recorded sequence to smooth any part of it, which is why smoothing is fundamentally an
    offline operation.

    Yields:
        List[dict]: Growing list of ``{"timestamp", "x", "P", "F", "Q"}`` entries, one per
            predict/correction call, in call order. "x"/"P" are the state (without the leading
            timestamp element) and covariance *after* that call. "F"/"Q" are the state transition and
            process noise matrices used to reach it from the previous entry: the real
            `_state_transition_jacobian`/`_process_noise_covariance` for a `predict` call, or identity/
            zero for a correction call, since a correction is a pure information update that does not
            advance time.

    Examples:
        >>> ekf = UUV_EKF()
        >>> ekf.initialize_state(timestamp, position, quaternion, velocity, angular_velocity)
        >>> with ekf.recording() as steps:
        ...     for timestamp, angular_velocity in imu_stream:
        ...         ekf.predict(timestamp, angular_velocity)
        ...         ekf.correction_attitude(quaternion_measurement)
        ...         # ... corrections from other sensors ...
        >>> timestamps, Xs_smooth, Ps_smooth = ekf.rts_smooth(steps)
    """
    steps: List[dict] = []
    originals = {name: getattr(self, name) for name in self._STATE_MUTATING_METHODS}

    def _predict_wrapper(orig):
        def wrapped(timestamp, angular_velocity, *args, **kwargs):
            dt = timestamp - self._current_state[0]
            result = orig(timestamp, angular_velocity, *args, **kwargs)
            F = self._state_transition_jacobian(dt, angular_velocity)
            Q = self._process_noise_covariance(dt)
            steps.append(
                {
                    "timestamp": self._current_state[0],
                    "x": self._current_state[1:].copy(),
                    "P": self._current_covariance.copy(),
                    "F": F,
                    "Q": Q,
                }
            )
            return result

        return wrapped

    def _correction_wrapper(orig):
        def wrapped(*args, **kwargs):
            # Several correction_* methods return (d_squared, scaled) gating
            # diagnostics -- must be preserved, not swallowed by the wrapper.
            result = orig(*args, **kwargs)
            n = self._state_dim
            steps.append(
                {
                    "timestamp": self._current_state[0],
                    "x": self._current_state[1:].copy(),
                    "P": self._current_covariance.copy(),
                    "F": np.eye(n, dtype=np.float64),
                    "Q": np.zeros((n, n), dtype=np.float64),
                }
            )
            return result

        return wrapped

    try:
        self.predict = _predict_wrapper(originals["predict"])
        for name in self._STATE_MUTATING_METHODS[1:]:
            setattr(self, name, _correction_wrapper(originals[name]))
        yield steps
    finally:
        for name, orig in originals.items():
            setattr(self, name, orig)

reset(timestamp, position, quaternion, velocity, angular_velocity, bottom_height=0.0)

Reset the EKF state after a discontinuity (e.g., time jump).

This method reinitializes the state vector and optionally resets the covariance matrix to reflect increased uncertainty after a time jump or data gap.

Parameters:

Name	Type	Description	Default
timestamp	float	Timestamp of the measurement.	required
position	Union[List[Union[int, float]], ndarray]	Reset position of the vehicle.	required
quaternion	Union[List[Union[int, float]], ndarray]	Reset quaternion of the vehicle. Order: (x, y, z, w).	required
velocity	Union[List[Union[int, float]], ndarray]	Reset velocity of the vehicle.	required
angular_velocity	Union[List[Union[int, float]], ndarray]	Reset angular velocity of the vehicle.	required
bottom_height	Union[float, int]	Reset bottom height of the vehicle. Default is 0.0.	0.0

Source code in navlib/nav/kalman_filter.py

def reset(
    self,
    timestamp: Union[float, int],
    position: Union[List[Union[int, float]], np.ndarray],
    quaternion: Union[List[Union[int, float]], np.ndarray],
    velocity: Union[List[Union[int, float]], np.ndarray],
    angular_velocity: Union[List[Union[int, float]], np.ndarray],
    bottom_height: Union[float, int] = 0.0,
) -> None:
    """
    Reset the EKF state after a discontinuity (e.g., time jump).

    This method reinitializes the state vector and optionally resets the covariance
    matrix to reflect increased uncertainty after a time jump or data gap.

    Args:
        timestamp (float): Timestamp of the measurement.
        position (Union[List[Union[int, float]], np.ndarray]): Reset position of the vehicle.
        quaternion (Union[List[Union[int, float]], np.ndarray]): Reset quaternion of the vehicle. Order: (x, y, z, w).
        velocity (Union[List[Union[int, float]], np.ndarray]): Reset velocity of the vehicle.
        angular_velocity (Union[List[Union[int, float]], np.ndarray]): Reset angular velocity of the vehicle.
        bottom_height (Union[float, int]): Reset bottom height of the vehicle. Default is 0.0.
    """
    # Reuse the validation and state setting from initialize_state
    self.initialize_state(timestamp, position, quaternion, velocity, angular_velocity, bottom_height)

    # Reset covariance to initial uncertainty (see _initial_state_covariance for
    # why this uses sensor R, not process noise for one predict() step).
    self._current_covariance = self._initial_state_covariance()

    # Optionally add higher initial uncertainty for position and velocity
    # since we're resetting after a discontinuity
    self._current_covariance[:3, :3] *= 10.0  # Position uncertainty
    self._current_covariance[7:10, 7:10] *= 5.0  # Velocity uncertainty

    # Reset velocity filtering statistics
    self._velocity_d_squared_acummulted = 0.0
    self._velocity_d_squared_count = 0
    self._velocity_q_tuner = _QTuner()

rts_smooth(steps)

Rauch-Tung-Striebel smoother over a recorded run of this estimator (see recording).

Parameters:

Name	Type	Description	Default
steps	List[dict]	Output of recording() -- a chronological list of {"timestamp", "x", "P", "F", "Q"} entries, one per predict/correction call.	required

Returns:

Name	Type	Description
timestamps	ndarray	Timestamp of each step, shape (n,).
Xs_smooth	ndarray	Smoothed states (without the leading timestamp element), shape (n, state_dim).
Ps_smooth	ndarray	Smoothed covariances, shape (n, state_dim, state_dim).

Raises:

Type	Description
ValueError	If fewer than two steps are given.

Note

A quaternion and its negation represent the identical rotation, and _check_state_attitude (run after every predict/correction call during recording) is free to flip which of the two it reports whenever the scalar part crosses zero -- which happens whenever the true rotation passes near 180 degrees. Before smoothing, this method aligns quaternion signs across consecutive recorded samples (flipping whichever one disagrees with its predecessor) so the backward recursion never sees an artificial ~2q jump from such a relabeling: kf_smooth's innovation is a plain vector difference, and a jump between +q and -q there is not a real residual, it is pure representation noise that would otherwise corrupt the entire smoothed trajectory back to the start of the recording. state_correction here only renormalizes the quaternion's norm at every step (needed because the recursion doesn't otherwise preserve it) -- it does not also force a positive scalar part at every step, since doing so mid-recursion would reintroduce the same kind of artificial jump this alignment removes, just at whatever arbitrary point the smoothed quaternion's own scalar part happens to cross zero. The positive- scalar-part convention is instead applied once, as a final cosmetic pass after the whole recursion finishes, where it can no longer feed back into anything.

This is deliberately not a full tangent-space/error-state treatment of the quaternion -- but it is exact for the predict-dominated part of a trajectory: this class's predict-step Jacobian for the quaternion block is the literal right-multiplication matrix of a fixed (state-independent) incremental rotation, so F @ x reproduces the true nonlinear quaternion update exactly, not just to first order. Only the (typically much sparser) correction steps' contribution to the smoothing blend carries the standard over-parameterized-EKF approximation.

Source code in navlib/nav/kalman_filter.py

def rts_smooth(self, steps: List[dict]) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Rauch-Tung-Striebel smoother over a recorded run of this estimator (see `recording`).

    Args:
        steps (List[dict]): Output of `recording()` -- a chronological list of
            ``{"timestamp", "x", "P", "F", "Q"}`` entries, one per predict/correction call.

    Returns:
        timestamps (np.ndarray): Timestamp of each step, shape (n,).
        Xs_smooth (np.ndarray): Smoothed states (without the leading timestamp element), shape
            (n, state_dim).
        Ps_smooth (np.ndarray): Smoothed covariances, shape (n, state_dim, state_dim).

    Raises:
        ValueError: If fewer than two steps are given.

    Note:
        A quaternion and its negation represent the identical rotation, and `_check_state_attitude`
        (run after every predict/correction call during recording) is free to flip which of the two it
        reports whenever the scalar part crosses zero -- which happens whenever the true rotation
        passes near 180 degrees. Before smoothing, this method aligns quaternion signs across
        consecutive recorded samples (flipping whichever one disagrees with its predecessor) so the
        backward recursion never sees an artificial ~2q jump from such a relabeling: `kf_smooth`'s
        innovation is a plain vector difference, and a jump between +q and -q there is not a real
        residual, it is pure representation noise that would otherwise corrupt the entire smoothed
        trajectory back to the start of the recording. `state_correction` here only renormalizes the
        quaternion's norm at every step (needed because the recursion doesn't otherwise preserve it) --
        it does *not* also force a positive scalar part at every step, since doing so mid-recursion
        would reintroduce the same kind of artificial jump this alignment removes, just at whatever
        arbitrary point the smoothed quaternion's own scalar part happens to cross zero. The positive-
        scalar-part convention is instead applied once, as a final cosmetic pass after the whole
        recursion finishes, where it can no longer feed back into anything.

        This is deliberately not a full tangent-space/error-state treatment of the quaternion -- but it
        is exact for the predict-dominated part of a trajectory: this class's predict-step Jacobian for
        the quaternion block is the literal right-multiplication matrix of a *fixed* (state-independent)
        incremental rotation, so `F @ x` reproduces the true nonlinear quaternion update exactly, not
        just to first order. Only the (typically much sparser) correction steps' contribution to the
        smoothing blend carries the standard over-parameterized-EKF approximation.
    """
    if len(steps) < 2:
        raise ValueError("rts_smooth requires at least two recorded steps")

    timestamps = np.array([s["timestamp"] for s in steps])
    Xs = np.array([s["x"] for s in steps])
    Ps = np.array([s["P"] for s in steps])
    Fs = np.array([s["F"] for s in steps])
    Qs = np.array([s["Q"] for s in steps])

    Xs = Xs.copy()
    for i in range(1, len(Xs)):
        if np.dot(Xs[i, 3:7], Xs[i - 1, 3:7]) < 0:
            Xs[i, 3:7] = -Xs[i, 3:7]

    def _renormalize_quaternion_norm(x: np.ndarray) -> np.ndarray:
        x = x.copy()
        x[3:7] = x[3:7] / np.linalg.norm(x[3:7])
        return x

    Xs_smooth, Ps_smooth = kf_smooth(Xs, Ps, Fs, Qs, state_correction=_renormalize_quaternion_norm)

    q = Xs_smooth[:, 3:7]
    negative_scalar = q[:, 3] < 0
    q[negative_scalar] *= -1
    Xs_smooth[:, 3:7] = q

    return timestamps, Xs_smooth, Ps_smooth

kf_lti_discretize(Ac, Bc=None, Qc=None, dt=1)

Discretize a Linear Time-Invariant (LTI) system using the matrix fraction decomposition for use in a discrete-time Kalman filter.

Parameters:

Name	Type	Description	Default
Ac	ndarray	Continuos state transition matrix.	required
Bc	ndarray	Continuos input matrix, by default None.	None
Qc	ndarray	Continuos covariance matrix, by default None.	None
dt	float	Time step, by default 1.	1

Returns:

Name	Type	Description
transition_mat	ndarray	Discrete state transition matrix.
inpu_mat	ndarray	Discrete input matrix.
covariance_mat	ndarray	Discrete covariance matrix.

Raises:

Type	Description
TypeError	If Ac, Bc or Qc are not numpy arrays.
ValueError	If Ac is not a 2D matrix, if Ac and Bc do not have the same number of rows, if Qc is not a square matrix, if Qc does not have the same number of rows as Ac.

Source code in navlib/nav/kalman_filter.py

def kf_lti_discretize(
    Ac: np.ndarray,
    Bc: np.ndarray = None,
    Qc: np.ndarray = None,
    dt: float = 1,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Discretize a Linear Time-Invariant (LTI) system using the matrix fraction decomposition
    for use in a discrete-time Kalman filter.

    Args:
        Ac (np.ndarray): Continuos state transition matrix.
        Bc (np.ndarray): Continuos input matrix, by default None.
        Qc (np.ndarray): Continuos covariance matrix, by default None.
        dt (float): Time step, by default 1.

    Returns:
        transition_mat (np.ndarray): Discrete state transition matrix.
        inpu_mat (np.ndarray): Discrete input matrix.
        covariance_mat (np.ndarray): Discrete covariance matrix.

    Raises:
        TypeError: If Ac, Bc or Qc are not numpy arrays.
        ValueError: If Ac is not a 2D matrix, if Ac and Bc do not have the same number of rows,
            if Qc is not a square matrix, if Qc does not have the same number of rows as Ac.
    """
    # Convert to numpy array
    if isinstance(Ac, list):
        Ac = np.array(Ac)
    if isinstance(Bc, list):
        Bc = np.array(Bc)
    if isinstance(Qc, list):
        Qc = np.array(Qc)

    # Check inputs type
    if not isinstance(Ac, np.ndarray):
        raise TypeError("Ac must be a numpy array")
    if Bc is not None and not isinstance(Bc, np.ndarray):
        raise TypeError("Bc must be a numpy array")
    if Qc is not None and not isinstance(Qc, np.ndarray):
        raise TypeError("Qc must be a numpy array")
    if not isinstance(dt, (int, float)):
        raise TypeError("dt must be a number")

    # Force the input matrix to be a column vector
    if Bc is not None and Bc.ndim == 1:
        Bc = Bc[:, np.newaxis] if Bc.ndim == 1 else np.vstack(Bc)

    # Check that the shape of the matrices is correct
    if Ac.ndim != 2:
        raise ValueError("Ac must be a 2D matrix")
    if Bc is not None and Bc.shape[0] != Ac.shape[0]:
        raise ValueError("Ac and Bc must have the same number of rows")
    if Qc is not None and Qc.shape[0] != Qc.shape[1]:
        raise ValueError("Qc must be a square matrix")
    if Qc is not None and Qc.shape[0] != Ac.shape[0]:
        raise ValueError("Qc must have the same number of rows as Ac")

    # Check the number of states
    n = Ac.shape[0]

    # Default to zero non provided matrices
    if Bc is None:
        Bc = np.zeros([n, 1])

    if Qc is None:
        Qc = np.zeros([n, n])

    # Discretize state transition and input matrix (close form)
    # Ad = expm(Ac*dt)
    M = np.vstack([np.hstack([Ac, Bc]), np.zeros([1, n + 1])])
    ME = expm(M * dt)

    # Discretize state transition and input matrix
    Ad = ME[:n, :n]
    Bd = ME[:n, n:]

    # Discretize Covariance: by (Van Loan, 1978)
    F = np.vstack([np.hstack([-Ac, Qc]), np.hstack([np.zeros([n, n]), Ac.T])])
    G = expm(F * dt)
    Qd = np.dot(G[n:, n:].T, G[:n, n:])

    # # Discretize Covariance: by matrix fraction decomposition
    # Phi = vstack([hstack([Ac,            Qc]),
    #               hstack([np.zeros([n,n]),-Ac.T])])
    # AB  = np.dot (scipy.linalg.expm(Phi*dt), vstack([np.zeros([n,n]),np.eye(n)]))
    # Qd  = np.linalg.solve(AB[:n,:].T, AB[n:2*n,:].T).T

    return Ad, Bd, Qd

kf_predict(x, P, A=None, Q=None, B=None, u=None)

Prediction step of the Kalman filter.

Parameters:

Name	Type	Description	Default
x	ndarray	State mean.	required
P	ndarray	State covariance.	required
A	ndarray	State transition matrix, by default None.	None
Q	ndarray	Process noise covariance, by default None.	None
B	ndarray	Input matrix, by default None.	None
u	ndarray	Input vector, by default None.	None

Returns:

Name	Type	Description
updated_state_mean	ndarray	Updated state mean.
updated_state_cov	ndarray	Updated state covariance.

Raises:

Type	Description
TypeError	If A, Q, B, u, x or P are not numpy arrays.
ValueError	If A is not a 2D matrix, if x and A do not have the same number of rows, if B is not a 2D matrix, if the number of columns in B is not equal to the number of rows in u, if B does not have the same number of rows as x, if P is not a square matrix, if P is not a square matrix, if Q is not a square matrix, if Q does not have the same number of rows as A.

Source code in navlib/nav/kalman_filter.py

def kf_predict(
    x: np.ndarray,
    P: np.ndarray,
    A: np.ndarray = None,
    Q: np.ndarray = None,
    B: np.ndarray = None,
    u: np.ndarray = None,
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Prediction step of the Kalman filter.

    Args:
        x (np.ndarray): State mean.
        P (np.ndarray): State covariance.
        A (np.ndarray): State transition matrix, by default None.
        Q (np.ndarray): Process noise covariance, by default None.
        B (np.ndarray): Input matrix, by default None.
        u (np.ndarray): Input vector, by default None.

    Returns:
        updated_state_mean (np.ndarray): Updated state mean.
        updated_state_cov (np.ndarray): Updated state covariance.

    Raises:
        TypeError: If A, Q, B, u, x or P are not numpy arrays.
        ValueError: If A is not a 2D matrix, if x and A do not have the same number of rows,
            if B is not a 2D matrix, if the number of columns in B is not equal to the number of rows in u,
            if B does not have the same number of rows as x, if P is not a square matrix,
            if P is not a square matrix, if Q is not a square matrix, if Q does not have the same number of rows as A.
    """
    # Convert to numpy array
    if isinstance(A, list):
        A = np.array(A)
    if isinstance(Q, list):
        Q = np.array(Q)
    if isinstance(B, list):
        B = np.array(B)
    if isinstance(u, list):
        u = np.array(u)
    if isinstance(x, list):
        x = np.array(x)
    if isinstance(P, list):
        P = np.array(P)

    # Check inputs type
    if A is not None and not isinstance(A, np.ndarray):
        raise TypeError("A must be a numpy array")
    if Q is not None and not isinstance(Q, np.ndarray):
        raise TypeError("Q must be a numpy array")
    if B is not None and not isinstance(B, np.ndarray):
        raise TypeError("B must be a numpy array")
    if u is not None and not isinstance(u, np.ndarray):
        raise TypeError("u must be a numpy array")
    if not isinstance(x, np.ndarray):
        raise TypeError("x must be a numpy array")
    if not isinstance(P, np.ndarray):
        raise TypeError("P must be a numpy array")

    # Force the state, input and control matrices to be column vectors
    x = x[:, np.newaxis] if x.ndim == 1 else x
    if u is not None:
        u = u[:, np.newaxis] if u.ndim == 1 else u

    # Check that the shape of the matrices is correct
    if A is not None and A.ndim != 2:
        raise ValueError("A must be a 2D matrix")
    if A is not None and x.shape[0] != A.shape[0]:
        raise ValueError("x and A must have the same number of rows")
    if B is not None and B.ndim != 2:
        raise ValueError("B must be a 2D matrix")
    if u is not None and B is not None and u.shape[0] != B.shape[1]:
        raise ValueError("The number of columns in B must be equal to the number of rows in u")
    if B is not None and B.shape[0] != x.shape[0]:
        raise ValueError("B must have the same number of rows as x")
    if P.ndim != 2 or P.shape[0] != P.shape[1]:
        raise ValueError("P must be a square matrix")
    if A is not None and P.shape[0] != A.shape[0]:
        raise ValueError("P must have the same number of rows as A")
    if Q is not None and (Q.ndim != 2 or Q.shape[0] != Q.shape[1]):
        raise ValueError("Q must be a square matrix")
    if Q is not None and A is not None and Q.shape[0] != A.shape[0]:
        raise ValueError("Q must have the same number of rows as A")

    # Check Arguments
    n = A.shape[0] if A is not None else x.shape[0]

    # Default state transition matrix to the identity matrix if not provided
    if A is None:
        A = np.eye(n)

    # Default process noise covariance to zero matrix if not provided
    if Q is None:
        Q = np.zeros((n, n))

    # Default input matrix to the identity matrix if not provided
    if B is None and u is not None:
        B = np.eye(n, u.shape[0])

    # Prediction step
    # State
    if u is None:
        x = A @ x
    else:
        x = A @ x + B @ u

    # Covariance
    P = A @ P @ A.T + Q

    return x.squeeze(), P.squeeze()

kf_smooth(Xs, Ps, Fs, Qs, state_correction=None)

Rauch-Tung-Striebel (RTS) smoother for a linear (or locally linearized) Kalman filter.

Runs a backward pass over a chronological sequence of filtered means and covariances, using the state transition matrix and process noise covariance that produced each step, to produce a smoothed estimate that uses information from the entire sequence rather than only the past. This is a non-causal, offline operation: smoothing step k requires every step after k to already be available, so this cannot be run as part of a live/online estimation loop -- only as a post-processing pass over an already-recorded run.

Parameters:

Name	Type	Description	Default
Xs	ndarray	Filtered state means, shape (n, state_dim).	required
Ps	ndarray	Filtered state covariances, shape (n, state_dim, state_dim).	required
Fs	ndarray	State transition matrix used going into each step, shape (n, state_dim, state_dim). Fs[k] is the transition from step k-1 to step k; Fs[0] is unused.	required
Qs	ndarray	Process noise covariance used going into each step, same shape and indexing as Fs.	required
state_correction	Callable[[ndarray], ndarray]	If given, applied to each smoothed state immediately after it is computed, before it becomes the "future" reference for the next (earlier) step of the backward recursion. Must accept and return an array of shape (state_dim,). Intended for states with a non-Euclidean component -- e.g. renormalizing a quaternion sub-block back onto the unit sphere at every step, rather than only once after the whole pass finishes, which would let it drift for the length of the recursion before ever being corrected. Defaults to no correction, which reproduces the plain textbook recursion.	None

Returns:

Name	Type	Description
Xs_smooth	ndarray	Smoothed state means, same shape as Xs.
Ps_smooth	ndarray	Smoothed state covariances, same shape as Ps.

Raises:

Type	Description
TypeError	If Xs, Ps, Fs or Qs are not numpy arrays.
ValueError	If Xs is not 2D, if fewer than two steps are given, or if Ps/Fs/Qs don't have shape (n, state_dim, state_dim) matching Xs.

Note

This is the standard linear/EKF Rauch-Tung-Striebel recursion when state_correction is not given. For a state that includes a quaternion, see UUV_EKF.rts_smooth for the quaternion-aware convenience wrapper used for navigation states, which supplies a state_correction for exactly this reason.

Source code in navlib/nav/kalman_filter.py

def kf_smooth(
    Xs: np.ndarray,
    Ps: np.ndarray,
    Fs: np.ndarray,
    Qs: np.ndarray,
    state_correction: Callable[[np.ndarray], np.ndarray] = None,
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Rauch-Tung-Striebel (RTS) smoother for a linear (or locally linearized) Kalman filter.

    Runs a backward pass over a chronological sequence of filtered means and covariances, using the state
    transition matrix and process noise covariance that produced each step, to produce a smoothed estimate
    that uses information from the entire sequence rather than only the past. This is a non-causal, offline
    operation: smoothing step k requires every step after k to already be available, so this cannot be run
    as part of a live/online estimation loop -- only as a post-processing pass over an already-recorded run.

    Args:
        Xs (np.ndarray): Filtered state means, shape (n, state_dim).
        Ps (np.ndarray): Filtered state covariances, shape (n, state_dim, state_dim).
        Fs (np.ndarray): State transition matrix used going into each step, shape (n, state_dim, state_dim).
            Fs[k] is the transition from step k-1 to step k; Fs[0] is unused.
        Qs (np.ndarray): Process noise covariance used going into each step, same shape and indexing as Fs.
        state_correction (Callable[[np.ndarray], np.ndarray], optional): If given, applied to each smoothed
            state immediately after it is computed, before it becomes the "future" reference for the next
            (earlier) step of the backward recursion. Must accept and return an array of shape (state_dim,).
            Intended for states with a non-Euclidean component -- e.g. renormalizing a quaternion sub-block
            back onto the unit sphere at every step, rather than only once after the whole pass finishes,
            which would let it drift for the length of the recursion before ever being corrected. Defaults
            to no correction, which reproduces the plain textbook recursion.

    Returns:
        Xs_smooth (np.ndarray): Smoothed state means, same shape as Xs.
        Ps_smooth (np.ndarray): Smoothed state covariances, same shape as Ps.

    Raises:
        TypeError: If Xs, Ps, Fs or Qs are not numpy arrays.
        ValueError: If Xs is not 2D, if fewer than two steps are given, or if Ps/Fs/Qs don't have shape
            (n, state_dim, state_dim) matching Xs.

    Note:
        This is the standard linear/EKF Rauch-Tung-Striebel recursion when `state_correction` is not
        given. For a state that includes a quaternion, see `UUV_EKF.rts_smooth` for the quaternion-aware
        convenience wrapper used for navigation states, which supplies a `state_correction` for exactly
        this reason.
    """
    if isinstance(Xs, list):
        Xs = np.array(Xs)
    if isinstance(Ps, list):
        Ps = np.array(Ps)
    if isinstance(Fs, list):
        Fs = np.array(Fs)
    if isinstance(Qs, list):
        Qs = np.array(Qs)

    if not isinstance(Xs, np.ndarray):
        raise TypeError("Xs must be a numpy array")
    if not isinstance(Ps, np.ndarray):
        raise TypeError("Ps must be a numpy array")
    if not isinstance(Fs, np.ndarray):
        raise TypeError("Fs must be a numpy array")
    if not isinstance(Qs, np.ndarray):
        raise TypeError("Qs must be a numpy array")

    if Xs.ndim != 2:
        raise ValueError("Xs must be a 2D array of shape (n, state_dim)")
    n, state_dim = Xs.shape
    if n < 2:
        raise ValueError("kf_smooth requires at least two steps")
    if Ps.shape != (n, state_dim, state_dim):
        raise ValueError("Ps must have shape (n, state_dim, state_dim) matching Xs")
    if Fs.shape != (n, state_dim, state_dim):
        raise ValueError("Fs must have shape (n, state_dim, state_dim) matching Xs")
    if Qs.shape != (n, state_dim, state_dim):
        raise ValueError("Qs must have shape (n, state_dim, state_dim) matching Xs")

    Xs_smooth = Xs.copy()
    Ps_smooth = Ps.copy()

    for k in range(n - 2, -1, -1):
        F_next = Fs[k + 1]
        Q_next = Qs[k + 1]
        x_k, P_k = Xs[k], Ps[k]

        # Predicted covariance for step k+1, from step k's posterior.
        Pp = F_next @ P_k @ F_next.T + Q_next
        # Smoother gain C = P_k F^T Pp^-1, computed as solve(Pp, F P_k)^T (Pp and P_k are
        # symmetric). Pp can be exactly singular when a step adds no process noise (a correction,
        # or a zero-dt predict, both recorded with F = I and Q = 0) while P_k is itself
        # rank-deficient -- e.g. the initial covariance, which leaves the quaternion's radial
        # direction at zero variance. The pseudo-inverse is then the correct gain: zero along
        # directions the filter is certain about, standard RTS everywhere else.
        try:
            C = np.linalg.solve(Pp, F_next @ P_k).T
        except np.linalg.LinAlgError:
            C = P_k @ F_next.T @ np.linalg.pinv(Pp, hermitian=True)

        x_pred = F_next @ x_k
        Xs_smooth[k] = x_k + C @ (Xs_smooth[k + 1] - x_pred)
        if state_correction is not None:
            Xs_smooth[k] = state_correction(Xs_smooth[k])
        Ps_smooth[k] = P_k + C @ (Ps_smooth[k + 1] - Pp) @ C.T

    return Xs_smooth, Ps_smooth

kf_update(x, P, y, H, R)

Update step of the Kalman filter.

Parameters:

Name	Type	Description	Default
x	ndarray	State mean.	required
P	ndarray	State covariance.	required
y	ndarray	Measurement.	required
H	ndarray	Measurement matrix.	required
R	ndarray	Measurement noise covariance.	required

Returns:

Name	Type	Description
x	ndarray	Updated state mean.
P	ndarray	Updated state covariance.
K	ndarray	Kalman Gain.
dy	ndarray	Measurement residual.
S	ndarray	Covariance residual.

Raises:

Type	Description
TypeError	If x, P, y, H or R are not numpy arrays.
ValueError	If x is not a numpy array, if P is not a numpy array, if y is not a numpy array, if H is not a 2D matrix, if R is not a square matrix, if P is not a square matrix, if P does not have the same number of rows as x, if H does not have the same number of columns as rows in x, if x and y do not have the same number of rows, if R does not have the same number of rows as y.

Source code in navlib/nav/kalman_filter.py

def kf_update(
    x: np.ndarray,
    P: np.ndarray,
    y: np.ndarray,
    H: np.ndarray,
    R: np.ndarray,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """
    Update step of the Kalman filter.

    Args:
        x (np.ndarray): State mean.
        P (np.ndarray): State covariance.
        y (np.ndarray): Measurement.
        H (np.ndarray): Measurement matrix.
        R (np.ndarray): Measurement noise covariance.

    Returns:
        x (np.ndarray): Updated state mean.
        P (np.ndarray): Updated state covariance.
        K (np.ndarray): Kalman Gain.
        dy (np.ndarray): Measurement residual.
        S (np.ndarray): Covariance residual.

    Raises:
        TypeError: If x, P, y, H or R are not numpy arrays.
        ValueError: If x is not a numpy array, if P is not a numpy array, if y is not a numpy array,
            if H is not a 2D matrix, if R is not a square matrix, if P is not a square matrix,
            if P does not have the same number of rows as x, if H does not have the same number of columns as rows in x,
            if x and y do not have the same number of rows, if R does not have the same number of rows as y.
    """
    # Convert to numpy array
    if isinstance(x, list):
        x = np.array(x)
    if isinstance(P, list):
        P = np.array(P)
    if isinstance(y, list):
        y = np.array(y)
    if isinstance(H, list):
        H = np.array(H)
    if isinstance(R, list):
        R = np.array(R)

    # Check inputs type
    if not isinstance(x, np.ndarray):
        raise TypeError("x must be a numpy array")
    if not isinstance(P, np.ndarray):
        raise TypeError("P must be a numpy array")
    if not isinstance(y, np.ndarray):
        raise TypeError("y must be a numpy array")
    if not isinstance(H, np.ndarray):
        raise TypeError("H must be a numpy array")
    if not isinstance(R, np.ndarray):
        raise TypeError("R must be a numpy array")

    # Force the state and measurements to be column vectors
    x = x[:, np.newaxis] if x.ndim == 1 else x
    y = y[:, np.newaxis] if y.ndim == 1 else y

    # Check dimensions
    if P.ndim != 2 or P.shape[0] != P.shape[1]:
        raise ValueError("P must be a square matrix")
    if P.shape[0] != x.shape[0]:
        raise ValueError("P must have the same number of rows as x")
    if H.ndim != 2:
        raise ValueError("H must be a 2D matrix")
    if x.shape[0] != H.shape[1]:
        raise ValueError("H must have the same number of columns as rows in x")
    if R.ndim != 2 or R.shape[0] != R.shape[1]:
        raise ValueError("R must be a square matrix")
    if R.shape[0] != y.shape[0]:
        raise ValueError("R must have the same number of rows as y")

    # Compute measurement residual
    dy = y - H @ x
    # Compute covariance residual
    S = R + H @ P @ H.T
    # Compute Kalman Gain
    K = P @ H.T @ np.linalg.inv(S)

    # Update state estimate
    x = x + K @ dy
    P = P - K @ H @ P

    return x.squeeze(), P.squeeze(), K.squeeze(), dy.squeeze(), S.squeeze()