Skip to content

Commit e7738d1

Browse files
lucas-bremondlucas-bremond
committed
[feature] Add simple orbit frame angular velocity estimation
1 parent ba555be commit e7738d1

File tree

1 file changed

+106
-111
lines changed
  • src/OpenSpaceToolkit/Astrodynamics/Trajectory

1 file changed

+106
-111
lines changed

‎src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp

Lines changed: 106 additions & 111 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <OpenSpaceToolkit/Physics/Units/Mass.hpp>
2424

2525
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Transformations/Rotations/RotationMatrix.hpp>
26+
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Transformations/Rotations/RotationVector.hpp>
2627

2728
#include <OpenSpaceToolkit/Core/Error.hpp>
2829
#include <OpenSpaceToolkit/Core/Utilities.hpp>
@@ -371,8 +372,10 @@ Shared<const Frame> Orbit::getOrbitalFrame (
371372

372373
using ostk::math::obj::Vector3d ;
373374
using ostk::math::geom::d3::trf::rot::Quaternion ;
375+
using ostk::math::geom::d3::trf::rot::RotationVector ;
374376
using ostk::math::geom::d3::trf::rot::RotationMatrix ;
375377

378+
using ostk::physics::time::Duration ;
376379
using ostk::physics::coord::spherical::LLA ;
377380
using FrameManager = ostk::physics::coord::frame::Manager ;
378381
using DynamicProvider = ostk::physics::coord::frame::provider::Dynamic ;
@@ -390,6 +393,42 @@ Shared<const Frame> Orbit::getOrbitalFrame (
390393
return FrameManager::Get().accessFrameWithName(frameName) ;
391394
}
392395

396+
const auto generateDynamicProvider = [this] (const auto& anAttitudeGenerator) -> auto
397+
{
398+
399+
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
400+
(
401+
[this, anAttitudeGenerator] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
402+
{
403+
404+
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
405+
406+
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
407+
const Vector3d v_GCRF_in_GCRF = state.accessVelocity().accessCoordinates() ;
408+
409+
const Vector3d x_VVLH_GCRF_in_GCRF = -x_GCRF ; // [m]
410+
const Vector3d v_VVLH_GCRF_in_GCRF = -v_GCRF_in_GCRF ; // [m/s]
411+
412+
const Quaternion q_VVLH_GCRF = anAttitudeGenerator(state) ;
413+
414+
const Duration delta = Duration::Seconds(1.0) ; // TBM This should be a parameter
415+
416+
const Quaternion q_VVLH_next_GCRF = anAttitudeGenerator(this->getStateAt(anInstant + delta).inFrame(Frame::GCRF())) ;
417+
418+
const Quaternion q_VVLH_next_VVLH = q_VVLH_next_GCRF * q_VVLH_GCRF.toConjugate() ;
419+
const RotationVector rv_VVLH_next_VVLH = RotationVector::Quaternion(q_VVLH_next_VVLH) ;
420+
421+
const Vector3d w_VVLH_GCRF_in_VVLH = rv_VVLH_next_VVLH.getAxis() * (rv_VVLH_next_VVLH.getAngle().inRadians() / delta.inSeconds()) ; // [rad/s]
422+
423+
return { anInstant, x_VVLH_GCRF_in_GCRF, v_VVLH_GCRF_in_GCRF, q_VVLH_GCRF, w_VVLH_GCRF_in_VVLH, Transform::Type::Passive } ;
424+
425+
}
426+
) ;
427+
428+
return dynamicProviderSPtr ;
429+
430+
} ;
431+
393432
Shared<const Frame> orbitalFrameSPtr = nullptr ;
394433

395434
switch (aFrameType)
@@ -398,32 +437,28 @@ Shared<const Frame> Orbit::getOrbitalFrame (
398437
case Orbit::FrameType::NED:
399438
{
400439

401-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
402-
(
403-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
404-
{
440+
const auto calculateAttitude = [this] (const State& aState) -> Quaternion
441+
{
405442

406-
// Get state in central body centered, central body fixed frame
443+
// Get state in central body centered, central body fixed frame
407444

408-
const State state = this->getStateAt(anInstant).inFrame(celestialObjectSPtr_->accessFrame()) ;
445+
const State state = this->getStateAt(aState.getInstant()).inFrame(celestialObjectSPtr_->accessFrame()) ;
409446

410-
// Express the state position in geodetic coordinates
447+
// Express the state position in geodetic coordinates
411448

412-
const LLA lla = LLA::Cartesian(state.accessPosition().accessCoordinates(), celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ;
449+
const LLA lla = LLA::Cartesian(state.accessPosition().accessCoordinates(), celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ;
413450

414-
// Compute the NED frame to central body centered, central body fixed frame transform at position
451+
// Compute the NED frame to central body centered, central body fixed frame transform at position
415452

416-
const Transform transform = ostk::physics::coord::frame::utilities::NorthEastDownTransformAt(lla, celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; // [TBM] This should be optimized: LLA <> ECEF calculation done twice
453+
const Transform transform = ostk::physics::coord::frame::utilities::NorthEastDownTransformAt(lla, celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; // [TBM] This should be optimized: LLA <> ECEF calculation done twice
417454

418-
const Vector3d velocity = - state.accessVelocity().accessCoordinates() ; // [TBM] Check if derivation frame is correct
419-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
455+
const Quaternion q_NED_GCRF = transform.getOrientation() ;
420456

421-
return { anInstant, transform.getTranslation(), velocity, transform.getOrientation(), angularVelocity, Transform::Type::Passive } ;
457+
return q_NED_GCRF ;
422458

423-
}
424-
) ;
459+
} ;
425460

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

428463
break ;
429464

@@ -436,31 +471,23 @@ Shared<const Frame> Orbit::getOrbitalFrame (
436471
// Z axis along orbital momentum
437472
// Y axis toward velocity vector
438473

439-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
440-
(
441-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
442-
{
443-
444-
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
445-
446-
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
447-
const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ;
474+
const auto calculateAttitude = [] (const State& aState) -> Quaternion
475+
{
448476

449-
const Vector3d xAxis = x_GCRF.normalized() ;
450-
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
451-
const Vector3d yAxis = zAxis.cross(xAxis) ;
477+
const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ;
478+
const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ;
452479

453-
const Quaternion q_LVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
480+
const Vector3d xAxis = x_GCRF.normalized() ;
481+
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
482+
const Vector3d yAxis = zAxis.cross(xAxis) ;
454483

455-
const Vector3d velocity = -v_GCRF ;
456-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
484+
const Quaternion q_LVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
457485

458-
return { anInstant, -x_GCRF, velocity, q_LVLH_GCRF, angularVelocity, Transform::Type::Passive } ;
486+
return q_LVLH_GCRF ;
459487

460-
}
461-
) ;
488+
} ;
462489

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

465492
break ;
466493

@@ -473,31 +500,23 @@ Shared<const Frame> Orbit::getOrbitalFrame (
473500
// Y axis along negative orbital momentum
474501
// X axis toward velocity vector
475502

476-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
477-
(
478-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
479-
{
480-
481-
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
482-
483-
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
484-
const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ;
503+
const auto calculateAttitude = [] (const State& aState) -> Quaternion
504+
{
485505

486-
const Vector3d zAxis = -x_GCRF.normalized() ;
487-
const Vector3d yAxis = -x_GCRF.cross(v_GCRF).normalized() ;
488-
const Vector3d xAxis = yAxis.cross(zAxis) ;
506+
const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ;
507+
const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ;
489508

490-
const Quaternion q_VVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
509+
const Vector3d zAxis = -x_GCRF.normalized() ;
510+
const Vector3d yAxis = -x_GCRF.cross(v_GCRF).normalized() ;
511+
const Vector3d xAxis = yAxis.cross(zAxis) ;
491512

492-
const Vector3d velocity = -v_GCRF ;
493-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
513+
const Quaternion q_VVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
494514

495-
return { anInstant, -x_GCRF, velocity, q_VVLH_GCRF, angularVelocity, Transform::Type::Passive } ;
515+
return q_VVLH_GCRF ;
496516

497-
}
498-
) ;
517+
} ;
499518

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

502521
break ;
503522

@@ -509,31 +528,23 @@ Shared<const Frame> Orbit::getOrbitalFrame (
509528
// X axis along position vector
510529
// Z axis along orbital momentum
511530

512-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
513-
(
514-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
515-
{
516-
517-
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
531+
const auto calculateAttitude = [] (const State& aState) -> Quaternion
532+
{
518533

519-
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
520-
const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ;
534+
const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ;
535+
const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ;
521536

522-
const Vector3d xAxis = x_GCRF.normalized() ;
523-
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
524-
const Vector3d yAxis = zAxis.cross(xAxis) ;
537+
const Vector3d xAxis = x_GCRF.normalized() ;
538+
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
539+
const Vector3d yAxis = zAxis.cross(xAxis) ;
525540

526-
const Quaternion q_QSW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
541+
const Quaternion q_QSW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
527542

528-
const Vector3d velocity = -v_GCRF ;
529-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
543+
return q_QSW_GCRF ;
530544

531-
return { anInstant, -x_GCRF, velocity, q_QSW_GCRF, angularVelocity, Transform::Type::Passive } ;
545+
} ;
532546

533-
}
534-
) ;
535-
536-
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ;
547+
orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude)) ;
537548

538549
break ;
539550

@@ -545,31 +556,23 @@ Shared<const Frame> Orbit::getOrbitalFrame (
545556
// X axis along velocity vector
546557
// Z axis along orbital momentum
547558

548-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
549-
(
550-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
551-
{
552-
553-
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
554-
555-
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
556-
const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ;
559+
const auto calculateAttitude = [] (const State& aState) -> Quaternion
560+
{
557561

558-
const Vector3d xAxis = v_GCRF.normalized() ;
559-
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
560-
const Vector3d yAxis = zAxis.cross(xAxis) ;
562+
const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ;
563+
const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ;
561564

562-
const Quaternion q_TNW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
565+
const Vector3d xAxis = v_GCRF.normalized() ;
566+
const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ;
567+
const Vector3d yAxis = zAxis.cross(xAxis) ;
563568

564-
const Vector3d velocity = -v_GCRF ;
565-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
569+
const Quaternion q_TNW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
566570

567-
return { anInstant, -x_GCRF, velocity, q_TNW_GCRF, angularVelocity, Transform::Type::Passive } ;
571+
return q_TNW_GCRF ;
568572

569-
}
570-
) ;
573+
} ;
571574

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

574577
break ;
575578

@@ -581,31 +584,23 @@ Shared<const Frame> Orbit::getOrbitalFrame (
581584
// X axis along velocity vector
582585
// Y axis along orbital momentum
583586

584-
const Shared<const DynamicProvider> dynamicProviderSPtr = std::make_shared<const DynamicProvider>
585-
(
586-
[this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead
587-
{
588-
589-
const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ;
590-
591-
const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ;
592-
const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ;
587+
const auto calculateAttitude = [] (const State& aState) -> Quaternion
588+
{
593589

594-
const Vector3d xAxis = v_GCRF.normalized() ;
595-
const Vector3d yAxis = x_GCRF.cross(v_GCRF).normalized() ;
596-
const Vector3d zAxis = xAxis.cross(yAxis) ;
590+
const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ;
591+
const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ;
597592

598-
const Quaternion q_VNC_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
593+
const Vector3d xAxis = v_GCRF.normalized() ;
594+
const Vector3d yAxis = x_GCRF.cross(v_GCRF).normalized() ;
595+
const Vector3d zAxis = xAxis.cross(yAxis) ;
599596

600-
const Vector3d velocity = -v_GCRF ;
601-
const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity
597+
const Quaternion q_VNC_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ;
602598

603-
return { anInstant, -x_GCRF, velocity, q_VNC_GCRF, angularVelocity, Transform::Type::Passive } ;
599+
return q_VNC_GCRF ;
604600

605-
}
606-
) ;
601+
} ;
607602

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

610605
break ;
611606

0 commit comments

Comments
 (0)