C++ Module: imuSensor
Executive Summary
Sensor model to simulate an IMU.
The accelerometer and gyro error states evolve independently according to
where PMatrixAccel and PMatrixGyro are matrix square roots of the
respective process-noise covariance matrices. AMatrixAccel and
AMatrixGyro both default to zero, so configuring only a PMatrix
produces independent white Gaussian measurement noise. The accelerometer and
gyro use separate repeatable random streams derived from RNGSeed.
Correlated processes and random walks must be configured explicitly with
setAMatrixAccel() or setAMatrixGyro(). For example, the following
configuration creates a bounded gyro random walk:
imu.setAMatrixGyro([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
imu.setErrorBoundsGyro([0.01, 0.01, 0.01]) # [rad/s]
Each positive error-bound entry is an exact hard bound on the corresponding error state. A non-positive entry disables clipping for that state.
The module
PDF Description
contains further information on this module’s function,
how to run it, as well as testing.
The corruption types are outlined in this
PDF Description.
Delta-v Output
DVFramePlatform is the change of the inertial velocity of the sensor due to non-gravitational acceleration
between two consecutive IMU calls, resolved in the platform frame at the current call:
where \({}^{N}\mathbf{DV}_{C/N}\) is TotalAccumDV_CN_N of the spacecraft state message and
\(\Delta\) is the difference between the current and the previous IMU call. The second term is the change of
the velocity of the sensor relative to the center of mass. For an offset sensor, this term depends on the
attitude at both calls through the sensor’s rotational velocity. The accumulated center-of-mass delta v is
differenced in inertial coordinates, so changes in attitude do not turn previously accumulated delta v into a
spurious increment. Because the calculation uses accumulated values and the states at the IMU calls, the IMU
may run at a slower rate than the dynamics.
The following points apply:
sensorPos_Bis the sensor position relative to the body frame origin \(B\), not to the center of mass. If the center of mass is offset from \(B\), a sensor at \(B\) also senses the rotation of \(B\) about the center of mass, so its delta v is not zero during a coast.The gravity of C++ Module: spacecraft is evaluated at the center of mass and is uniform across the spacecraft, so the tidal acceleration at a sensor offset from the center of mass is not modeled.
The input message must fill
TotalAccumDV_CN_N,v_BN_Nandv_CN_Nthe way C++ Module: spacecraft does. Other producers ofSCStatesMsgPayload, such as MJSite, do not accumulate delta v and set these fields to zero or omit the non-gravitational part, so the delta v output is not meaningful with them.With
pointMassTranslationalOnly, C++ Module: spacecraft does not accumulate delta v, so the delta v output is zero even when a force acts on the spacecraft.A velocity changed directly through the spacecraft states, for example an impulsive maneuver applied with
setState(), is not part of the accumulated delta v and is not sensed.
Note
On the first run the module outputs a zero’d message. Only on the 2nd run onwards does the sensor
have IMU data. The same applies to the first run after a call to Reset().
Message Connection Descriptions
The following table lists all the module input and output messages. The module msg connection is set by the user from python. The msg type contains a link to the message structure definition, while the description provides information on what this message is used for.
Msg Variable Name |
Msg Type |
Description |
|---|---|---|
scStateInMsg |
input message name for spacecraft state |
|
sensorOutMsg |
output message name for IMU output data |
-
class ImuSensor : public SysModel
- #include <imuSensor.h>
An IMU sensor model that simulates accelerometer and gyro measurements with configurable noise.
The IMU sensor supports various noise configurations through:
PMatrix: Matrix square root of the process-noise covariance
AMatrix: Propagation matrix for the error model
Error bounds: Optional hard bounds on the propagated error states
Default behaviors:
AMatrixGyro: Initialized as a 3x3 zero matrix for white noise
AMatrixAccel: Initialized as a 3x3 zero matrix for white noise
Hard clipping: Disabled by default
Noise matrices: Set to zero by default
Example Python usage:
imu = imuSensor.ImuSensor() # Configure accelerometer noise (m/s^2) imu.PMatrixAccel = [[0.001, 0.0, 0.0], [0.0, 0.001, 0.0], [0.0, 0.0, 0.001]] # Configure gyro noise (rad/s) imu.PMatrixGyro = [[0.0001, 0.0, 0.0], [0.0, 0.0001, 0.0], [0.0, 0.0, 0.0001]] # Optional: configure a bounded gyro random walk explicitly imu.setAMatrixGyro([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) imu.setErrorBoundsGyro([0.01, 0.01, 0.01])
Public Functions
-
ImuSensor()
-
~ImuSensor()
-
void Reset(uint64_t CurrentSimNanos)
Reset the module
- Parameters:
CurrentSimNanos – current time (ns)
-
void UpdateState(uint64_t CurrentSimNanos)
update module states
- Parameters:
CurrentSimNanos – current time (ns)
-
void readInputMessages()
read input messages
-
void writeOutputMessages(uint64_t Clock)
write output messages
-
void setBodyToPlatformDCM(double yaw, double pitch, double roll)
set body orientation DCM relative to platform
-
void computePlatformDR()
This function gathers actual spacecraft attitude from the spacecraft output message. It then differences the state attitude between this time and the last time the IMU was called to get a DR (delta radians or delta rotation) The angular rate is retrieved directly from the spacecraft output message and passed through to theother IMU functions which add noise, etc.
-
void computePlatformDV(uint64_t CurrentTime)
This function computes the sensed acceleration and the delta-v between IMU calls. The acceleration of point B is read from the spacecraft output message, the terms due to the offset of the sensor from point B are added and the result is converted to the platform frame. The delta-v is the change of the inertial velocity of the sensor due to non-gravitational acceleration since the last IMU call. It is the sum of the change of TotalAccumDV_CN_N, which the spacecraft accumulates in inertial components, and the change of the sensor velocity relative to the center of mass. The sum is converted to the platform frame with the current attitude, so a rotation never turns previously accumulated delta-v into a new delta-v.
- Parameters:
CurrentTime – [ns] current simulation time
-
void applySensorErrors(uint64_t CurrentTime)
apply sensor errors
-
void applySensorDiscretization(uint64_t CurrentTime)
apply sensor direction
- Parameters:
CurrentTime –
-
void applySensorSaturation(uint64_t CurrentTime)
apply sensor saturation
-
void computeSensorErrors()
compute sensor errors
-
void scaleTruth()
scale truth method
-
void setLSBs(double LSBa, double LSBo)
set LSB values
-
void setCarryError(bool aCarry, bool oCarry)
set Carry error value
-
void setRoundDirection(roundDirection_t aRound, roundDirection_t oRound)
set round direction value
-
void setAMatrixAccel(const Eigen::MatrixXd &propMatrix)
Setter for
AMatrixAccel- Parameters:
propMatrix – Matrix to set
-
void setAMatrixGyro(const Eigen::MatrixXd &propMatrix)
Setter for
AMatrixGyro- Parameters:
propMatrix – Matrix to set
-
Eigen::MatrixXd getAMatrixAccel() const
Getter for
AMatrixAccel- Returns:
Current matrix
-
Eigen::MatrixXd getAMatrixGyro() const
Getter for
AMatrixGyro- Returns:
Current matrix
-
void setWalkBoundsAccel(const Eigen::Vector3d &bounds)
Setter for
walkBoundsAccel- Parameters:
bounds – Bounds vector to set
-
void setWalkBoundsGyro(const Eigen::Vector3d &bounds)
Setter for
walkBoundsGyro- Parameters:
bounds – Bounds vector to set
-
void setErrorBoundsAccel(const Eigen::Vector3d &bounds)
Sets accelerometer error bounds [m/s^2]
Setter for
errorBoundsAccel- Parameters:
bounds – Bounds vector to set
-
void setErrorBoundsGyro(const Eigen::Vector3d &bounds)
Sets gyro error bounds [rad/s]
Setter for
errorBoundsGyro- Parameters:
bounds – Bounds vector to set
Public Members
-
ReadFunctor<SCStatesMsgPayload> scStateInMsg
input message name for spacecraft state
-
Message<IMUSensorMsgPayload> sensorOutMsg
output message name for IMU output data
-
double senRotMax
[r/s] Gyro saturation value
-
double senTransMax
[m/s2] Accelerometer saturation value
-
uint64_t OutputBufferCount
— number of output msgs stored
-
bool NominalReady
— Flag indicating that system is in run
-
Eigen::Matrix3d PMatrixAccel
[m/s^2] Matrix square root of the accelerometer process-noise covariance
[m/s^2] Current acceleration errors applied to truth
[rad/s] Current angular-rate errors applied to truth
-
IMUSensorMsgPayload trueValues
[-] total measurement without perturbations
-
IMUSensorMsgPayload sensedValues
[-] total measurement including perturbations
-
BSKLogger bskLogger
— BSK Logging
Private Members
-
Discretize aDisc
(-) instance of discretization utility for linear acceleration
-
Discretize oDisc
(-) instance of discretization utility for angular rate
-
uint64_t PreviousTime
— Timestamp from previous frame
-
int64_t numStates
— Number of States for Gauss Markov Models
-
SCStatesMsgPayload StatePrevious
— Previous state to delta in IMU
-
SCStatesMsgPayload StateCurrent
— Current SSBI-relative state
-
GaussMarkov errorModelAccel
[-] Gauss-markov error states
-
GaussMarkov errorModelGyro
[-] Gauss-markov error states
-
Eigen::Vector3d current_nonConservativeAccelpntB_B
— nonConservativeAccelpntB_B from the current message
-
Eigen::Vector3d previous_TotalAccumDV_CN_N
[m/s] TotalAccumDV_CN_N from the previous spacecraft message
-
Eigen::Vector3d current_TotalAccumDV_CN_N
[m/s] TotalAccumDV_CN_N from the current spacecraft message
-
Eigen::Vector3d previous_v_SC_N
[m/s] sensor velocity relative to the center of mass at the previous call, inertial components