Skip to content

Commit 8ca4ffd

Browse files
lucas-bremondlucas-bremond
committed
[fix] Wrong angular velocity calculation in Profile::getStateAt
1 parent e7738d1 commit 8ca4ffd

File tree

2 files changed

+114
-22
lines changed

2 files changed

+114
-22
lines changed

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

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -90,14 +90,17 @@ State Profile::getStateAt (
9090
throw ostk::core::error::runtime::Undefined("Profile") ;
9191
}
9292

93-
const Transform transform = transformProvider_.getTransformAt(anInstant) ;
93+
const Transform T_REF_B = transformProvider_.getTransformAt(anInstant) ;
94+
95+
const Quaternion q_REF_B = T_REF_B.getOrientation() ;
96+
const Vector3d w_REF_B_in_REF = T_REF_B.getAngularVelocity() ;
9497

95-
const Vector3d position = transform.applyToPosition({ 0.0, 0.0, 0.0 }) ;
96-
const Vector3d velocity = transform.applyToVelocity({ 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 }) ;
97-
const Quaternion attitude = transform.getOrientation().toConjugate() ;
98-
const Vector3d angularVelocity = transform.getAngularVelocity() ; // [TBC]
98+
const Vector3d x_REF = T_REF_B.applyToPosition({ 0.0, 0.0, 0.0 }) ;
99+
const Vector3d v_REF_in_REF = T_REF_B.applyToVelocity({ 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 }) ;
100+
const Quaternion q_B_REF = q_REF_B.toConjugate() ;
101+
const Vector3d w_B_REF_in_B = q_B_REF * (w_REF_B_in_REF * -1.0) ;
99102

100-
return { anInstant, position, velocity, attitude, angularVelocity, frameSPtr_ } ;
103+
return { anInstant, x_REF, v_REF_in_REF, q_B_REF, w_B_REF_in_B, frameSPtr_ } ;
101104

102105
}
103106

‎test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp

Lines changed: 105 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -337,7 +337,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, InertialPointing)
337337
const Real positionTolerance_m = 1e-3 ;
338338
const Real velocityTolerance_meterPerSec = 1e-6 ;
339339
const Real angularTolerance_asec = 0.0 ;
340-
const Real angularVelocityTolerance_radPerSec = 0.0 ;
340+
const Real angularVelocityTolerance_radPerSec = 1e-10 ;
341341

342342
// Reference data setup
343343

@@ -373,7 +373,104 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, InertialPointing)
373373

374374
}
375375

376-
TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
376+
// TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_LVLH)
377+
// {
378+
379+
// using ostk::core::types::Shared ;
380+
// using ostk::core::types::Real ;
381+
// using ostk::core::types::String ;
382+
// using ostk::core::ctnr::Array ;
383+
// using ostk::core::ctnr::Table ;
384+
// using ostk::core::fs::Path ;
385+
// using ostk::core::fs::File ;
386+
387+
// using ostk::math::obj::Vector3d ;
388+
// using ostk::math::geom::d3::trf::rot::Quaternion ;
389+
390+
// using ostk::physics::units::Length ;
391+
// using ostk::physics::units::Angle ;
392+
// using ostk::physics::units::Derived ;
393+
// using ostk::physics::time::Scale ;
394+
// using ostk::physics::time::Instant ;
395+
// using ostk::physics::time::Duration ;
396+
// using ostk::physics::time::Interval ;
397+
// using ostk::physics::time::DateTime ;
398+
// using ostk::physics::coord::Frame ;
399+
// using ostk::physics::Environment ;
400+
// using ostk::physics::env::obj::celest::Earth ;
401+
402+
// using ostk::astro::trajectory::Orbit ;
403+
// using ostk::astro::trajectory::orbit::models::Kepler ;
404+
// using ostk::astro::trajectory::orbit::models::kepler::COE ;
405+
// using ostk::astro::flight::Profile ;
406+
// using ostk::astro::flight::profile::State ;
407+
408+
// // LVLH #1
409+
410+
// {
411+
412+
// const Environment environment = Environment::Default() ;
413+
414+
// const Length semiMajorAxis = Length::Kilometers(7000.0) ;
415+
// const Real eccentricity = 0.0 ;
416+
// const Angle inclination = Angle::Degrees(0.0) ;
417+
// const Angle raan = Angle::Degrees(0.0) ;
418+
// const Angle aop = Angle::Degrees(0.0) ;
419+
// const Angle trueAnomaly = Angle::Degrees(0.0) ;
420+
421+
// const COE coe = { semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly } ;
422+
423+
// const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ;
424+
// const Derived gravitationalParameter = Earth::GravitationalParameter ;
425+
// const Length equatorialRadius = Earth::EquatorialRadius ;
426+
// const Real J2 = Earth::J2 ;
427+
428+
// const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, Kepler::PerturbationType::None } ;
429+
430+
// const Orbit orbit = { keplerianModel, environment.accessCelestialObjectWithName("Earth") } ;
431+
432+
// const Profile profile = Profile::NadirPointing(orbit, Orbit::FrameType::LVLH) ;
433+
434+
// const Real positionTolerance_m = 1e-3 ;
435+
// const Real velocityTolerance_meterPerSec = 1e-6 ;
436+
// const Real angularTolerance_asec = 0.0 ;
437+
// const Real angularVelocityTolerance_radPerSec = 1e-10 ;
438+
439+
// // Reference data setup
440+
441+
// const File referenceDataFile = File::Path(Path::Parse("/app/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/NadirPointing/VVLH/Satellite_1 t_UTC x_GCRF v_GCRF q_B_GCRF w_B_GCRF_in_GCRF.csv")) ;
442+
443+
// const Table referenceData = Table::Load(referenceDataFile, Table::Format::CSV, true) ;
444+
445+
// for (const auto& referenceRow : referenceData)
446+
// {
447+
448+
// const Instant instant_ref = Instant::DateTime(DateTime::Parse(referenceRow["Time (UTCG)"].accessString()), Scale::UTC) ;
449+
450+
// const Vector3d x_BODY_GCRF_ref = { referenceRow["x (m)"].accessReal(), referenceRow["y (m)"].accessReal(), referenceRow["z (m)"].accessReal() } ;
451+
// const Vector3d v_BODY_GCRF_in_GCRF_ref = { referenceRow["vx (m/sec)"].accessReal(), referenceRow["vy (m/sec)"].accessReal(), referenceRow["vz (m/sec)"].accessReal() } ;
452+
// const Quaternion q_BODY_GCRF_ref = Quaternion::XYZS(referenceRow["q1"].accessReal(), referenceRow["q2"].accessReal(), referenceRow["q3"].accessReal(), referenceRow["q4"].accessReal()).normalize() ;
453+
// const Vector3d w_BODY_GCRF_in_BODY_ref = { referenceRow["wx (rad/sec)"].accessReal(), referenceRow["wy (rad/sec)"].accessReal(), referenceRow["wz (rad/sec)"].accessReal() } ;
454+
455+
// const State state = profile.getStateAt(instant_ref) ;
456+
457+
// const Vector3d x_BODY_GCRF = state.getPosition() ;
458+
// const Vector3d v_BODY_GCRF_in_GCRF = state.getVelocity() ;
459+
// const Quaternion q_BODY_GCRF = state.getAttitude() ;
460+
// const Vector3d w_BODY_GCRF_in_BODY = state.getAngularVelocity() ;
461+
462+
// ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ;
463+
// ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ;
464+
// ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ;
465+
// ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
466+
467+
// }
468+
469+
// }
470+
471+
// }
472+
473+
TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_VVLH)
377474
{
378475

379476
using ostk::core::types::Shared ;
@@ -434,7 +531,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
434531
const Real positionTolerance_m = 1e-3 ;
435532
const Real velocityTolerance_meterPerSec = 1e-6 ;
436533
const Real angularTolerance_asec = 0.0 ;
437-
const Real angularVelocityTolerance_radPerSec = 0.0 ;
534+
const Real angularVelocityTolerance_radPerSec = 1e-10 ;
438535

439536
// Reference data setup
440537

@@ -462,7 +559,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
462559
ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ;
463560
ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ;
464561
ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ;
465-
// ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
562+
ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
466563

467564
}
468565

@@ -497,7 +594,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
497594
const Real positionTolerance_m = 1e-3 ;
498595
const Real velocityTolerance_meterPerSec = 1e-6 ;
499596
const Real angularTolerance_asec = 0.0 ;
500-
const Real angularVelocityTolerance_radPerSec = 0.0 ;
597+
const Real angularVelocityTolerance_radPerSec = 1e-10 ;
501598

502599
// Reference data setup
503600

@@ -525,7 +622,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
525622
ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ;
526623
ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ;
527624
ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ;
528-
// ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
625+
ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
529626

530627
}
531628

@@ -560,7 +657,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
560657
const Real positionTolerance_m = 1e-3 ;
561658
const Real velocityTolerance_meterPerSec = 1e-6 ;
562659
const Real angularTolerance_asec = 0.0 ;
563-
const Real angularVelocityTolerance_radPerSec = 0.0 ;
660+
const Real angularVelocityTolerance_radPerSec = 1e-7 ;
564661

565662
// Reference data setup
566663

@@ -588,20 +685,12 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing)
588685
ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ;
589686
ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ;
590687
ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ;
591-
// ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
688+
ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ;
592689

593690
}
594691

595692
}
596693

597-
// VVLHGD
598-
599-
// {
600-
601-
// [TBI]
602-
603-
// }
604-
605694
}
606695

607696
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

0 commit comments

Comments
 (0)