sen::util¶

This library houses Sen tools that, while not part of the Sen core, are frequently used within Sen environments.

This library currently contains only Dead Reckoning utilities.

Check out the API Reference for a detailed description of the Sen Util library.

Dead Reckoning utilities¶

What is it?¶

Dead Reckoning is a way to cut data transmission in distributed simulations and networked applications. Instead of transmitting real time positions continuously, each participant predicts where an entity has moved between infrequent state updates: it extrapolates the entity forward with a kinematic model, from the last position, velocity and acceleration it received. Bandwidth drops, and the entities still appear to be in step.

This library enables the computation of Dead Reckoning extrapolations on entities by using these two APIs:

• The base API where a custom Situation struct is used for the extrapolation.
• An API adapted to RPR which takes a BaseEntity object instance and extrapolates its spatial situation following the algorithms specified in IEEE 1278.1-2012, Annex E.

Additionally, this library allows users to update BaseEntity object instances based on error between real-time data and extrapolated values, thereby reducing spatial data transmission.

How to use it?¶

Information on how to use the Dead Reckoning library can be found in this guide.

Detailed design¶

This section targets readers interested in the detailed design of the Dead Reckoning library.

Our main goals¶

The primary objectives that motivated the design of this library are as follows:

• Coverage: This library extrapolates position and orientation data for all the Dead Reckoning algorithms contemplated in the IEEE 1278.1-2012, Annex E.

• Performance: Dead Reckoning extrapolations are usually computed at a frequency higher than the network's frequency, thus making efficiency a key requirement of this library. All the algorithms were implemented paying attention to their computational performance, e.g., using quaternions instead of matrix multiplications for rotations.

• User Friendly: The API of the library was made as simple as possible for the user, only requiring the user to provide a simple Smoothing/Threshold configuration and a reference to the Sen object (local/remote).

• A library, not a package: This software is a Sen library, but it is not a package. This can be confusing because Sen packages are also .so files (shared library). Being a Sen library means that it provides a set of helpers which can be used by another package, but it cannot be instantiated directly by the Sen kernel.

A general diagram of the library is shown below:

classDiagram
    class DeadReckonerBase {
        +situation(timeStamp) Situation
        +geodeticSituation(timeStamp) GeodeticSituation
        +updateSituation(Situation)
        +updateGeodeticSituation(GeodeticSituation)
    }
    class DeadReckonerTemplateBase~T~ {
        #extrapolate(SpatialVariant, time, lastTimeStamp) Situation
    }
    class DeadReckoner~T~ {
        +situation(timeStamp) override Situation
        +geodeticSituation(timeStamp) override GeodeticSituation
        +getObject() T
    }
    class DrConfig {
        smoothing
        maxDistance
        maxDeltaTime
        smoothingInterval
        positionConvergenceTime
        positionDamping
        orientationConvergenceTime
        orientationDamping
    }
    class SettableDeadReckoner~T~ {
        +setSpatial(situation) bool
        +setSpatial(geodeticSituation) bool
        +setFrozen(timeStamp, isFrozen)
        +object() T
    }
    class DrThreshold {
        distanceThreshold
        orientationThreshold
        referenceSystem
    }
    DeadReckonerBase <|-- DeadReckonerTemplateBase~T~
    DeadReckonerTemplateBase~T~ <|-- DeadReckoner
    DeadReckonerTemplateBase~T~ <|-- SettableDeadReckoner
    DrConfig --o DeadReckonerBase : 1
    SettableDeadReckoner o-- DrThreshold : 1

The DeadReckonerBase class encapsulates the core functionality of the dead reckoning, and that class can be directly used to extrapolate (and optionally smooth) any Situation that does not need to be coming from the RPR Spatial attribute.

The DeadReckonerTemplateBase<T> class particularizes this functionality for RPR object instances and the DeadReckoner<T> and SettableDeadReckoner<T> classes inherit from it.

Data models and configuration¶

The dead reckoning library extrapolates the position and orientation of an entity from a situation. Situation and GeodeticSituation differ only in the frame the position and orientation are expressed in: the first uses ECEF, the second latitude/longitude/altitude with orientation relative to NED.

sen::util::Situation: Cartesian, earth-centered earth-fixed.

sen::util::GeodeticSituation: identical apart from the three rows below.

sen::util::DrConfig: the smoothing configuration. The defaults are tuned to work in most scenarios; the how-to guide explains which way to turn each one.

The lengths, angles and rates above are quantity types (LengthMeters, AngleRadians, VelocityMetersPerSecond and so on), so the unit is part of the type rather than a convention you have to remember.

In C++ a quantity is a wrapper over its underlying type that converts to and from it implicitly, so you can pass one wherever the plain number is expected and assign a plain number back. The unit is enforced where values cross between components, not inside arithmetic you write yourself.

/// Situation structure with the following parameters:
struct Situation
{
  /// When true, no extrapolation is performed because the entity is frozen.
  bool isFrozen = false;

  /// TimeStamp of the instant when the situation is computed.
  sen::TimeStamp timeStamp;

  ///  Position in ECEF.
  Location worldLocation {};

  /// Orientation of the body reference system (x forward, y right, z down) with respect to ECEF.
  Orientation orientation {};

  /// Velocity vector with respect to ECEF or body reference system (depending on
  /// the reference system of the algorithm extrapolated).
  Velocity velocityVector {};

  /// Angular velocity vector with respect to body reference system.
  AngularVelocity angularVelocity {};

  /// Acceleration vector with respect to ECEF or body reference system (depending on
  /// the reference system of the algorithm extrapolated).
  Acceleration accelerationVector {};

  /// Angular acceleration vector with respect to body reference system.
  AngularAcceleration angularAcceleration {};
};

/// GeodeticSituation structure with the following parameters:
struct GeodeticSituation
{
  /// When true, no extrapolation is performed because the entity is frozen.
  bool isFrozen = false;

  /// TimeStamp of the instant when the situation is computed.
  sen::TimeStamp timeStamp;

  /// World Location in Geodetic (Latitude, Longitude, Altitude).
  GeodeticWorldLocation worldLocation {};

  /// Orientation of the body reference system (x forward, y right, z down)
  /// with respect to NED (North - East - Down).
  Orientation orientation {};

  /// Velocity vector with respect to NED.
  Velocity velocityVector {};

  /// Angular velocity vector with respect to body-reference system.
  AngularVelocity angularVelocity {};

  ///  Acceleration vector with respect to NED.
  Acceleration accelerationVector {};

  /// Angular acceleration vector with respect to body-reference system.
  AngularAcceleration angularAcceleration {};
};

/// Dead Reckoning configuration.
struct DrConfig
{
  /// If true, the position and orientation of the input data is smoothed removing noise
  bool smoothing = true;

  ///  No smoothing is performed for displacements bigger than this distance.
  LengthMeters maxDistance = 100000.0;

  /// No smoothing is performed for time deltas bigger than this duration
  sen::Duration maxDeltaTime {std::chrono::seconds(1)};

  /// Maximum time interval used to update the smoothed solution. It is used to prevent the smoothed solution from
  /// becoming unstable.
  sen::Duration smoothingInterval {std::chrono::milliseconds(20)};

  /// Convergence time for the smoothed position to match the updated position
  sen::Duration positionConvergenceTime {std::chrono::milliseconds(500)};

  /// Damping coefficient for the smoothed position solution.
  DampingCoefficient positionDamping = 1.0;

  /// Convergence time for the smoothed orientation to match the updated orientation
  sen::Duration orientationConvergenceTime {std::chrono::milliseconds(50)};

  /// Damping coefficient for the smoothed orientation solution.
  DampingCoefficient orientationDamping = 20.0;

  /// Measure the extrapolation from the instant the producer committed the data rather than from
  /// the instant it was first read. Set to false when the producer's clock is not the caller's.
  bool useCommitTimeAsOrigin = true;

  /// Which algorithm DeadReckonerBase applies. A Situation does not carry one, so it has to be told;
  /// the classes taking an RPR Spatial read it from the data and ignore this. Leave the fields the
  /// chosen algorithm does not own at zero: world position always applies the acceleration term.
  SpatialAlgorithm algorithm = SpatialAlgorithm::drRVW;
};

The Dead Reckoner Base¶

The DeadReckonerBase has the following API:

/// Extrapolates the Situation of an entity at a certain time. The extrapolation is smoothed by
/// default unless the user specifies otherwise.
///
/// Extrapolates with drRvw only: a Situation does not carry which of the nine algorithms describes
/// its movement. DeadReckonerTemplateBase takes the Spatial variant, which does, and selects from it.
class DeadReckonerBase
{

public:
  SEN_NOCOPY_NOMOVE(DeadReckonerBase)

public:
  explicit DeadReckonerBase(DrConfig config = {});
  virtual ~DeadReckonerBase() = default;

public:
  /// Returns the extrapolated/smoothed situation of the object at the timestamp introduced as argument, expressed in
  /// the following coordinates:
  /// - World position: ECEF coordinates
  /// - Orientation: Euler angles of the body reference system (x forward, y right, z down) with respect to ECEF
  /// - Velocity: In world coordinates for world-centered DR algorithms and in body coordinates for body-centered
  /// algorithms
  /// - Acceleration: In world coordinates for world-centered DR algorithms and in body coordinates for body-centered
  /// algorithms
  /// - AngularVelocity: In body coordinates.
  /// - AngularAcceleration: In body coordinates.
  [[nodiscard]] virtual Situation situation(sen::TimeStamp timeStamp);

  /// Returns the extrapolated/smoothed situation of the object at the timestamp introduced as argument, expressed in
  /// the following coordinates:
  /// - World position: Geodetic Latitude, Longitude and Altitude
  /// - Orientation: Euler angles of the body reference system (x forward, y right, z down) with respect to NED
  /// - Velocity: In NED coordinates for world-referenced algorithms, in body coordinates for body-referenced
  /// algorithms.
  /// - Acceleration: In NED coordinates for world-referenced algorithms, in body coordinates for body-referenced
  /// algorithms.
  /// - AngularVelocity: In body coordinates.
  /// - AngularAcceleration: In body coordinates.
  [[nodiscard]] virtual GeodeticSituation geodeticSituation(sen::TimeStamp timeStamp);

  /// Updates the last known real time Situation. A valid timestamp is needed inside the Situation provided as
  /// argument.
  void updateSituation(const Situation& value);

  /// Updates the last known real time GeodeticSituation. A valid timestamp is needed inside the GeodeticSituation
  /// provided as argument.
  void updateGeodeticSituation(const GeodeticSituation& value);

public:  // config
  [[nodiscard]] const DrConfig& getConfig() const noexcept;
  void setConfig(const DrConfig& config);

protected:
  [[nodiscard]] const Situation& getSmoothSituation() const noexcept;
  void smooth(const Situation& update);

  /// Smooths the update when configured to and returns what to publish. With smoothing off the walk
  /// is skipped and the returned reference is the argument itself.
  [[nodiscard]] const Situation& smoothIfEnabled(const Situation& update);

  void invalidateCache();

  [[nodiscard]] bool isSituationCached(sen::TimeStamp timeStamp) const noexcept;
  void setCachedSituation(const Situation& situation);
  [[nodiscard]] const Situation& getCachedSituation() const noexcept;

  [[nodiscard]] bool isGeodeticSituationCached(sen::TimeStamp timeStamp) const noexcept;
  void setCachedGeodeticSituation(const GeodeticSituation& situation);
  [[nodiscard]] const GeodeticSituation& getCachedGeodeticSituation() const noexcept;

private:
  DrConfig config_;
  Situation lastSituation_;
  Situation smoothSituation_;

  // cache
  Situation cachedSituation_;
  GeodeticSituation cachedGeodeticSituation_;
};

Where the user needs to call updateSituation (with a situation containing a valid timestamp) every time the last-known data can be updated. Calling situation or geodeticSituation retrieves the position/orientation extrapolated to the timestamp provided as argument.

The Dead Reckoner¶

The DeadReckoner<T> provides an API to the user tailored to accept rpr::BaseEntityInterface object instances and perform extrapolations on them following the algorithms specified in IEEE 1278.1-2012, Annex E. Their updates are detected automatically in this case. The API is shown below:

template <typename T>
class DeadReckoner: public DeadReckonerTemplateBase<T>
{
public:
  SEN_NOCOPY_NOMOVE(DeadReckoner)

public:  // RPR types from DeadReckonerTemplateBase
  using Parent = DeadReckonerTemplateBase<T>;
  using SpatialVariant = typename Parent::SpatialVariant;
  using StaticSpatial = typename Parent::StaticSpatial;
  using FpsSpatial = typename Parent::FpsSpatial;
  using RpsSpatial = typename Parent::RpsSpatial;
  using RvsSpatial = typename Parent::RvsSpatial;
  using FvsSpatial = typename Parent::FvsSpatial;
  using RprLocation = typename Parent::RprLocation;
  using RprOrientation = typename Parent::RprOrientation;
  using RprVelocity = typename Parent::RprVelocity;
  using RprAcceleration = typename Parent::RprAcceleration;
  using RprAngularVelocity = typename Parent::RprAngularVelocity;

public:  // type aliases
  using SituationProcessor = std::function<Situation(sen::TimeStamp)>;
  using GeodeticSituationProcessor = std::function<GeodeticSituation(sen::TimeStamp)>;

public:
  /// Constructor for the DeadReckoner where an object inheriting from rpr::BaseEntity is inputted as a reference.
  /// This is the easiest version of the API to instantiate a DeadReckoner.
  explicit DeadReckoner(const T& object, DrConfig config = {});
  ~DeadReckoner() override = default;

public:  // overrides DeadReckonerBase
  [[nodiscard]] Situation situation(sen::TimeStamp timeStamp) override;
  [[nodiscard]] GeodeticSituation geodeticSituation(sen::TimeStamp timeStamp) override;

public:
  /// Provides direct mutable access to the internal object managed by this instance of the DeadReckoner
  [[nodiscard]] T& getObject() noexcept;

public:  // situation translation helpers
  /// Translates a SpatialVariant to a Situation struct
  [[nodiscard]] static Situation toSituation(const SpatialVariant& spatial, sen::TimeStamp timeStamp = {});

  /// Translates a SpatialVariant to a GeodeticSituation struct
  [[nodiscard]] static GeodeticSituation toGeodeticSituation(const SpatialVariant& spatial,
                                                             sen::TimeStamp timeStamp = {});

private:
  /// Extrapolates the held spatial to the given instant. Smoothing is only available for world
  /// centered algorithms, so a body referenced spatial is returned unsmoothed.
  [[nodiscard]] Situation extrapolateAt(sen::TimeStamp time);

  /// True when the spatial's algorithm is body referenced, which decides how velocities are
  /// converted and whether smoothing applies.
  [[nodiscard]] static bool isBodyReferenced(const SpatialVariant& spatial) noexcept;

  /// Updates the lastSpatial and lastTimeStamp members when a new Spatial is received
  void updateSpatial(sen::TimeStamp time);

  /// The instant the held spatial was produced, which is what the extrapolation measures from.
  /// Falls back to the query instant when the object cannot report a usable one.
  [[nodiscard]] sen::TimeStamp spatialOrigin(sen::TimeStamp time) const noexcept;

private:
  const T& object_;
  sen::TimeStamp lastTimeStamp_;
  SpatialVariant lastSpatial_;
};

The following diagram shows how the dead reckoner performs extrapolations of the rpr::BaseEntity object:

sequenceDiagram
    User-->>DeadReckoner: situation(timeStamp)
    DeadReckoner->>User: extrapolated/smoothed situation
    BaseEntityInterface->>BaseEntityInterface: Spatial update
    BaseEntityInterface->>DeadReckoner: Situation update
    User-->>DeadReckoner: situation(timeStamp)
    DeadReckoner->>User: extrapolated/smoothed situation
    User-->>DeadReckoner: situation(timeStamp)
    DeadReckoner->>User: extrapolated/smoothed situation
    User-->>DeadReckoner: situation(timeStamp)
    DeadReckoner->>User: extrapolated/smoothed situation
    BaseEntityInterface->>BaseEntityInterface: Spatial update
    BaseEntityInterface->>DeadReckoner: Situation update

The Settable Dead Reckoner¶

The main functionality of the SettableDeadReckoner<T> is to allow an application to write Spatial data in a local object of type T (inheriting from BaseEntityBase) when the difference between the extrapolation and the new Spatial updates is bigger than a configurable threshold. This facilitates the reduction of the load in the Sen network, by only updating the object when necessary.

The Settable Dead Reckoner takes the DrThreshold as a configuration. The definition of the DrThreshold is shown below:

/// How far the extrapolation may drift from the data before the Spatial is written: a distance per axis and an angle
/// between orientations, both compared in ECEF. The reference system selects which algorithm family the written
/// Spatial is tagged with, not the frame of the comparison.
struct DrThreshold
{
  LengthMeters distanceThreshold {1.0};
  AngleRadians orientationThreshold {0.05f};
  ReferenceSystem referenceSystem {ReferenceSystem::world};
};

Both thresholds default to a value that already suppresses most updates, one meter and 0.05 radians (about 2.9 degrees), so the reduction in traffic happens without any configuration. Setting either to zero has the opposite effect of what this class is for: every update would exceed it.

As you can see, it enables the configuration of the following parameters:

• Distance Threshold: Minimum distance in meters between the extrapolation and the new position updates (in any of the components of the position x, y or z) to trigger a set of the Spatial property of the local object. The distance threshold can be expressed with the following expressions, where \(new\) indicates updates in the position and \(dr\) extrapolated positions.

• Orientation Threshold: Minimum angle in radians between the extrapolation and new orientation updates to trigger a set of the Spatial property of the local Sen object. The orientation threshold can be easily computed using quaternions with the following expressions, where \(q_{new}\) is the quaternion representing the new orientation update, \(q_{dr}\) is the quaternion representing the extrapolated orientation and \(\beta\) is the angle between both orientations:

• Reference System: Reference system used for the Spatial property of the local BaseEntityBase object. Spatial dead reckoning algorithms use two different reference systems, body (x forward, y right, z down) and world (ECEF). This setting picks which of the two the written Spatial is tagged with, and it applies to both setSpatial overloads. The GeodeticSituation overload converts its position, orientation, velocity and acceleration to ECEF before writing them, so leaving this at its world default is what matches that data; setting it to body on that path produces a Spatial holding world vectors under a body-frame algorithm. It does not change what the two thresholds above are compared in: those are always measured per axis in ECEF, whichever reference system you choose.

The two main methods of the SettableDeadReckoner<T> class are the two overloads of the setSpatial method, which take a Situation and a GeodeticSituation as inputs. These two overloads allow the consumer to input the Situation in two possible reference systems.

Both return true when the call published a new Spatial and false when the extrapolation was still within the threshold, which is the common case for anything in near-linear motion. You can ignore the return, and most callers do. It is there for a producer that needs to know whether it wrote on this cycle: instrumentation that must not report activity on a cycle where the object published nothing, for instance. Note that it reports what was published, not whether the situation you passed in differed from the last one.

As you can see in the algorithms annex, the Spatial property of a BaseEntity can use multiple DeadReckoning algorithms. The SettableDeadReckoner<T> automatically selects the algorithm that will be used by the Spatial dynamically depending on the data inputted in the Situation, e.g, if an input situation's acceleration vector is null, a first order extrapolation of the position will be chosen.

sequenceDiagram
    User->>SettableDeadReckoner: setSpatial(situation)
    User->>SettableDeadReckoner: setSpatial(situation)
    User->>SettableDeadReckoner: setSpatial(situation)
    SettableDeadReckoner->>BaseEntityBase: setNextSpatial(value)
    User->>SettableDeadReckoner: setSpatial(situation)
    User->>SettableDeadReckoner: setSpatial(situation)
    SettableDeadReckoner->>BaseEntityBase: setNextSpatial(value)

You can see the functionality of the SettableDeadReckoner<T> in the diagram above. The setNextSpatial is only performed in the local Sen object (rpr::BaseEntityBase) when the threshold of the difference between a local extrapolation and the new Spatial values is exceeded.

Smoothing¶

In applications where the smoothness of the spatial data is key, such as IGs, we recommend enabling smoothing as the extrapolation by itself makes rough corrections every time new data is received from the bus.

The smoothing algorithm simply filters all discontinuities in the input spatial data by holding an internal smoothed situation that updates its acceleration depending on its total error with respect to the input data. The following diagram depicts the smoothing of the x location. The same smoothing algorithm is applied to both position and orientation:

Updating the acceleration helps to avoid discontinuities in the solution because we perform a second order integration. The condition to update the acceleration is that the smoothed solution needs to converge with the input data after a convergence time. This directly translates to higher accelerations as the error increases. The smoothed solution can be unstable for small convergence times, which is why the update is advanced in steps no longer than smoothingInterval, 20 ms by default. The convergence time you configure is used as given.

In addition to the convergence time, a damping is applied to the smoothed solution to avoid overshooting. This coefficient is configurable via DrConfig, and the defaults are tuned to damp the position and orientation solutions well. Changing them is not recommended unless you need to.

Finally, when the error between the smoothed solution and the input data surpasses a maximum distance threshold (configurable in the DrConfig), the error is corrected instantly without smoothing. This applies, for example, when the IOS applies big corrections to the location of the entities.

Dead Reckoning algorithms¶

Here we detail the extrapolation algorithms implemented in this library. This library covers all the algorithms specified on IEEE 1278.1-2012, Annex E.

Static: No movement.

FPW: Linear extrapolation of the position, not considering rotation and using the world reference system.

RPW: Linear extrapolation of the position, considering rotation and using the world reference system.

RVW: Second order extrapolation of the position, considering rotation and using the world reference system.

FVW: Second order extrapolation of the position, not considering rotation and using the world reference system.

FPB: First order extrapolation of the position, not considering rotation and using the body reference system.

RPB: First order extrapolation of the position, considering rotation and using the body reference system.

RVB: Second order extrapolation of the position, considering rotation and using the body reference system.

FVB: Second order extrapolation of the position, not considering rotation and using the body reference system.