@@ -138,21 +138,21 @@ void CobConverter::UpdateState(uint64_t CurrentSimNanos)
138
138
centerOfMass[2 ] = 1.0 ;
139
139
140
140
/* ! - Get the heading in the image plane */
141
- Eigen::Vector3d rhat_COB_C = cameraCalibrationMatrixInverse * centerOfBrightness;
142
- Eigen::Vector3d rhat_COM_C = cameraCalibrationMatrixInverse * centerOfMass;
141
+ Eigen::Vector3d rhatCOB_C = cameraCalibrationMatrixInverse * centerOfBrightness;
142
+ Eigen::Vector3d rhatCOM_C = cameraCalibrationMatrixInverse * centerOfMass;
143
143
144
144
/* ! - Retrieve the vector from target to camera and normalize */
145
- rhat_COB_C *= - 1 ;
146
- double rhatCOBNorm = rhat_COB_C .norm ();
147
- rhat_COB_C .normalize ();
148
- rhat_COM_C *= - 1 ;
149
- rhat_COM_C .normalize ();
145
+ rhatCOB_C *= - 1 ;
146
+ double rhatCOBNorm = rhatCOB_C .norm ();
147
+ rhatCOB_C .normalize ();
148
+ rhatCOM_C *= - 1 ;
149
+ rhatCOM_C .normalize ();
150
150
151
151
/* ! - Rotate the vector into frames of interest */
152
- Eigen::Vector3d rhat_COB_N = dcm_NC * rhat_COB_C ;
153
- Eigen::Vector3d rhat_COB_B = dcm_CB.transpose () * rhat_COB_C ;
154
- Eigen::Vector3d rhat_COM_N = dcm_NC * rhat_COM_C ;
155
- Eigen::Vector3d rhat_COM_B = dcm_CB.transpose () * rhat_COM_C ;
152
+ Eigen::Vector3d rhatCOB_N = dcm_NC * rhatCOB_C ;
153
+ Eigen::Vector3d rhatCOB_B = dcm_CB.transpose () * rhatCOB_C ;
154
+ Eigen::Vector3d rhatCOM_N = dcm_NC * rhatCOM_C ;
155
+ Eigen::Vector3d rhatCOM_B = dcm_CB.transpose () * rhatCOM_C ;
156
156
157
157
/* ! - define diagonal terms of the COB covariance */
158
158
Eigen::Matrix3d covarCob_C;
@@ -174,18 +174,18 @@ void CobConverter::UpdateState(uint64_t CurrentSimNanos)
174
174
eigenMatrix3d2CArray (covar_N, uVecCOBMsgBuffer.covar_N );
175
175
eigenMatrix3d2CArray (covar_C, uVecCOBMsgBuffer.covar_C );
176
176
eigenMatrix3d2CArray (covar_B, uVecCOBMsgBuffer.covar_B );
177
- eigenVector3d2CArray (rhat_COB_N , uVecCOBMsgBuffer.rhat_BN_N );
178
- eigenVector3d2CArray (rhat_COB_C , uVecCOBMsgBuffer.rhat_BN_C );
179
- eigenVector3d2CArray (rhat_COB_B , uVecCOBMsgBuffer.rhat_BN_B );
177
+ eigenVector3d2CArray (rhatCOB_N , uVecCOBMsgBuffer.rhat_BN_N );
178
+ eigenVector3d2CArray (rhatCOB_C , uVecCOBMsgBuffer.rhat_BN_C );
179
+ eigenVector3d2CArray (rhatCOB_B , uVecCOBMsgBuffer.rhat_BN_B );
180
180
uVecCOBMsgBuffer.timeTag = (double ) cobMsgBuffer.timeTag * NANO2SEC;
181
181
uVecCOBMsgBuffer.valid = true ;
182
182
183
183
eigenMatrix3d2CArray (covar_N, uVecCOMMsgBuffer.covar_N );
184
184
eigenMatrix3d2CArray (covar_C, uVecCOMMsgBuffer.covar_C );
185
185
eigenMatrix3d2CArray (covar_B, uVecCOMMsgBuffer.covar_B );
186
- eigenVector3d2CArray (rhat_COM_N , uVecCOMMsgBuffer.rhat_BN_N );
187
- eigenVector3d2CArray (rhat_COM_C , uVecCOMMsgBuffer.rhat_BN_C );
188
- eigenVector3d2CArray (rhat_COM_B , uVecCOMMsgBuffer.rhat_BN_B );
186
+ eigenVector3d2CArray (rhatCOM_N , uVecCOMMsgBuffer.rhat_BN_N );
187
+ eigenVector3d2CArray (rhatCOM_C , uVecCOMMsgBuffer.rhat_BN_C );
188
+ eigenVector3d2CArray (rhatCOM_B , uVecCOMMsgBuffer.rhat_BN_B );
189
189
uVecCOMMsgBuffer.timeTag = (double ) cobMsgBuffer.timeTag * NANO2SEC;
190
190
uVecCOMMsgBuffer.valid = validCOM;
191
191
0 commit comments