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.

Module I/O Messages¶
Msg Variable Name	Msg Type	Description
thrusterConfigBInMsg	THRConfigMsgPayload	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	CmdTorqueBodyMsgPayload	Input message containing the stabilizing integral feedback control torque.
attGuidInMsg	AttGuidMsgPayload	Input message containing the attitude and angular rates of the body frame with respect to the guidance reference frame.
vehConfigInMsg	VehicleConfigMsgPayload	(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	VehicleConfigMsgPayload	Output message containing the estimated location of the CM of the system.
cmEstDataOutMsg	CMEstDataMsgPayload	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:

\[\epsilon = \sqrt{\left| \sigma_\mathcal{B/R} \right|^2 + \left| \omega_\mathcal{B/R} \right|^2}\]

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:

\[\begin{split}[\boldsymbol{K}_n] = [\boldsymbol{P}_n] [\boldsymbol{C}_n]^T \left( [\boldsymbol{C}_n] [\boldsymbol{P}_n] [\boldsymbol{C}_n]^T + [\boldsymbol{R}] \right)^{-1} \\ \boldsymbol{x}_{n+1} = \boldsymbol{x}_n + [\boldsymbol{K}_n] \left( \boldsymbol{y}_n - [\boldsymbol{C}_n] \boldsymbol{x}_n \right) \\ \left[\boldsymbol{P}_{n+1}\right] = \left( [\boldsymbol{I}] - [\boldsymbol{K}_n] [\boldsymbol{C}_n] \right) [\boldsymbol{P}_n]\end{split}\]

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:

Module Parameters¶
Parameter	Default	Description
`attitudeTol`	0	convergence error \(\epsilon\)
`r_CB_B`	[0, 0, 0]	initial guess on the CM location
`P0`	[0, 0, 0]	diagonal elements of the initial state covariance
`R0`	[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

Private Members

Eigen::Matrix3d I¶: identity matrix

Eigen::Matrix3d P¶: state covariance

Eigen::Matrix3d R¶: measurement noise covariance

Eigen::Vector3d r_CB_est¶: CM location estimate.

bool cmKnowledge¶: boolean to assess if vehConfigInMsg is connected

BSKLogger bskLogger¶: — BSK Logging