stateExtrapolation

Functions

static inline PlanetSpin planetSpin(const Eigen::Matrix3d &dcm_NPfix, const Eigen::Matrix3d &dcm_NPfix_dot)

Extract the inertial angular velocity of a planet-fixed frame from its orientation and derivative.

dcm_NPfix_dot = [omega_N x] dcm_NPfix, hence [omega_N x] = dcm_NPfix_dot * dcm_NPfix^T.

Parameters:

• dcm_NPfix – [-] [NP] orientation of the inertial frame relative to the planet-fixed frame, which maps planet-fixed components to inertial components

• dcm_NPfix_dot – [1/s] time derivative of dcm_NPfix

Returns:

the planet spin, with isZero set if the derivative is zero

static inline Eigen::Matrix3d advanceDcm(const Eigen::Matrix3d &dcm_NPfix, const PlanetSpin &spin, double dt)

Advance a planet-fixed orientation matrix by a time offset with a precomputed planet spin.

The orientation is rotated about the planet angular velocity by |omega| dt, with the Rodrigues formula, so the result stays orthonormal. No orientation or rate is recomputed, only the rotation for the given time offset.

Parameters:

• dcm_NPfix – [-] [NP] orientation to advance

• spin – planet spin from planetSpin() of the same orientation and its derivative

• dt – [s] signed time offset to advance the orientation by

Returns:

the advanced orientation matrix, or dcm_NPfix if the spin or the offset is zero

static inline Eigen::Matrix3d advanceDcmDot(const PlanetSpin &spin, const Eigen::Matrix3d &dcmAdvanced_NPfix, const Eigen::Matrix3d &dcm_NPfix_dot)

Advance the time derivative of a planet-fixed orientation with a precomputed planet spin.

With a constant inertial angular velocity, dcm_NPfix_dot = [omega_N x] dcm_NPfix holds at every epoch, so the derivative at the advanced epoch is [omega_N x] dcmAdvanced_NPfix. This assumes a consistent orientation and rate pair, for which dcm_NPfix_dot * dcm_NPfix^T is skew-symmetric.

Parameters:

• spin – planet spin from planetSpin()

• dcmAdvanced_NPfix – [-] [NP] orientation after the advance, from advanceDcm()

• dcm_NPfix_dot – [1/s] time derivative before the advance, returned if the spin is zero

Returns:

[1/s] the derivative at the advanced epoch

static inline bool isStepLagMessage(uint64_t currentSimNanos, uint64_t timeWrittenNanos, uint64_t previousUpdateNanos)

Returns true if a spacecraft state message is the output of the previous update of the environment module.

The message-age based extrapolation is only valid if the spacecraft message was written at the previous update of the environment module. Then the age of the message is exactly the update interval of the module, and half of the age is the middle of that interval. This holds if the module and the spacecraft are updated with the same task period. A message written at any other earlier time is either stale (for example a state written once at the start of the simulation), or the spacecraft is updated at a different task period than the environment module.

Parameters:

• currentSimNanos – [ns] current simulation time

• timeWrittenNanos – [ns] time the spacecraft state message was written

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

true if the message is older than the current time and was written at the previous module update

static inline SCStatesMsgPayload extrapolateScStateToStepMidpoint(const SCStatesMsgPayload &scState, uint64_t currentSimNanos, uint64_t timeWrittenNanos, uint64_t previousUpdateNanos)

Extrapolate a spacecraft state message to the middle of the interval the next spacecraft update integrates.

Environment modules (atmosphere, wind, magnetic field, eclipse, solar flux) run before the spacecraft within a task, so the state they read was written at the end of the previous step. The spacecraft update at the current time integrates the interval between that message time and the current time, so the best estimate of where the spacecraft is while the environment output acts is the state advanced by half of the message age. The position is advanced with the message velocity.

Note

The requirement concerns the task period, not the integrator. The environment module must be updated with the same task period as the spacecraft, so that the message written by the spacecraft at the previous module update is exactly one update interval old. The internal substeps of a variable-step integrator do not matter: they still cover the update interval requested of the spacecraft. The state is returned unchanged if the message was written at the current time, or if it was not written at the previous update of the environment module. The latter is either a stale message (written once at the start of the simulation), or a spacecraft that runs faster or slower than the environment module, for which half of the message age is not the middle of the module interval. ScStateExtrapolation additionally waits until the spacecraft period has been observed.

Parameters:

• scState – spacecraft state message payload as read from the message

• currentSimNanos – [ns] current simulation time

• timeWrittenNanos – [ns] time the spacecraft state message was written

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

copy of the payload with r_BN_N and r_CN_N advanced by v * 0.5 * (currentSimNanos - timeWrittenNanos)

static inline SpicePlanetStateMsgPayload extrapolatePlanetStateToEpoch(const SpicePlanetStateMsgPayload &planetState, uint64_t targetNanos, uint64_t timeWrittenNanos)

Move a planet state message to the epoch the spacecraft state was extrapolated to.

The relative position of the spacecraft with respect to a planet is only meaningful if both are evaluated at the same epoch. The position is advanced with the planet velocity, and the planet-fixed orientation is advanced with planetSpin() and advanceDcm(). The offset is signed: it is negative if the planet message is newer than the target epoch.

Note

An all-zero orientation, as left by a message that only sets position and velocity, is treated as the identity like in GravBodyData, so the returned orientation is always a valid rotation matrix.

Note

As in GravBodyData::computeGravityInertial(), the planet is advanced by the time between the message write and the target epoch, with no check on the age of the message. A message written once with a non-zero velocity is projected forward over the whole simulation.

Parameters:

• planetState – planet state message payload as read from the message

• targetNanos – [ns] epoch to evaluate the planet at

• timeWrittenNanos – [ns] time the planet state message was written

Returns:

copy of the payload with the position and the orientation advanced to the target epoch

struct PlanetSpin

#include <stateExtrapolation.h>

Angular velocity of a planet-fixed frame, derived once from an orientation and its derivative.

The planet angular velocity is constant while an orientation is extrapolated, so a caller that advances the same orientation repeatedly (for example at every integrator stage) computes it once with planetSpin() and reuses it with advanceDcm() and advanceDcmDot().

Public Members

Eigen::Matrix3d omegaTilde_N = Eigen::Matrix3d::Zero()

[1/s] dcm_NPfix_dot * dcm_NPfix^T, [omega_N x]

Eigen::Vector3d omega_N = Eigen::Vector3d::Zero()

[rad/s] inertial angular velocity of the planet

bool isZero = true

true if the orientation derivative is zero

class ScStateExtrapolation

#include <stateExtrapolation.h>

Opt-in switch and rate-mismatch monitor for the spacecraft state extrapolation of an environment module.

The extrapolation is disabled by default, in which case the state message is used as written. When enabled, the module calls prepare() once per update with the write times of all its spacecraft state messages. The spacecraft states are extrapolated with extrapolateScStateToStepMidpoint() and the planets with extrapolatePlanetStateToEpoch() only if all spacecraft messages were written at the previous module update and two successive write times were observed with an interval equal to the module update interval. The extrapolation is therefore deferred until the spacecraft period is known (the first updates are never extrapolated). If any spacecraft state message does not satisfy these conditions, no spacecraft is extrapolated, and a warning is logged once if a spacecraft message was not written at the previous module update, which is typically a task period mismatch. While no spacecraft state is extrapolated but all were written at the previous module update, the planets are moved back to that epoch, so the relative geometry stays consistent during the startup and under a period mismatch. A mismatch is not always detectable: a message written at the current module update (for example by a faster spacecraft that runs before the module) is used as written and gives no warning. No message that is not the output of the previous module update is ever extrapolated.

Public Functions

inline void setEnabled(bool enable)

Enable or disable the extrapolation.

Parameters:

enable – true to extrapolate the spacecraft state to the step midpoint

inline bool isEnabled() const

Returns whether the extrapolation is enabled.

Returns:

true if the spacecraft state is extrapolated

inline bool mismatchDetected() const

Returns whether the warning that a spacecraft message was not written at the previous module update was logged, which is typically a task period mismatch between the module and the spacecraft.

Returns:

true if the warning was logged since the last reset

inline std::vector<uint64_t> &writeTimesBuffer()

Returns an empty reusable buffer for the write times passed to prepare(), which avoids an allocation at every module update.

Returns:

empty vector, valid until the next call

inline void reset()

Clear the observed message history and re-arm the warning.

inline void prepare(uint64_t currentSimNanos, uint64_t previousUpdateNanos, const std::vector<uint64_t> &timesWrittenNanos, BSKLogger &logger)

Decide once per module update whether the extrapolation applies to all spacecraft and the planets.

The planet state messages are shared by all spacecraft of a module, so the decision cannot be made per spacecraft: a planet moved to the step midpoint is inconsistent with a spacecraft left at the message epoch, and the other way around. The extrapolation is therefore applied to every spacecraft and to the planets only if all spacecraft state messages were written at the previous module update (see isStepLagMessage()). If one of them was not, no spacecraft is extrapolated and a warning is logged once. The planets are still moved to the common epoch of the spacecraft messages if all of them were written at the previous module update.

Must be called once per update, before apply() and applyPlanet().

Parameters:

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

• timesWrittenNanos – [ns] write time of each spacecraft state message, in the order of the apply() indexes

• logger – logger of the calling module

template<typename Messages>
inline void prepareFromMessages(uint64_t currentSimNanos, uint64_t previousUpdateNanos, Messages &scStateInMsgs, BSKLogger &logger)

Call prepare() with the write times read from the spacecraft state input messages.

Template Parameters:

Messages – range of read functors that provide timeWritten()

Parameters:

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

• scStateInMsgs – spacecraft state input messages, in the order of the apply() indexes

• logger – logger of the calling module

template<typename Message>
inline void prepareFromMessage(uint64_t currentSimNanos, uint64_t previousUpdateNanos, Message &scStateInMsg, BSKLogger &logger)

Call prepare() for a module with a single spacecraft state input message.

Template Parameters:

Message – read functor that provides timeWritten()

Parameters:

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

• scStateInMsg – spacecraft state input message

• logger – logger of the calling module

template<typename Message>
inline SCStatesMsgPayload applyMessage(Message &scStateInMsg, uint64_t currentSimNanos, uint64_t previousUpdateNanos) const

Read a spacecraft state input message and apply() the extrapolation to it.

Template Parameters:

Message – read functor that returns an SCStatesMsgPayload and provides timeWritten()

Parameters:

• scStateInMsg – spacecraft state input message

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

the payload, with the position advanced to the step midpoint if the extrapolation applies

template<typename Message>
inline SpicePlanetStateMsgPayload applyPlanetMessage(Message &planetInMsg, uint64_t currentSimNanos, uint64_t previousUpdateNanos) const

Read a planet state input message and applyPlanet() the extrapolation to it.

Template Parameters:

Message – read functor that returns a SpicePlanetStateMsgPayload and provides timeWritten()

Parameters:

• planetInMsg – planet state input message

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

the planet payload moved to the epoch of the spacecraft states

inline SCStatesMsgPayload apply(const SCStatesMsgPayload &scState, uint64_t currentSimNanos, uint64_t timeWrittenNanos, uint64_t previousUpdateNanos) const

Return the spacecraft state, extrapolated if prepare() found all spacecraft consistent.

Parameters:

• scState – spacecraft state message payload as read from the message

• currentSimNanos – [ns] current simulation time

• timeWrittenNanos – [ns] time the spacecraft state message was written

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

the payload, with the position advanced to the step midpoint if the extrapolation applies

inline SpicePlanetStateMsgPayload applyPlanet(const SpicePlanetStateMsgPayload &planetState, uint64_t currentSimNanos, uint64_t timeWrittenNanos, uint64_t previousUpdateNanos) const

Return the planet state at the epoch the spacecraft states were extrapolated to.

The planet is moved to the middle of the module update interval only if prepare() found the extrapolation applicable to all spacecraft, so that the relative geometry of every spacecraft is evaluated at one epoch. If all spacecraft messages were written at the previous module update but the extrapolation does not apply (the spacecraft period is not yet known, or differs from the module period), the spacecraft states stay at that epoch and the planet is moved back to it. Otherwise the planet is returned as written.

Note

prepare() must be called first in every update.

Parameters:

• planetState – planet state message payload as read from the message

• currentSimNanos – [ns] current simulation time

• timeWrittenNanos – [ns] time the planet state message was written

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

the planet payload with the position and the orientation advanced to the step midpoint

inline uint64_t evaluationEpochNanos(uint64_t currentSimNanos, uint64_t previousUpdateNanos) const

Return the epoch the spacecraft and planet states returned by apply() and applyPlanet() are evaluated at.

Time dependent models (for example the local solar time of an atmosphere model) must use the same epoch as the geometry. It is the middle of the module update interval if the extrapolation applies, the previous module update if all spacecraft messages were written then, and the current time otherwise.

Note

prepare() must be called first in every update.

Parameters:

• currentSimNanos – [ns] current simulation time

• previousUpdateNanos – [ns] time of the previous update of the calling environment module

Returns:

[ns] evaluation epoch

Private Functions

inline SpicePlanetStateMsgPayload movePlanetToEpoch(const SpicePlanetStateMsgPayload &planetState, uint64_t currentSimNanos, uint64_t timeWrittenNanos, uint64_t previousUpdateNanos) const

Move the planet to the epoch selected by prepare(), see applyPlanet().

Private Members

bool enabled = false

true if the spacecraft state is extrapolated

bool warned = false

true once the rate mismatch warning was logged

bool lagConsistent = false

true if all spacecraft messages were written at the previous update and their period is known

bool allStepLag = false

true if all spacecraft messages were written at the previous update

std::vector<uint64_t> lastWriteNanos = {}

[ns] last observed write time of each state message

std::vector<uint64_t> writeIntervalNanos = {}

[ns] last observed interval between two write times, 0 if unknown

std::vector<bool> rewritten = {}

true if the state message was seen with more than one write time

std::vector<uint64_t> timesBuffer = {}

[ns] reusable buffer for the write times passed to prepare()

std::vector<bool> seen = {}

true if the state message was observed in a previous update