Skip to content

Commit

Permalink
Corrected time drift bias
Browse files Browse the repository at this point in the history
  • Loading branch information
@DominicDirkx
DominicDirkx committed Jan 29, 2024
1 parent f013ec5 commit f295180
Show file tree
Hide file tree
Showing 4 changed files with 95 additions and 90 deletions.
8 changes: 4 additions & 4 deletions include/tudat/astro/orbit_determination/estimatable_parameters/observationBiasParameter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -463,7 +463,7 @@ class TimeBiasParameterBase: public EstimatableParameter< Eigen::VectorXd >
* used in the simulations for the observation bias. This is due to the fact that the ConstantTimeDriftBias class is
* templated by the observable size, while this class is not.
*/
class ConstantTimeDriftBiasParameter: public TimeBiasParameterBase
class ConstantTimeDriftBiasParameter: public EstimatableParameter< Eigen::VectorXd >
{

public:
Expand All @@ -485,7 +485,7 @@ class ConstantTimeDriftBiasParameter: public TimeBiasParameterBase
const observation_models::LinkEnds linkEnds,
const observation_models::ObservableType observableType,
const double referenceEpoch ):
TimeBiasParameterBase( constant_time_drift_observation_bias, linkEnds.begin( )->second.bodyName_ ),
EstimatableParameter< Eigen::VectorXd >( constant_time_drift_observation_bias, linkEnds.begin( )->second.bodyName_ ),
getCurrentBias_( getCurrentBias ), resetCurrentBias_( resetCurrentBias ), linkEndIndex_( linkEndIndex ),
linkEnds_( linkEnds ), observableType_( observableType ), referenceEpoch_( referenceEpoch ){ }

Expand Down Expand Up @@ -654,7 +654,7 @@ class ConstantTimeDriftBiasParameter: public TimeBiasParameterBase
* simulations for the observation bias. This is due to the fact that the ArcWiseTimeDriftBias class
* is templated by the observable size, while this class is not.
*/
class ArcWiseTimeDriftBiasParameter: public TimeBiasParameterBase
class ArcWiseTimeDriftBiasParameter: public EstimatableParameter< Eigen::VectorXd >
{

public:
Expand All @@ -678,7 +678,7 @@ class ArcWiseTimeDriftBiasParameter: public TimeBiasParameterBase
const observation_models::LinkEnds linkEnds,
const observation_models::ObservableType observableType,
const std::vector< double > referenceEpochs ):
TimeBiasParameterBase( arc_wise_time_drift_observation_bias, linkEnds.begin( )->second.bodyName_ ),
EstimatableParameter< Eigen::VectorXd >( arc_wise_time_drift_observation_bias, linkEnds.begin( )->second.bodyName_ ),
arcStartTimes_( arcStartTimes ), getBiasList_( getBiasList ), resetBiasList_( resetBiasList ),
linkEndIndex_( linkEndIndex ), linkEnds_( linkEnds ), observableType_( observableType ), referenceEpochs_( referenceEpochs )
{
Expand Down
69 changes: 40 additions & 29 deletions include/tudat/astro/orbit_determination/observation_partials/observationBiasPartial.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,11 +144,13 @@ class ObservationPartialWrtConstantTimeDriftBias: public ObservationPartial< Obs
* \param linkEndIndex Link end index from which the 'current time' is determined
* \param referenceEpoch Reference epoch at which the time drift is initialised.
*/
ObservationPartialWrtConstantTimeDriftBias( const std::shared_ptr< TimeBiasPartial< ObservationSize > > timeBiasPartial,
ObservationPartialWrtConstantTimeDriftBias( const observation_models::ObservableType observableType,
const observation_models::LinkEnds& linkEnds,
const int linkEndIndex,
const double referenceEpoch ):
ObservationPartial< ObservationSize >(
std::make_pair( estimatable_parameters::constant_time_drift_observation_bias, timeBiasPartial->getLinkEnds( ).begin( )->second.getDualStringLinkEnd( ) ) ),
timeBiasPartial_( timeBiasPartial ), referenceEpoch_( referenceEpoch )
ObservationPartial< ObservationSize >(
std::make_pair( estimatable_parameters::constant_time_drift_observation_bias, linkEnds.begin( )->second.getDualStringLinkEnd( ) ) ),
observableType_( observableType ), linkEnds_( linkEnds ), linkEndIndex_( linkEndIndex ), referenceEpoch_( referenceEpoch )
{ }

//! Destructor
Expand All @@ -165,22 +167,31 @@ class ObservationPartialWrtConstantTimeDriftBias: public ObservationPartial< Obs
* \return Vector of pairs containing partial values and associated times.
*/
std::vector< std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > > calculatePartial(
const std::vector< Eigen::Vector6d >& states,
const std::vector< double >& times,
const observation_models::LinkEndType linkEndOfFixedTime = observation_models::receiver,
const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr,
const Eigen::Matrix< double, ObservationSize, 1 >& currentObservation =
Eigen::Matrix< double, ObservationSize, 1 >::Constant( TUDAT_NAN ) )
const std::vector< Eigen::Vector6d >& states,
const std::vector< double >& times,
const observation_models::LinkEndType linkEndOfFixedTime = observation_models::receiver,
const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr,
const Eigen::Matrix< double, ObservationSize, 1 >& currentObservation =
Eigen::Matrix< double, ObservationSize, 1 >::Constant( TUDAT_NAN ) )
{
std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > unscaledPartial = timeBiasPartial_->getObservationPartialWrtObservationTime(
states, times, linkEndOfFixedTime, ancillarySettings, currentObservation );
double timeInterval = unscaledPartial.second - referenceEpoch_;
return { std::make_pair( unscaledPartial.first * timeInterval, unscaledPartial.second ) };
Eigen::Matrix< double, ObservationSize, 1 > observationTime;
for ( unsigned int i = 0 ; i < ObservationSize ; i++ )
{
observationTime( i, 0 ) = times.at( linkEndIndex_ ) - referenceEpoch_;
}
return { std::make_pair( observationTime, times.at( linkEndIndex_ ) ) };
}

private:

std::shared_ptr< TimeBiasPartial< ObservationSize > > timeBiasPartial_;
//! Observable type for which the bias is active.
observation_models::ObservableType observableType_;

//! Observation link ends for which the bias is active.
observation_models::LinkEnds linkEnds_;

//! Link end index from which the 'current time' is determined
int linkEndIndex_;

//! Reference epoch at which the time drift is initialised.
double referenceEpoch_;
Expand Down Expand Up @@ -210,14 +221,14 @@ class ObservationPartialWrtArcWiseTimeDriftBias: public ObservationPartial< Obse
* \param referenceEpochs Reference epochs (for each arc) at which the time drifts are initialised
*/
ObservationPartialWrtArcWiseTimeDriftBias( const observation_models::ObservableType observableType,
const observation_models::LinkEnds& linkEnds,
const std::shared_ptr< interpolators::LookUpScheme< double > > arcLookupScheme,
const int linkEndIndex,
const int numberOfArcs,
const std::vector< double > referenceEpochs ):
ObservationPartial< ObservationSize >( std::make_pair( estimatable_parameters::arc_wise_time_drift_observation_bias, linkEnds.begin( )->second.getDualStringLinkEnd( ) ) ),
observableType_( observableType ), linkEnds_( linkEnds ), arcLookupScheme_( arcLookupScheme ),
linkEndIndex_( linkEndIndex ), numberOfArcs_( numberOfArcs ), referenceEpochs_( referenceEpochs )
const observation_models::LinkEnds& linkEnds,
const std::shared_ptr< interpolators::LookUpScheme< double > > arcLookupScheme,
const int linkEndIndex,
const int numberOfArcs,
const std::vector< double > referenceEpochs ):
ObservationPartial< ObservationSize >( std::make_pair( estimatable_parameters::arc_wise_time_drift_observation_bias, linkEnds.begin( )->second.getDualStringLinkEnd( ) ) ),
observableType_( observableType ), linkEnds_( linkEnds ), arcLookupScheme_( arcLookupScheme ),
linkEndIndex_( linkEndIndex ), numberOfArcs_( numberOfArcs ), referenceEpochs_( referenceEpochs )
{
totalPartial_ = Eigen::VectorXd::Zero( ObservationSize * numberOfArcs_ );
}
Expand All @@ -236,12 +247,12 @@ class ObservationPartialWrtArcWiseTimeDriftBias: public ObservationPartial< Obse
* \return Vector of pairs containing partial values and associated times.
*/
std::vector< std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > > calculatePartial(
const std::vector< Eigen::Vector6d >& states,
const std::vector< double >& times,
const observation_models::LinkEndType linkEndOfFixedTime = observation_models::receiver,
const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr,
const Eigen::Matrix< double, ObservationSize, 1 >& currentObservation =
Eigen::Matrix< double, ObservationSize, 1 >::Zero( ) )
const std::vector< Eigen::Vector6d >& states,
const std::vector< double >& times,
const observation_models::LinkEndType linkEndOfFixedTime = observation_models::receiver,
const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr,
const Eigen::Matrix< double, ObservationSize, 1 >& currentObservation =
Eigen::Matrix< double, ObservationSize, 1 >::Zero( ) )
{
int currentIndex = arcLookupScheme_->findNearestLowerNeighbour( times.at( linkEndIndex_ ) );

Expand Down
102 changes: 51 additions & 51 deletions include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,57 +205,57 @@ std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartia
}
break;
}
// case estimatable_parameters::constant_time_drift_observation_bias:
// {
// if( useObservationBiasPartials )
// {
// // Check input consistency
// std::shared_ptr< estimatable_parameters::ConstantTimeDriftBiasParameter > constantTimeDriftBias =
// std::dynamic_pointer_cast< estimatable_parameters::ConstantTimeDriftBiasParameter >(
// parameterToEstimate );
// if( constantTimeDriftBias == nullptr )
// {
// throw std::runtime_error( "Error when making partial w.r.t. time drift bias, type is inconsistent" );
// }
// else
// {
// // Check dependency between parameter and link properties.
// if( linkEnds == constantTimeDriftBias->getLinkEnds( ) && observableType == constantTimeDriftBias->getObservableType( ) )
// {
// observationPartial = std::make_shared< ObservationPartialWrtConstantTimeDriftBias< ObservationSize > >(
// observableType, linkEnds, constantTimeDriftBias->getLinkEndIndex( ), constantTimeDriftBias->getReferenceEpoch( ) );
// }
// }
// }
// break;
// }
// case estimatable_parameters::arc_wise_time_drift_observation_bias:
// {
// if( useObservationBiasPartials )
// {
// // Check input consistency
// std::shared_ptr< estimatable_parameters::ArcWiseTimeDriftBiasParameter > arcwiseTimeDriftBias =
// std::dynamic_pointer_cast< estimatable_parameters::ArcWiseTimeDriftBiasParameter >( parameterToEstimate );
// if ( arcwiseTimeDriftBias == nullptr )
// {
// throw std::runtime_error( "Error when making partial w.r.t. arcwise time drift bias, type is inconsistent" );
// }
// else
// {
// // Check dependency between parameter and link properties.
// if ( linkEnds == arcwiseTimeDriftBias->getLinkEnds( ) && observableType == arcwiseTimeDriftBias->getObservableType( ) )
// {
// observationPartial = std::make_shared< ObservationPartialWrtArcWiseTimeDriftBias< ObservationSize > >(
// observableType, linkEnds,
// arcwiseTimeDriftBias->getLookupScheme( ),
// arcwiseTimeDriftBias->getLinkEndIndex( ),
// arcwiseTimeDriftBias->getArcStartTimes( ).size( ),
// arcwiseTimeDriftBias->getReferenceEpochs( ) );
// }
// }
// }
// break;
// }
case estimatable_parameters::constant_time_drift_observation_bias:
{
if( useObservationBiasPartials )
{
// Check input consistency
std::shared_ptr< estimatable_parameters::ConstantTimeDriftBiasParameter > constantTimeDriftBias =
std::dynamic_pointer_cast< estimatable_parameters::ConstantTimeDriftBiasParameter >(
parameterToEstimate );
if( constantTimeDriftBias == nullptr )
{
throw std::runtime_error( "Error when making partial w.r.t. time drift bias, type is inconsistent" );
}
else
{
// Check dependency between parameter and link properties.
if( linkEnds == constantTimeDriftBias->getLinkEnds( ) && observableType == constantTimeDriftBias->getObservableType( ) )
{
observationPartial = std::make_shared< ObservationPartialWrtConstantTimeDriftBias< ObservationSize > >(
observableType, linkEnds, constantTimeDriftBias->getLinkEndIndex( ), constantTimeDriftBias->getReferenceEpoch( ) );
}
}
}
break;
}
case estimatable_parameters::arc_wise_time_drift_observation_bias:
{
if( useObservationBiasPartials )
{
// Check input consistency
std::shared_ptr< estimatable_parameters::ArcWiseTimeDriftBiasParameter > arcwiseTimeDriftBias =
std::dynamic_pointer_cast< estimatable_parameters::ArcWiseTimeDriftBiasParameter >( parameterToEstimate );
if ( arcwiseTimeDriftBias == nullptr )
{
throw std::runtime_error( "Error when making partial w.r.t. arcwise time drift bias, type is inconsistent" );
}
else
{
// Check dependency between parameter and link properties.
if ( linkEnds == arcwiseTimeDriftBias->getLinkEnds( ) && observableType == arcwiseTimeDriftBias->getObservableType( ) )
{
observationPartial = std::make_shared< ObservationPartialWrtArcWiseTimeDriftBias< ObservationSize > >(
observableType, linkEnds,
arcwiseTimeDriftBias->getLookupScheme( ),
arcwiseTimeDriftBias->getLinkEndIndex( ),
arcwiseTimeDriftBias->getArcStartTimes( ).size( ),
arcwiseTimeDriftBias->getReferenceEpochs( ) );
}
}
}
break;
}
case estimatable_parameters::constant_time_observation_bias:
{
if( useTimeBiasPartials )
Expand Down
6 changes: 0 additions & 6 deletions src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -399,12 +399,6 @@ bool isParameterObservationLinkTimeProperty( const EstimatebleParametersEnum par
case arc_wise_time_observation_bias:
flag = true;
break;
// case constant_time_drift_observation_bias:
// flag = true;
// break;
// case arc_wise_time_drift_observation_bias:
// flag = true;
// break;
default:
flag = false;
break;
Expand Down

0 comments on commit f295180

Please sign in to comment.