Module: thrustCMEstimation
Executive Summary
This module estimates the location of the center of mass (CM) of the entire spacecraft system based on a series of torque measurements. The module assumes the ability to measure and provide the control torques delivered to the system at steady state, to ensure equilibrium. Torque measurements are provided sequentially, and measurements updates are performed for each new measurement using a sequential, weighted least-squares (LS) algorithm.
Message Connection Descriptions
The following table lists all the module input and output messages. 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 |
|---|---|---|
thrusterConfigBInMsg |
Input message the thrust application point with respect to the origin of the \(\mathcal{B}\) frame, thrust unit direction vector, and thrust magnitude, all in \(\mathcal{B}\)-frame components. |
|
intFeedbackTorqueInMsg |
Input message containing the stabilizing integral feedback control torque. |
|
attGuidInMsg |
Input message containing the attitude and angular rates of the body frame with respect to the guidance reference frame. |
|
vehConfigInMsg |
(Optional) Input message containing the real location of the CM of the system. It is used to verify the correctness of the estimated solution. |
|
vehConfigOutMsg |
Output message containing the estimated location of the CM of the system. |
|
cmEstDataOutMsg |
Output message containing the estimated state, state errors, state covariance, pre- and post-fit residuals. |
Detailed Module Description
This module assumes the presence of a gimbaled thruster that is periodically actuated in order to change the direction of the thrust vector with respect to the body frame. This is done primarily beacuse one thrust measurement alone is not enough to resolve the three components of the CM location. Moreover, fixing the thrust vector along a certain direction causes momentum to build up on the system if such direction is not aligned with the CM. The relative attitude \(\boldsymbol{\sigma}_\mathcal{B/R}\) and relative angular rates \(\boldsymbol{\omega}_\mathcal{B/R}\) of the spacecraft hub with respect to the reference are part of the inputs because measurements are only processed when the attitude and rate errors drop below a certain user-defined threshold:
This is done because the torque measurement should only be processed when the spacecraft hub has converged to the reference within a certain accuracy, which means that the integral feedback torque has converged to the external perturbation. Measurements updates do not happen when attGuidInMsg is not written. Whenever the thrust vector is not aligned with the CM, the thruster torque is acting as an external perturbation on the system, which is what is used in this module to infer the location of the CM. During the time when the thruster remains fixed in the body-frame, such disturbance is constant, which aligns with the assumption of a constant unmodeled torque that can be compensated by the integral feedback term in Module: mrpFeedback. Module: thrusterPlatformReference computes the ideal reference angles for a dual-gimbaled thruster platform in order to articulate the thruster in a way that ensures that momentum is constantly dumped and does not cause wheel saturation.
With \(\boldsymbol{t}\) being the thrust vector and \(\boldsymbol{Z}\) being the negative of the integral feedback term, the following quantities are defined at each step:
\([\boldsymbol{C}_n] = [\boldsymbol{\tilde{t}}]\) : linear model
\(\boldsymbol{y}_n = \boldsymbol{Z} + [\boldsymbol{\tilde{t}}] \boldsymbol{r}_{T/B}\)
\([\boldsymbol{K}_n]\) : optimal gain
\(\boldsymbol{x}_n = \boldsymbol{r}_{C/B}\) : CM location estimate
\([\boldsymbol{P}_n]\) : covariance of the state estimate.
At each step, the state and covariance estimates are updated according to the following:
where \([\boldsymbol{R}]\) is the measurement noise covariance.
Module Assumptions and Limitations
The correct functioning of this module can only be guaranteed as long as multiple, linearly independent torque measurements are being provided. One static measurement is not enough to resolve the CM location.
When additional external disturbances act on the system, the estimated CM location can be affected. In the presence of a biased, unmodeled external torque such as SRP, the estimated location does not coincide with the CM location, but rather the point through which the thruster produces a torque that cancels the SRP effect. Despite the bias in the measurement, this result is still useful combined with Module: thrusterPlatformReference because it guarantees to reach a steady-state equilibrium. See scenarioSepMomentumManagement for an integrated example scenario.
The frequency at which the thruster is articulated needs to be chosen carefully, because holding the thruster fixed for too long can cause reaction wheel saturation, with consequent loss of attitude and inability to estimate the CM location.
More details can be found in R. Calaon, C. Allard, and H. Schaub, “Continuous Center-Of-Mass Estimation For A Gimbaled Ion-Thruster Equipped Spacecraft”.
User Guide
The required module configuration is:
cmEstimation = thrustCMEstimation.ThrustCMEstimation()
cmEstimation.ModelTag = "cmEstimator"
cmEstimation.attitudeTol = 1e-4
cmEstimation.r_CB_B = [0.01, -0.025, 0.04]
cmEstimation.P0 = [0.0025, 0.0025, 0.0025]
cmEstimation.R0 = [1e-9, 1e-9, 1e-9]
unitTestSim.AddModelToTask(unitTaskName, cmEstimation)
The module is configurable with the following parameters:
Parameter |
Default |
Description |
|---|---|---|
|
0 |
convergence error \(\epsilon\) |
|
[0, 0, 0] |
initial guess on the CM location |
|
[0, 0, 0] |
diagonal elements of the initial state covariance |
|
[0, 0, 0] |
diagonal elements of the measurement noise covariance |
-
class ThrustCMEstimation : public SysModel
- #include <thrustCMEstimation.h>
Top level structure for the thrust CM estimation kalman filter. Used to estimate the spacecraft’s center of mass position with respect to the B frame.
Public Functions
-
ThrustCMEstimation()
-
~ThrustCMEstimation() override
-
void SelfInit() override
Initialize C-wrapped output messages
-
void Reset(uint64_t CurrentSimNanos) override
Reset the flyby OD filter to an initial state and initializes the internal estimation matrices.
- Parameters:
CurrentSimNanos – The clock time at which the function was called (nanoseconds)
- Returns:
void
-
void UpdateState(uint64_t CurrentSimNanos) override
Take the relative position measurements and outputs an estimate of the spacecraft states in the inertial frame.
- Parameters:
CurrentSimNanos – The clock time at which the function was called (nanoseconds)
- Returns:
void
Public Members
-
double attitudeTol
declare these user-defined quantities
-
ReadFunctor<THRConfigMsgPayload> thrusterConfigBInMsg
thr config in msg in B-frame coordinates
-
ReadFunctor<CmdTorqueBodyMsgPayload> intFeedbackTorqueInMsg
integral feedback torque input msg
-
ReadFunctor<AttGuidMsgPayload> attGuidInMsg
attitude guidance input msg
-
ReadFunctor<VehicleConfigMsgPayload> vehConfigInMsg
(optional) vehicle configuration input msg
-
Message<CMEstDataMsgPayload> cmEstDataOutMsg
estimated CM output msg
-
Message<VehicleConfigMsgPayload> vehConfigOutMsg
output C++ vehicle configuration msg
-
VehicleConfigMsg_C vehConfigOutMsgC = {}
output C vehicle configuration msg
-
Eigen::Vector3d r_CB_B
initial CM estimate
-
Eigen::Vector3d P0
initial CM state covariance
-
Eigen::Vector3d R0
measurement noise covariance
-
ThrustCMEstimation()