ADD: added movement tests and updated the calcuation of the updated position in earth centered coordinates

include/Entities/Movement.hpp

@@ -16,10 +16,42 @@ namespace Entities
     {   
         public:
             Movement();
+             /**
+            * @brief Movement constructor
+            * @param std::shared_ptr<SimCore::Position> pos
+            */
             Movement(std::shared_ptr<SimCore::Position> pos);
-            Movement(std::shared_ptr<SimCore::Position>, double course);
-            Movement(std::shared_ptr<SimCore::Position>, double course, double speed);
-            Movement(std::shared_ptr<SimCore::Position>, double course, double speed, double pitch);
+            
+            /**
+            * @brief Movement constructor
+            * @param std::shared_ptr<SimCore::Position> pos
+            * @param double course in Degree
+            */
+            Movement(std::shared_ptr<SimCore::Position> pos, double course);
+
+            /**
+            * @brief Movement constructor
+            * @param std::shared_ptr<SimCore::Position> pos
+            * @param double course in Degree
+            * @param double speed in m/s
+            */
+            Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed);
+
+            /**
+            * @brief Movement constructor
+            * @param std::shared_ptr<SimCore::Position> pos
+            * @param double course in Degree
+            * @param double speed in m/s
+            * @param double sppitch (climbangle) in Degree
+            */
+            Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed, double pitch);
+            
+            /**
+            * @brief Movement constructor
+            * @param std::shared_ptr<SimCore::Position> pos
+            * @param Eigen::Vector3d ECEF vector
+            */
+            Movement(std::shared_ptr<SimCore::Position> pos, Eigen::Vector3d ECEF);
 
             
 
@@ -31,34 +63,67 @@ namespace Entities
 
 
             /**
-            * @brief returns the current Course representing the direction the front of the entity is pointing 
-            *
+            * @brief returns the current Course in rad representing the direction the front of the entity is pointing 
+            * @return double course in rad
             */
             double getCourse();
 
+            /**
+            * @brief set the course
+            * @param double course in degree
+            */
             void setCourse(double course);
 
+            /**
+            * @brief set the course
+            * @param double course in rad
+            */
+            void setCourseInRad(double courseInRad);
+
+
             /**
             * @brief returns the speed in m/s
             *
             */
             double getSpeed();
-
+       
+            /**
+            * @brief set the speed in m/s
+            * @param set speed in m/s
+            */
             void setSpeed(double speed);
 
             /**
-            * returns the climbing angle of the entity of the entity
+            * @brief set the speed in knots
+            * @param set speed in knots
+            */
+            void setSpeedInKnots(double speed);
+
+
+            /**
+            * returns the climbing angle in rad of the entity of the entity
             *
             */
             double getPitch();
-
+            
+            /**
+            * @brief set the course
+            * @param double pitch /climbangle in degree
+            */
             void setPitch(double pitch);
 
+            /**
+            * @brief set the course
+            * @param double pitch /climbangle in rad
+            */
+            void setPitchInRad(double pitch);
+
             /**
             * @brief returns the kinematicVector of Entity
             *
             */
             Eigen::Vector3d getKinematicVector();
+            // void calKinematicVector();
 
             std::shared_ptr<SimCore::Position> getPosition();
 
@@ -66,29 +131,72 @@ namespace Entities
 
             void setPosition(SimCore::Position pos);
 
-
+            /**
+            * @return returns copy of orientation class
+            *
+            */
             SimCore::Orientation getownOrientation();
 
+            /**
+            * @return Eigen::Vector3d  of the current eulerangles
+            *
+            */
             Eigen::Vector3d getEulerAngles();
 
+            // Eigen::Matrix3d getRotationMatrix(double lat, double lon);
+
+            /**
+            * @brief transforms from local ENU (East North Up) Vector to ECEF (Earth Centred Earth Fixed)  
+            * @param Eigen::Vector3d ENU 
+            * @param SimCore::Position Position
+            * @return Eigen::Vector3d ECEF 
+            * from https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
+            */
+           static  Eigen::Vector3d ToECEFFromENU(Eigen::Vector3d ENU, SimCore::Position position);
 
 
+            /**
+            * @brief transforms Course, Speed and ClimbAngle to local ENU (East North Up) Vector   
+            * @param double course 
+            * @param double speed 
+            * @param double climbAngle 
+            * @return Eigen::Vector3d ENU 
+            */
+            static Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle);
 
+            /**
+            * @brief transforms get ENU (East North Up) from ECEF Vector
+            * @param Eigen::Vector3d ECEF  
+            * @param SimCore::Position position 
+            * @return Eigen::Vector3d ENU 
+            */
+            static Eigen::Vector3d ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position);
+
+            /**
+            * @brief converts ENU (East North Up) velocity Vector to course speed and climbangles
+            * @param Eigen::Vector3d ENU
+            * @return std::tuple<double, double, double> course speed, climbangle
+            */
+            static std::tuple<double, double, double> getCourseSpeedClimbAngleFromENU(Eigen::Vector3d ENU);
 
         private:
             std::shared_ptr<SimCore::Position> ownPosition_;
             SimCore::Orientation ownOrientation_;
+            
+            Eigen::Vector3d ToECEFFromENU();
+
+            Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector();
+    
+            /// East North Up Vector wich represents the local Speed Vector
+            Eigen::Vector3d ENUVec_;
 
-            Eigen::Vector3d kinematicVec_;
             Eigen::Vector3d accelerationVec_;
 
 
             double course_;
             double speed_;
-            double pitch_;
+            double climbAngle_;
 
-            void calKinematicVector();
-            Eigen::Matrix3d getRotationMatrix(double lat, double lon);
 
 
             mutable std::mutex mx;