Skip to content

Commit 0b1593d

Browse files
committed
[fix] Orbit frame computation
1 parent 8ca4ffd commit 0b1593d

File tree

3 files changed

+32
-23
lines changed

3 files changed

+32
-23
lines changed

src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -207,25 +207,34 @@ State State::inFrame (
207207
throw ostk::core::error::runtime::Undefined("State") ;
208208
}
209209

210+
const Vector3d& x_OLD = position_ ;
211+
const Vector3d& v_OLD_in_OLD = velocity_ ;
212+
const Quaternion& q_B_OLD = attitude_ ;
213+
const Vector3d& w_B_OLD_in_B = angularVelocity_ ;
214+
210215
const Transform transform_NEW_OLD = frameSPtr_->getTransformTo(aFrameSPtr, instant_) ;
211216

217+
const Quaternion q_NEW_OLD = transform_NEW_OLD.getOrientation() ;
218+
const Vector3d w_NEW_OLD_in_NEW = transform_NEW_OLD.getAngularVelocity() ;
219+
212220
// x_NEW = T_NEW_OLD(x_OLD)
213221

214-
const Vector3d position = transform_NEW_OLD.applyToPosition(position_) ;
222+
const Vector3d x_NEW = transform_NEW_OLD.applyToPosition(x_OLD) ;
215223

216224
// v_NEW = T_NEW_OLD(v_OLD)
217225

218-
const Vector3d velocity = transform_NEW_OLD.applyToVelocity(position_, velocity_) ;
226+
const Vector3d v_NEW_in_NEW = transform_NEW_OLD.applyToVelocity(x_OLD, v_OLD_in_OLD) ;
219227

220228
// q_B_NEW = q_B_OLD * q_OLD_NEW
221229

222-
const Quaternion attitude = attitude_ * transform_NEW_OLD.getOrientation().toConjugate() ;
230+
const Quaternion q_B_NEW = q_B_OLD * q_NEW_OLD.toConjugate() ;
223231

224-
// 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
232+
// w_B_NEW_in_B = w_B_OLD_in_B + w_OLD_NEW_in_B
233+
// = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW
225234

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

228-
return { instant_, position, velocity, attitude, angularVelocity, aFrameSPtr } ;
237+
return { instant_, x_NEW, v_NEW_in_NEW, q_B_NEW, w_B_NEW_in_B, aFrameSPtr } ;
229238

230239
}
231240

src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -393,15 +393,15 @@ Shared<const Frame> Orbit::getOrbitalFrame (
393393
return FrameManager::Get().accessFrameWithName(frameName) ;
394394
}
395395

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

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

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

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

459459
} ;
460460

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

463463
break ;
464464

@@ -487,7 +487,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (
487487

488488
} ;
489489

490-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
490+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;
491491

492492
break ;
493493

@@ -516,7 +516,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (
516516

517517
} ;
518518

519-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
519+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;
520520

521521
break ;
522522

@@ -544,7 +544,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (
544544

545545
} ;
546546

547-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
547+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;
548548

549549
break ;
550550

@@ -572,7 +572,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (
572572

573573
} ;
574574

575-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
575+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;
576576

577577
break ;
578578

@@ -600,7 +600,7 @@ Shared<const Frame> Orbit::getOrbitalFrame (
600600

601601
} ;
602602

603-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
603+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ;
604604

605605
break ;
606606

test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -720,11 +720,11 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit, GetOrbitalFrame)
720720
const Quaternion q_ITRF_NED = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getOrientation() ;
721721
const Vector3d w_ITRF_NED_in_ITRF = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getAngularVelocity() ;
722722

723-
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()) ;
724-
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()) ;
723+
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()) ;
724+
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()) ;
725725

726-
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()) ;
727-
// 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()) ;
726+
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()) ;
727+
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()) ;
728728

729729
}
730730

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

793-
// // 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()) ;
794-
// // 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()) ;
793+
// // 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()) ;
794+
// // 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()) ;
795795

796-
// // 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()) ;
797-
// // // 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()) ;
796+
// // 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()) ;
797+
// // // 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()) ;
798798

799799
// // }
800800

0 commit comments

Comments
 (0)