pntos-api 2.1.0.post0.dev0__py3-none-any.whl

pntos/api/plugins/inertial.py

@@ -0,0 +1,414 @@
+"""Python API of pntOS."""
+
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any, TypeVar
+
+from aspn23 import AspnBase, MeasurementImu, TypeTimestamp
+from numpy import float64
+from numpy.typing import NDArray
+
+from .common import CommonPlugin, Message
+
+
+class InertialFrame(IntEnum):
+    """
+    An enumeration that specifies frame.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    INERTIAL_FRAME_NED = 0
+    """
+    Force vectors in this frame are north-east-down in :math:`m/s^2`.
+
+	Rotation rate vectors in this frame are of an inertial sensor with respect to inertial frame, in
+	the inertial sensor frame (:math:`rad/s`). Sometimes represented as
+	:math:`w^\\text{s}_\\text{is}` (or :math:`w^\\text{b}_\\text{ib}` if the body frame is aligned
+	with the inertial sensor frame.
+    """
+
+
+@dataclass
+class InertialForcesRates:
+    """
+    A struct containing specific forces and rotation rates from the inertial.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+
+    Attributes:
+        forces_and_rates (MeasurementImu): An ASPN IMU message which has been repurposed to hold
+            specific forces (the ``meas_accel`` field) and rotation rates (the ``meas_gyro`` field)
+            in a different frame (see :attr:`frame` below).
+        frame (InertialFrame): Specifies the frame of the above forces and rates.
+    """
+
+    forces_and_rates: MeasurementImu
+    frame: InertialFrame
+    """The frame at which the forces and rates are valid.
+    """
+
+
+@dataclass
+class StandardInertialErrors:
+    """
+    A structure representing the biases on a set of 3-axis gyros and 3-axis accelerometers.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+
+    Attributes:
+        accel_biases (NDArray[float64]): A 1D vector of length 3 containing biases for a 3-axis accelerometer
+            in the sensor's (X-Y-Z) frame, expressed in :math:`m/s^2`.
+        gyro_biases (NDArray[float64]): A 1D vector of length 3 containing biases for a 3-axis gyro in the
+            sensor's (X-Y-Z) frame, expressed in :math:`rad/s`.
+        accel_scale_factors (NDArray[float64]): A 1D vector of length 3 containing scale factor errors for a
+            3-axis accelerometer in the sensor's (X-Y-Z) frame, unitless.
+        gyro_scale_factors (NDArray[float64]): A 1D vector of length 3 containing scale factor errors for a
+            3-axis gyroscope in the sensor's (X-Y-Z) frame, unitless.
+    """
+
+    accel_biases: NDArray[float64]
+    gyro_biases: NDArray[float64]
+    accel_scale_factors: NDArray[float64]
+    gyro_scale_factors: NDArray[float64]
+
+
+class InertialSolutionRangeType(IntEnum):
+    """
+    Solution type to request from an inertial.
+
+    An enumeration that allows the user to decide if the solution they request is the best available
+    solution or one that has no discontinuities in it due to resets.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    INERTIAL_BEST_KNOWN_SOLUTION = 0
+    INERTIAL_NO_UPDATES_WITHIN_RANGE = 1
+
+
+class CommonInertial(ABC):
+    """
+    A common base type for an inertial.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def request_solution_message_type(self) -> type[AspnBase]:
+        """
+        Get the solution type.
+
+        Returns:
+            type[AspnBase]: The message type that will be returned by
+            :meth:`request_current_solution`, :meth:`request_solution`, and
+            :meth:`request_solutions`.
+        """
+        pass
+
+    @abstractmethod
+    def request_current_solution(self) -> Message:
+        """
+        Get the current inertial solution.
+
+        Returns:
+            Message: The current inertial solution.
+        """
+        pass
+
+    @abstractmethod
+    def request_solution(self, time: TypeTimestamp) -> Message | None:
+        """
+        Request solution at a specific time.
+
+        Args:
+            time (TypeTimestamp): The time at which the returned solution should be valid.
+
+        Returns:
+            Message | None: The solution computed by this inertial at ``time`` if ``time`` is in the
+            valid range, ``None`` otherwise (:meth:`is_time_in_range` can be used to check ``time``
+            before calling this method).
+        """
+        pass
+
+    @abstractmethod
+    def request_solutions(
+        self, times: list[TypeTimestamp], solution_type: InertialSolutionRangeType
+    ) -> list[Message | None] | None:
+        """
+        Request solutions at multiple specific times.
+
+        Args:
+            times (list[TypeTimestamp]): An array of times at which solutions are requested.
+            solution_type (InertialSolutionRangeType): The type of solution requested.
+
+        Returns:
+            list[Message | None] | None: An array of solutions. Returns ``None`` if ``solution_type`` is unsupported
+            by this inertial or every instance of ``times`` is outside the valid range. Otherwise
+            guaranteed to not be ``None``.
+        """
+        pass
+
+    @abstractmethod
+    def is_time_in_range(self, time: TypeTimestamp) -> bool:
+        """
+        Check if a solution exists at a given time.
+
+        Args:
+            time (TypeTimestamp): The query time.
+
+        Returns:
+            bool: ``True`` if a solution exists at ``time``, ``False`` otherwise. This result is
+            only valid until another method (for example, :meth:`process_pntos_message`) is called.
+        """
+        pass
+
+    @abstractmethod
+    def request_earliest_time(self) -> TypeTimestamp:
+        """
+        Get the earliest available time at which a solution or forces and rates can be requested.
+
+        This result is only valid until another method (for example, :meth:`process_pntos_message`)
+        is called.
+
+        Returns:
+            TypeTimestamp: The earliest available time at which a solution or forces and rates can
+            be requested.
+        """
+        pass
+
+    @abstractmethod
+    def request_latest_time(self) -> TypeTimestamp:
+        """
+        Get the latest available time at which a solution or forces and rates can be requested.
+
+        This result is only valid until another method (for example, :meth:`process_pntos_message`)
+        is called.
+
+        Returns:
+            TypeTimestamp: The latest available time at which a solution or forces and rates can
+            be requested.
+        """
+        pass
+
+    @abstractmethod
+    def request_process_pntos_message_types(self) -> list[type[AspnBase]]:
+        """
+        Returns an array of message types that are supported by this plugin.
+
+        Returns:
+            list[type[AspnBase]]: An array of message types that are supported by this plugin as
+            inputs to :meth:`process_pntos_message`.
+        """
+        pass
+
+    @abstractmethod
+    def process_pntos_message(self, message: Message) -> None:
+        """
+        A new message to be incorporated into the computed inertial solution.
+
+        Args:
+            message (Message)
+        """
+        pass
+
+    @abstractmethod
+    def request_forces_and_rates(
+        self, time: TypeTimestamp
+    ) -> InertialForcesRates | None:
+        """
+        Request forces and rates for a given time.
+
+        Args:
+            time (TypeTimestamp): The time at which the forces and rates should be valid.
+
+        Returns:
+            InertialForcesRates | None: The instantaneous forces and rates at ``time`` if ``time``
+            is in the valid range, ``None`` otherwise (:meth:`is_time_in_range` can be used to check
+            ``time`` before calling this method).
+        """
+        pass
+
+    @abstractmethod
+    def request_average_forces_and_rates(
+        self, time1: TypeTimestamp, time2: TypeTimestamp
+    ) -> InertialForcesRates | None:
+        """
+        Request average forces and rates over a time period.
+
+        Args:
+            time1 (TypeTimestamp): The start of the time range over which the forces and rates
+                should be valid.
+            time2 (TypeTimestamp): The end of the time range over which the forces
+                and rates should be valid.
+
+        Returns:
+            InertialForcesRates | None: The average forces and rates over the period of time defined
+            by ``time1`` and ``time2`` if at least one of them is in the valid range, ``None``
+            otherwise (:meth:`is_time_in_range` can be used to check both times before calling this
+            method).
+        """
+        pass
+
+
+# The external inertial case does not need any extra functionality, so just alias the base class
+# rather than inheriting from it and adding nothing.
+ExternalInertial = CommonInertial
+
+
+class StandardInertialMechanization(CommonInertial, ABC):
+    """
+    A struct produced by a :class:`pntos.api.InertialPlugin`. It generates solutions from raw IMU data.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def request_reset_message_types(self) -> list[type[AspnBase]] | None:
+        """
+        Get valid types of reset messages.
+
+        Returns:
+            list[type[AspnBase]] | None: An array of message types that are supported by this plugin
+            for resetting the inertial solution, or ``None`` if resetting the inertial solution is
+            an unsupported operation by the inertial plugin.
+        """
+        pass
+
+    @abstractmethod
+    def reset_solution(self, message: Message) -> None:
+        """
+        Set the solution to the values in ``message``.
+
+        For example, if ``message`` is PVA then the inertial solution will be set to that PVA. If
+        ``message`` is just position, then only the position portion of the inertial solution will
+        be set using ``message``.
+
+        Args:
+            message (Message): A message containing the information necessary to reset the solution.
+                To see the types supported by the implementation, call
+                :meth:`request_reset_message_types`.
+        """
+        pass
+
+    @abstractmethod
+    def correct_sensor_errors(
+        self, time: TypeTimestamp, errors: StandardInertialErrors
+    ) -> None:
+        """
+        Reset the current inertial internal bias values.
+
+        Reset the current inertial internal bias values with corrections from an external source,
+        such as a filter or error estimator. The errors passed in here will be adjusted for
+        internally by the inertial when processing incoming data. Thus, if errors are passed into the
+        inertial here they should not be corrected for in an external filter processing the inertial
+        output (which would lead to a double correction).
+
+        Args:
+            time (TypeTimestamp): The time at which ``errors`` should be valid.
+            errors (StandardInertialErrors): An estimate of the inertial sensor's errors.
+        """
+        pass
+
+    @abstractmethod
+    def request_sensor_errors(
+        self, time: TypeTimestamp
+    ) -> StandardInertialErrors | None:
+        """
+        Request inertial errors for a given time.
+
+        Args:
+            time (TypeTimestamp): Time at which inertial errors should be valid.
+
+        Returns:
+            StandardInertialErrors | None: Inertial errors at ``time`` if ``time`` is in the valid
+            range (:meth:`pntos.api.CommonInertial.is_time_in_range` can be used to check ``time`` before
+            calling this method), ``None`` otherwise.
+        """
+        pass
+
+
+InertialType = TypeVar(
+    'InertialType', StandardInertialMechanization, ExternalInertial, Any
+)
+"""
+An enumeration of the types of inertials an inertial plugin could provide.
+
+"Any" is included for future compatibility.
+"""
+
+
+class InertialPlugin(CommonPlugin, ABC):
+    """
+    An implementation of an inertial plugin.
+
+    This plugin generates :class:`pntos.api.CommonInertial` instances which may be used to generate INS
+    solutions from raw IMU data.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def is_inertial_type_supported(self, inertial_type: type[InertialType]) -> bool:
+        """
+        Check if the plugin supports a given type of inertial.
+
+        Args:
+            inertial_type (type[InertialType])
+
+        Returns:
+            bool: ``True`` if inertial type is supported, ``False`` otherwise.
+        """
+        pass
+
+    @abstractmethod
+    def new_inertial(
+        self,
+        inertial_type: type[InertialType],
+        solution: Message,
+        config_group: str | None = None,
+    ) -> InertialType | None:
+        """
+        Create an instance of an implementation of :class:`pntos.api.CommonInertial`.
+
+        Args:
+            inertial_type (type[InertialType]): Specifies the type of inertial that the returned value will
+                support. For example, if the user passes in ``STANDARD_INERTIAL_MECHANIZATION``,
+                then the returned value will be an implementation of
+                :class:`pntos.api.StandardInertialMechanization`. If ``inertial_type`` is unsupported by the plugin,
+                then ``None`` will be returned. Please use :meth:`is_inertial_type_supported` to
+                check if the type is supported by the plugin.
+            solution (Message): The initial solution (i.e. the alignment) to mechanize from.
+            config_group (str | None, optional): An optional parameter which can be used to specify
+                which group in the config should be used to initialize the new inertial. This allows
+                for multiple inertial instances to exist with unique settings.
+
+        Returns:
+            InertialType | None: A new inertial object. Returns ``None`` if ``inertial_type`` is
+            unsupported, ``solution`` is invalid, or ``config_group`` is invalid.
+            :meth:`is_inertial_type_supported` can be called to verify ``inertial_type`` before calling this
+            method.
+        """
+        pass

pntos/api/plugins/initialization.py

@@ -0,0 +1,278 @@
+"""Python API of pntOS."""
+
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any, TypeVar
+
+from aspn23 import TypeTimestamp
+from numpy import float64
+from numpy.typing import NDArray
+
+from .common import CommonPlugin, EstimateWithCovariance, Message
+from .inertial import StandardInertialErrors
+
+
+class InitializationStatus(IntEnum):
+    """
+    An enumeration that allows the user to know the initialization status.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    WAITING = 0
+    """Waiting to start initialization process."""
+
+    INITIALIZING_COARSE = 1
+    """Attempting to initialize and produce a navigation solution."""
+
+    INITIALIZING_FINE = 2
+    """
+    A coarse initialization has been calculated by the algorithm, and the initialization is being
+	tested or adjusted before producing a navigation solution.
+    """
+
+    INITIALIZED_GOOD = 3
+    """
+    We have a good initialization.
+
+    The provided solution can be used to kickoff inertial and fusion.
+    """
+
+    INITIALIZATION_FAILED = 4
+    """The initialization process failed in some way, and may attempt to restart."""
+
+
+class InitializationMotionNeeded(IntEnum):
+    """
+    An enumeration that specifies what type of motion is required by the initialization strategy.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    NO_MOTION = 0
+    """Stationary data is needed."""
+
+    MOTION_NEEDED = 1
+    """Dynamic data is needed."""
+
+    ANY_MOTION = 2
+    """No particular type of motion is required."""
+
+
+class CommonInitializationStrategy(ABC):
+    """
+    A common base type for initialization algorithms.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def request_motion_needed(self) -> InitializationMotionNeeded:
+        """
+        Check the type of motion (if any) needed.
+
+        Returns:
+            InitializationMotionNeeded
+        """
+        pass
+
+    @abstractmethod
+    def request_current_status(self) -> InitializationStatus:
+        """
+        Check the current initialization status.
+
+        Returns:
+            InitializationStatus
+        """
+        pass
+
+    @abstractmethod
+    def process_pntos_message(self, message: Message) -> None:
+        """
+        Incorporate a new message into the initialization algorithm.
+
+        Args:
+            message (Message)
+        """
+        pass
+
+
+@dataclass
+class InitialInertialSolution:
+    """
+    Holds both the current solution, inertial errors (and their covariance), and the current status.
+
+    Coupling these avoids time-of-check to time-of-use (TOCTOU) issues.
+
+    Attributes:
+        solution (Message | None): The initial solution.
+        inertial_errors (StandardInertialErrors | None): The inertial errors.
+        inertial_error_covariance (NDArray[float64] | None): The covariance matrix associated with the terms
+            in ``inertial_errors``.
+        status (InitializationStatus): Indicates the current initialization status. Should be
+            checked before using any of the other fields.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    solution: Message | None
+    inertial_errors: StandardInertialErrors | None
+    inertial_error_covariance: NDArray[float64] | None
+    status: InitializationStatus
+
+
+class InertialInitializationStrategy(CommonInitializationStrategy, ABC):
+    """
+    Generates an initial ASPN solution from sensor data.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def request_solution(self) -> InitialInertialSolution:
+        """
+        Get the current initial solution.
+
+        Returns:
+            InitialInertialSolution
+        """
+        pass
+
+
+@dataclass
+class InitialEstimateWithCovariance:
+    """
+    Holds both the current estimate and its associated covariance as well as the current status.
+
+    Coupling these avoids time-of-check to time-of-use (TOCTOU) issues.
+
+    Attributes:
+        time (TypeTimestamp): The time at which ``estimate_with_covariance`` is valid.
+        estimate_with_covariance (EstimateWithCovariance | None): The current estimate of the
+            initial solution. Check ``status`` for its validity (can be ``None`` if ``status`` is
+            anything other than ``INITIALIZED_GOOD``).
+        status (InitializationStatus): Indicates the current initialization status. Should be
+            checked before using ``estimate_with_covariance``.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    time: TypeTimestamp
+    estimate_with_covariance: EstimateWithCovariance | None
+    status: InitializationStatus
+
+
+class EwcInitializationStrategy(CommonInitializationStrategy):
+    """
+    Generates an initial estimate-with-covariance (EWC) solution from sensor data.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def request_solution(self) -> InitialEstimateWithCovariance | None:
+        """
+        Get the current initial solution.
+
+        Returns:
+            InitialEstimateWithCovariance | None: The current initial solution. Will be ``None`` if
+            the initialization strategy has not yet finished. Use
+            :meth:`pntos.api.CommonInitializationStrategy.request_current_status` to check current status of
+            the strategy. If the status is ``INITIALIZING_FINE`` or ``INITIALIZED_GOOD``, then the
+            result is guaranteed to not be ``None``.
+        """
+        pass
+
+
+InitializationType = TypeVar(
+    'InitializationType',
+    InertialInitializationStrategy,
+    EwcInitializationStrategy,
+    Any,
+)
+"""
+An enumeration of the types of initializers an initializer plugin could provide.
+
+"Any" is included for future compatibility.
+"""
+
+
+class InitializationPlugin(CommonPlugin, ABC):
+    """
+    An implementation of an initialization plugin.
+
+    This plugin generates :class:`pntos.api.CommonInitializationStrategy` instances which may be used to
+    generate an initial solution from additional external sensor data, such as IMU data.
+
+    Caution:
+        **Unstable**: This feature is unstable and is not yet considered part of the stable pntOS
+        API. Usage of this feature is highly discouraged in non-experimental code, and its
+        definition may change at any time.
+    """
+
+    @abstractmethod
+    def is_initialization_type_supported(
+        self, initialization_type: type[InitializationType]
+    ) -> bool:
+        """
+        Check if given ``InitializationType`` is supported.
+
+        Args:
+            initialization_type (type[InitializationType])
+
+        Returns:
+            bool: ``True`` if the plugin supports a given type of mechanization, ``False``
+            otherwise.
+        """
+        pass
+
+    @abstractmethod
+    def new_initialization_strategy(
+        self,
+        initialization_type: type[InitializationType],
+        config_group: str | None = None,
+    ) -> InitializationType | None:
+        """
+        Create an instance of :class:`pntos.api.CommonInitializationStrategy`.
+
+        Args:
+            initialization_type (type[InitializationType]): Specifies the type of initializer that the returned
+                value will support. For example, if the user passes in
+                ``INERTIAL_INITIALIZATION_STRATEGY``, then the returned value will be an
+                implementation of :class:`pntos.api.InertialInitializationStrategy`. If ``initialization_type`` is
+                unsupported by the plugin, then ``None`` will be returned. Please use
+                :meth:`is_initialization_type_supported` to check if the type is supported by the
+                plugin.
+            config_group (str | None, optional): An optional parameter which can be used to specify
+                which group in the config should be used to set up the new initialization strategy.
+                This allows for multiple initialization strategy instances to exist with unique
+                settings.
+
+        Returns:
+            InitializationType | None: The new initialization strategy object. Returns ``None``
+            if ``initialization_type`` is unsupported by this plugin (this can be checked using
+            :meth:`is_initialization_type_supported`) or if ``config_group`` is invalid.
+        """
+        pass