Skip to content

Commit

Permalink
[fix] Orbit frame computation
Browse files Browse the repository at this point in the history
  • Loading branch information
@lucas-bremond
lucas-bremond committed Mar 23, 2020
1 parent 8ca4ffd commit 0b1593d
Show file tree
Hide file tree
Showing 3 changed files with 32 additions and 23 deletions.
21 changes: 15 additions & 6 deletions src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,25 +207,34 @@ State State::inFrame (
throw ostk::core::error::runtime::Undefined("State") ;
}

const Vector3d& x_OLD = position_ ;
const Vector3d& v_OLD_in_OLD = velocity_ ;
const Quaternion& q_B_OLD = attitude_ ;
const Vector3d& w_B_OLD_in_B = angularVelocity_ ;

const Transform transform_NEW_OLD = frameSPtr_->getTransformTo(aFrameSPtr, instant_) ;

const Quaternion q_NEW_OLD = transform_NEW_OLD.getOrientation() ;
const Vector3d w_NEW_OLD_in_NEW = transform_NEW_OLD.getAngularVelocity() ;

// x_NEW = T_NEW_OLD(x_OLD)

const Vector3d position = transform_NEW_OLD.applyToPosition(position_) ;
const Vector3d x_NEW = transform_NEW_OLD.applyToPosition(x_OLD) ;

// v_NEW = T_NEW_OLD(v_OLD)

const Vector3d velocity = transform_NEW_OLD.applyToVelocity(position_, velocity_) ;
const Vector3d v_NEW_in_NEW = transform_NEW_OLD.applyToVelocity(x_OLD, v_OLD_in_OLD) ;

// q_B_NEW = q_B_OLD * q_OLD_NEW

const Quaternion attitude = attitude_ * transform_NEW_OLD.getOrientation().toConjugate() ;
const Quaternion q_B_NEW = q_B_OLD * q_NEW_OLD.toConjugate() ;

// w_B_NEW_in_B = w_B_OLD_in_B + w_OLD_NEW_in_B = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW
// w_B_NEW_in_B = w_B_OLD_in_B + w_OLD_NEW_in_B
// = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW

const Vector3d angularVelocity = angularVelocity_ - attitude * transform_NEW_OLD.getAngularVelocity() ;
const Vector3d w_B_NEW_in_B = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW ;

return { instant_, position, velocity, attitude, angularVelocity, aFrameSPtr } ;
return { instant_, x_NEW, v_NEW_in_NEW, q_B_NEW, w_B_NEW_in_B, aFrameSPtr } ;

}

Expand Down
18 changes: 9 additions & 9 deletions src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -393,15 +393,15 @@ Shared<const Frame> Orbit::getOrbitalFrame (
return FrameManager::Get().accessFrameWithName(frameName) ;
}

const auto generateDynamicProvider = [this] (const auto& anAttitudeGenerator) -> auto
const auto generateDynamicProvider = [this] (const auto& anAttitudeGenerator, const Shared<const Frame>& aReferenceFrame) -> auto
{

const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
(
[this, anAttitudeGenerator] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
[this, anAttitudeGenerator, aReferenceFrame] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
{

const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
const State state = this->getStateAt(anInstant).inFrame(aReferenceFrame) ;

const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
const Vector3d v_GCRF_in_GCRF = state.accessVelocity().accessCoordinates() ;
Expand Down Expand Up @@ -458,7 +458,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, celestialObjectSPtr_->accessFrame(), generateDynamicProvider(calculateAttitude, celestialObjectSPtr_->accessFrame())) ;

break ;

Expand Down Expand Up @@ -487,7 +487,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;

break ;

Expand Down Expand Up @@ -516,7 +516,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;

break ;

Expand Down Expand Up @@ -544,7 +544,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;

break ;

Expand Down Expand Up @@ -572,7 +572,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;

break ;

Expand Down Expand Up @@ -600,7 +600,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (

} ;

orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;

break ;

Expand Down
16 changes: 8 additions & 8 deletions test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -720,11 +720,11 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit, GetOrbitalFrame)
const Quaternion q_ITRF_NED = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getOrientation() ;
const Vector3d w_ITRF_NED_in_ITRF = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getAngularVelocity() ;

EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} ? {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ;
EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} ? {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ;
ASSERT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("@ {}: {} - {} = {} [m]", instant.toString(), x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ;
ASSERT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("@ {}: {} - {} = {} [m/s]", instant.toString(), v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ;

EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} ? {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ;
// EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} ? {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ;
ASSERT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("@ {}: {} / {} = {} [asec]", instant.toString(), q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ;
ASSERT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-8)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant.toString(), w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ;

}

Expand Down Expand Up @@ -790,11 +790,11 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit, GetOrbitalFrame)
// // const Quaternion q_ITRF_NED = lvlhOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getOrientation() ;
// // const Vector3d w_ITRF_NED_in_ITRF = lvlhOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getAngularVelocity() ;

// // EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} ? {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ;
// // EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} ? {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ;
// // EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} = {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ;
// // EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} = {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ;

// // EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} ? {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ;
// // // EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} ? {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ;
// // EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} = {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ;
// // // EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} = {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ;

// // }

Expand Down

0 comments on commit 0b1593d

Please sign in to comment.