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

CMakeLists.txt

@@ -92,4 +92,8 @@ IF (${TEST_ENTITIY_LIBRARY})
   catch_discover_tests(test_Tracklist)
 
 
+  add_executable(test_MovementClass tests/test_MovementClass.cpp)
+  target_link_libraries(test_MovementClass Catch2::Catch2 EntityLibrary loguru)
+  catch_discover_tests(test_MovementClass)
+  
 ENDIF()

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 kinematicVec_;
+            Eigen::Vector3d ToECEFFromENU();
+
+            Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector();
+    
+            /// East North Up Vector wich represents the local Speed Vector
+            Eigen::Vector3d ENUVec_;
+
             Eigen::Vector3d accelerationVec_;
 
 
             double course_;
             double speed_;
-            double pitch_;
+            double climbAngle_;
 
-            void calKinematicVector();
-            Eigen::Matrix3d getRotationMatrix(double lat, double lon);
 
 
             mutable std::mutex mx;

src/Entities/Movement.cpp

@@ -4,6 +4,7 @@
 #include "SimCore/UtilFunctions.hpp"
 #include <Entities/Movement.hpp>
 #include <Eigen/Geometry>
+#include <tuple>
 
 using namespace Eigen;
 
@@ -14,7 +15,7 @@ namespace Entities
     {
         course_ = 0;
         speed_= 0;
-        pitch_ = 0;
+        climbAngle_ = 0;
 
         ownOrientation_.setRoll(0);
         ownOrientation_.setHeading(0);
@@ -28,7 +29,7 @@ namespace Entities
     {
         course_ = 0;
         speed_= 0;
-        pitch_ = 0;
+        climbAngle_ = 0;
 
         ownOrientation_.setRoll(0);
         ownOrientation_.setHeading(0);
@@ -37,60 +38,80 @@ namespace Entities
 
     }
     
-    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course):ownPosition_(pos),course_(course)
+    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course))
     {
         speed_ = 0;
-        pitch_ = 0;
+        climbAngle_ = 0;
         ownOrientation_.setRoll(0);
         ownOrientation_.setPitch(0);
 
 
     }
-    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed)
+
+   
+    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course)),speed_(speed)
     {
-        pitch_ = 0;
+        climbAngle_ = 0;
         ownOrientation_.setRoll(0);
         ownOrientation_.setPitch(0);
 
 
     }
-    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch)
+
+    
+    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed, double pitch):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course)),speed_(speed),climbAngle_(SimCore::UtilFunctions::DegToRad(pitch))
     {
         ownOrientation_.setRoll(0);
     }
 
    
+    Movement::Movement(std::shared_ptr<SimCore::Position> pos, Eigen::Vector3d ECEF):ownPosition_(pos)
+    {
+
+        course_ = 0;
+        speed_= 0;
+        climbAngle_ = 0;
+
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setHeading(0);
+        ownOrientation_.setPitch(0);
+
+        ENUVec_ =  this->ENUFromECEF(ECEF,*pos.get());
+
+        auto mov = getCourseSpeedClimbAngleFromENU(ENUVec_);
+
+        auto [course, speed, climbAngle] = mov;
+        course_ = course;
+        speed_= speed;
+        climbAngle_ = climbAngle;
+
+        ownOrientation_.setHeading(course_);
+        ownOrientation_.setPitch(climbAngle_);
+
+    }
+
+
+
     void  Movement::updatePosition(double dt)
     {
 
         if (ownPosition_->isValid()) {
         
-            calKinematicVector();
+            auto ecefVelocityVector = this->ToECEFFromENU();
-            
-
-            // Create ECEF coordinate system based on reference point
-            double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE));  // in radians
-            double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE));  // in radians
-            double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT);  // in meters
-
-            Matrix3d RotationMatrix = getRotationMatrix(lat, lon);        
-            
-            Vector3d ecefVelocityVector = RotationMatrix * kinematicVec_;
-
 
             double X,Y,Z;    
             X = ownPosition_->getGeocentricPos()(SimCore::X) +  ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2);
             Y = ownPosition_->getGeocentricPos()(SimCore::Y) +  ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2);
             Z = ownPosition_->getGeocentricPos()(SimCore::Z) +  ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);
 
-            std::lock_guard<std::mutex> lock(mx);
+            // std::lock_guard<std::mutex> lock(mx);
 
-            kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
+            // kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
-            kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
+            // kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
-            kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
+            // kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
 
             ownPosition_->setGeocentricPos(X, Y, Z);
-            c.notify_one();
+            // c.notify_one();
 
         }
 
@@ -124,52 +145,119 @@ namespace Entities
     }
 
 
+    Eigen::Vector3d  Movement::ToECEFFromENU()
+    {
+        if (!ownPosition_->isValid()) {
+            return Eigen::Vector3d(0,0,0);
+        }
+        Eigen::Vector3d ENU = this->ENUFromCourseSpeedClbToLocalSpeedVector();
+
+        double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE));  // in radians
+        double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE));  // in radians        LocalRotationMatrix << 
+        
+        Eigen::Matrix3d LocalRotationMatrix;
+        LocalRotationMatrix << 
+            -sin(lon), - cos(lon)* sin(lat), cos(lon)*cos(lat) ,
+             cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat),
+            0          ,    cos(lat)            , sin(lat);
+
+
+        return LocalRotationMatrix * ENU;
+    }
+
+    Eigen::Vector3d  Movement::ENUFromCourseSpeedClbToLocalSpeedVector()
+    {
+         Eigen::Vector3d ENU;
+            ENU[0] = this->speed_ * cos(this->climbAngle_) * sin(this->course_);
+            ENU[1] = this->speed_ * cos(this->climbAngle_) * cos(this->course_);
+            ENU[2] = this->speed_ * sin(this->climbAngle_);
+
+            this->ENUVec_ = ENU;
+            return ENU;
+    }    
+
     Eigen::Vector3d  Movement::getKinematicVector()
     {
 
-        return kinematicVec_;
+        return ENUVec_;
 
     }
 
-    Eigen::Matrix3d  Movement::getRotationMatrix(double lat, double lon)
+    Eigen::Vector3d Movement::ToECEFFromENU(Eigen::Vector3d ENU, SimCore::Position position)
     {
-        Matrix3d RotationMatrix;
+        if (!position.isValid()) {
-        RotationMatrix <<  -sin(lon),  -sin(lat)*cos(lon),  cos(lat)*cos(lon),
+            return Eigen::Vector3d(0,0,0);
-                            cos(lon),  -sin(lat)*sin(lon),  cos(lat)*sin(lon),
+        }
-                                0 ,              cos(lat),               sin(lat);
+
-        return RotationMatrix;
+        double lat = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LATITUDE));  // in radians
+        double lon = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LONGITUDE));  // in radians        
+
+        Eigen::Matrix3d LocalRotationMatrix; 
+        LocalRotationMatrix << 
+            -sin(lon), - cos(lon)* sin(lat), cos(lon)*cos(lat) ,
+             cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat),
+            0          ,    cos(lat)            , sin(lat);
+ 
+
+        return LocalRotationMatrix * ENU;
     }
 
-
+    Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle)
-    void Movement::calKinematicVector()
     {
+        Eigen::Vector3d ENU;
+            ENU[0] = speed * cos(climbAngle) * sin(course);
+            ENU[1] = speed * cos(climbAngle) * cos(course);
+            ENU[2] = speed * sin(climbAngle);
 
-        Eigen::Vector3d UVW_Coord;
+            return ENU;
-
-        // Convert heading and climbing angle to radians
-        double heading_rad = SimCore::UtilFunctions::DegToRad( this->getCourse());
-        double climbing_angle_rad = SimCore::UtilFunctions::DegToRad(this->getPitch());
-        
-        // Calculate NED velocity vector components
-        double Vn = speed_ * cos(heading_rad) * cos (climbing_angle_rad) ;
-        double Ve = speed_ * sin(heading_rad) * cos (climbing_angle_rad);
-        double Vd = speed_ * sin(climbing_angle_rad);
-
-        // Create NED velocity vector using Eigen vector
-        Vector3d Vn2(Vn, Ve, -Vd);
-
-        // Create transformation matrix from NED to ENU frame using Eigen matrix
-        Matrix3d T;
-        T << 0, 1, 0,
-             1, 0, 0,
-             0, 0, -1;
-
-        // Transform NED velocity vector to ENU velocity vector using Eigen matrix multiplication
-        Vector3d Venu = T * Vn2;
-        kinematicVec_ = Venu;
-
     }
 
+     Eigen::Vector3d Movement::ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position)
+     {
+        
+        double lat = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LATITUDE));  // in radians
+        double lon = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LONGITUDE));  // in radians        
+            
+
+         Eigen::Matrix3d LocalRotationMatrix;
+        LocalRotationMatrix<<
+                - sin(lon)               ,   cos(lon)              , 0         , 
+                - cos(lon) * sin(lat) , - sin(lon) * sin(lat) , cos(lat),
+                  cos(lon) * cos(lat) ,   sin(lon) * cos(lat) , sin(lat);
+
+    
+
+            return  LocalRotationMatrix * ECEF;
+     }
+
+     std::tuple<double, double, double> Movement::getCourseSpeedClimbAngleFromENU(Eigen::Vector3d ENU)
+     {
+        const double dEPS = 0.000001; /// to filter out clalculation inaccuracies
+
+        std::tuple<double, double, double> Result;
+        double Course, Speed, ClimbAngle;
+        if (abs(ENU[0]) < dEPS && abs(ENU[1]) < dEPS && abs(ENU[2]) < dEPS)
+        {
+            Speed = 0;
+
+            Course = 0;
+
+            ClimbAngle = 0;
+
+        } else
+        {
+            Speed = pow( pow(ENU[0],2) + pow(ENU[1],2) + pow(ENU[2],2), 0.5);
+
+            Course = SimCore::UtilFunctions::ShiftRadCrs(atan2( ENU[0], ENU[1]));
+            
+            ClimbAngle = SimCore::UtilFunctions::ShiftRadClb(atan2( ENU[2], pow( pow(ENU[0],2) + pow(ENU[1],2), 0.5) ));
+
+        }
+        return std::make_tuple(Course,Speed,ClimbAngle);
+     }
+
+
+
 
     std::shared_ptr<SimCore::Position>  Movement::getPosition()
     {
@@ -215,7 +303,20 @@ namespace Entities
     {
         std::lock_guard<std::mutex> lock(mx);
         ownOrientation_.setHeading(course);
-        course_ = course;
+        course_ = SimCore::UtilFunctions::DegToRad(course);
+    }
+
+
+    void Movement::setCourseInRad(double courseInRad)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        ownOrientation_.setHeading(SimCore::UtilFunctions::RadToDeg(courseInRad));
+        course_ = courseInRad;
+    }
+
+    double Movement::getSpeed()
+    {
+        return speed_;
     }
 
     void Movement::setSpeed(double speed)
@@ -224,18 +325,32 @@ namespace Entities
         speed_ = speed;
     }
 
+    void Movement::setSpeedInKnots(double speed)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        speed_ = SimCore::UtilFunctions::KnotsToMs(speed);
+    }
+
+
     void Movement::setPitch(double pitch)
     {
         std::lock_guard<std::mutex> lock(mx);
         ownOrientation_.setPitch(pitch);
-        pitch_ = pitch;
+        climbAngle_ = SimCore::UtilFunctions::DegToRad(pitch);
     }
 
-      double Movement::getPitch() 
+    void Movement::setPitchInRad(double pitch)
-      {
+    {
         std::lock_guard<std::mutex> lock(mx);
-        return pitch_;
+        ownOrientation_.setPitch(SimCore::UtilFunctions::RadToDeg(pitch));
-      }
+        climbAngle_ = pitch;
+    }
+
+    double Movement::getPitch() 
+    {
+    std::lock_guard<std::mutex> lock(mx);
+    return climbAngle_;
+    }
 
 
 

tests/MovementCalculationTryOuts.txt

@@ -0,0 +1,183 @@
+struct KinVec{
+  double x,y,z;
+};
+struct MovementVec{
+  double dSpd,dClb,dCrs;
+};
+
+struct LLA_Rad{
+  double dLon,dLat;
+};
+struct UVW{
+  double dU,dV,dW;
+};
+
+struct Movement
+{
+    double dCrs, dSpd, dClb;
+};
+
+Eigen::Vector3d Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord)
+{
+
+    //Transform Mov_Dat to topocentric vector
+    // UVW UVW_Coord;
+    Eigen::Vector3d ENU;
+    ENU[0] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
+    ENU[1] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
+    ENU[2] = Mov_Dat.dSpd * sin(Mov_Dat.dClb);
+
+    //Transform topocentric vector to geocentric vector
+    // Eigen::Vector3d UVW(UVW_Coord.dU,UVW_Coord.dV, UVW_Coord.dW);
+    LOG_S(INFO)<<"UVW: "<< ENU[0]<< " "<< ENU[1]<< " " << ENU[2] ;
+
+    Eigen::Matrix3d LocalRotationMatrix;
+
+    LocalRotationMatrix << 
+    -sin(LLA_rCoord.dLon), - cos(LLA_rCoord.dLon)*sin(LLA_rCoord.dLat) , cos(LLA_rCoord.dLon)*cos(LLA_rCoord.dLat) ,
+     cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLon)* sin(LLA_rCoord.dLat), sin(LLA_rCoord.dLon) *cos(LLA_rCoord.dLat),
+     0                     ,    cos(LLA_rCoord.dLat)                        , sin(LLA_rCoord.dLat);
+
+
+    return LocalRotationMatrix*ENU;
+};
+
+
+
+
+struct Kin_Vect
+{
+    double dVx, dVy, dVz;
+};
+
+struct XYZ
+{
+    double dX,dY,dZ;
+};
+const double dEPS = 0.000001;
+
+double ShiftRadCrs(double Rad)
+{
+  while(Rad < 0)
+  {
+    Rad += 2 * M_PI;
+  }
+
+  while (Rad > 2 * M_PI) 
+  {
+  Rad -= 2 * M_PI;
+  }
+  return Rad;
+
+};
+
+double ShiftRadClb(double Rad)
+{
+   while(Rad < -M_PI)
+  {
+    Rad += 2 * M_PI;
+  }
+
+  while (Rad >  M_PI) 
+  {
+    Rad -= 2 * M_PI;
+  }
+  return Rad;
+};
+
+
+Movement Generate_Movement(Eigen::Vector3d Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
+{
+    Movement Result;
+
+    // Transform geocentric vector to topocentric vector
+
+    
+    Eigen::Vector3d ENU;
+    // UVW UVW_Coord;
+
+    // UVW_Coord.dU = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLon) +
+    //     (Kin_Dat.dVy) * cos(LLA_rCoord.dLon);
+
+    // UVW_Coord.dV = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) -
+    //     (Kin_Dat.dVy) * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
+    //     (Kin_Dat.dVz) * cos(LLA_rCoord.dLat);
+
+    // UVW_Coord.dW = (Kin_Dat.dVx) * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
+    //     (Kin_Dat.dVy) * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
+    //     (Kin_Dat.dVz) * sin(LLA_rCoord.dLat);
+
+     Eigen::Matrix3d LocalRotationMatrix;
+     LocalRotationMatrix<<
+     - sin(LLA_rCoord.dLon)                         ,   cos(LLA_rCoord.dLon)                          , 0,
+     - sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), cos(LLA_rCoord.dLat),
+       cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon),   cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), sin(LLA_rCoord.dLat);
+
+    
+
+    ENU = LocalRotationMatrix * Kin_Dat;
+
+
+    LOG_S(INFO)<<"ENU: "<< ENU[0]<< " "<<ENU[1]<< " " << ENU[2];
+
+    // Transform  topocentric vector to Mov_Dat
+
+    if (abs(ENU[0]) < dEPS && abs(ENU[1]) < dEPS && abs(ENU[2]) < dEPS)
+    {
+        Result.dSpd = 0;
+
+        Result.dClb = 0;
+
+        Result.dCrs = 0;
+
+    } else
+    {
+        Result.dSpd = pow( pow(ENU[0],2) + pow(ENU[1],2) + pow(ENU[2],2), 0.5);
+
+        Result.dCrs = ShiftRadCrs(atan2( ENU[0], ENU[1]));
+         
+        Result.dClb = ShiftRadClb(atan2( ENU[2], pow( pow(ENU[0],2) + pow(ENU[1],2), 0.5) ));
+
+    }
+
+
+    return Result;
+};
+
+
+ SimCore::Position  updatePosition(SimCore::Position ownPosition_, MovementVec mov,double dt)
+    {
+
+       
+        
+            
+
+            // Create ECEF coordinate system based on reference point
+            double lat = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LATITUDE));  // in radians
+            double lon = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LONGITUDE));  // in radians
+            double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT);  // in meters
+
+            LLA_Rad LLA_rCoord;
+            LLA_rCoord.dLat = lat;
+            LLA_rCoord.dLon = lon;
+
+            // Matrix3d RotationMatrix = getRotationMatrix(lat, lon);        
+            
+            // Vector3d ecefVelocityVector = RotationMatrix * kinematicVec_;
+
+            Eigen::Vector3d ecefVelocityVector = Generate_Kin_Vect(mov, LLA_rCoord);
+            Eigen::Vector3d accelerationVec_(0,0,0);
+
+            double X,Y,Z;    
+            X = ownPosition_.getGeocentricPos()(SimCore::X) +  ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2);
+            Y = ownPosition_.getGeocentricPos()(SimCore::Y) +  ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2);
+            Z = ownPosition_.getGeocentricPos()(SimCore::Z) +  ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);
+
+
+            // kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
+            // kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
+            // kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
+          SimCore::Position newPos(X, Y, Z);
+          return newPos;
+       
+    }

tests/test_MovementClass.cpp

@@ -1,61 +1,32 @@
 
-
 #include "Entities/Movement.hpp"
 #include "SimCore/Position.hpp"
 #include "SimCore/SimCore.hpp"
+#include <math.h>
+#include <cmath>
 #include <memory>
 #include <string>
 #include <thread>
 #define CATCH_CONFIG_MAIN
 #include <catch2/catch.hpp>
 #include <loguru.hpp>
+#include <SimCore/UtilFunctions.hpp>
+#include <math.h>
 
 
-struct KinVec{
-  double x,y,z;
-}
-struct MovementVec{
-  double dSpd,dClb,dCrs;
-}
-KinVec Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord)
-{
-    KinVec Result;
-
-    //Transform Mov_Dat to topocentric vector
-    UVW UVW_Coord;
-
-    UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
-    UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
-    UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);
-
-    //Transform topocentric vector to geocentric vector
-
-    Result.x = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
-                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
-                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);
-
-    Result.y = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
-                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
-                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);
-
-    Result.z = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
-                 UVW_Coord.dW * sin(LLA_rCoord.dLat);
-
-
-    return Result;
-};
-
 
 SCENARIO("Testing the SimCore Sensor")
 {
   GIVEN("different Attributes for a Movement ")
   {
-  SimCore::Position pos1;
+  std::shared_ptr<SimCore::Position> pos1 = std::make_shared<SimCore::Position>();
     
     WHEN("constructing Track Object with data")
     {
-      pos1.setGeodesicPos(55, 6, 0);
+
-      LOG_S(INFO)<<"LAT: "<<pos1.getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1.getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1.getGeodesicPos()(SimCore::HEIGHT);
+      
+      pos1->setGeodesicPos(55, 6, 0);
+      LOG_S(INFO)<<"LAT: "<<pos1->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1->getGeodesicPos()(SimCore::HEIGHT);
 
       Entities::Movement mov(pos1);
       mov.setCourse(90);
@@ -64,7 +35,7 @@ SCENARIO("Testing the SimCore Sensor")
 
       mov.updatePosition(10000);// 10 seconds
       
-      LOG_S(INFO)<<"LAT: "<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition().getGeodesicPos()(SimCore::HEIGHT);
+      LOG_S(INFO)<<"LAT: "<<mov.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
 
       GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
       double h2, lat2,lon2;
@@ -75,35 +46,40 @@ SCENARIO("Testing the SimCore Sensor")
       LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
         
 
-      Entities::Movement mov2(pos1);
+      double pitch = 0;
-      mov2.setCourse(0);//degree
+      double course = 360;
-      mov2.setSpeed(100);//m/s
+      double speed = 1;
-      mov2.setPitch(45);//degree
 
-      LOG_S(INFO)<<"LAT: "<<mov2.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT);
 
-      double Vd = 100 * sin(45* M_PI / 180.0);
+      Entities::Movement mov2(pos1,course,speed,pitch);
-      double h = Vd*10;
+     
-      LOG_S(INFO)<<"höhe: "<< h;
+
-      double i;
+      LOG_S(INFO)<<"LAT: "<<mov2.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
+
+   
+
+      std::shared_ptr<SimCore::Position> pos3 = std::make_shared<SimCore::Position>();
+      pos3->setGeodesicPos(55, 6, 0);
+      Entities::Movement mov3(pos3,0,1,90);
+      mov3.updatePosition(1000);
+
+      LOG_S(INFO)<<"LAT: "<<mov3.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov3.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
+
       
-      for ( i = 0; i <= 10; ) {
-        mov2.updatePosition(500);
-        i += 0.5;
-      }
      
-      MovementVec movc;
-      movc.dSpd = 10;
-      movc.dClb = 0;
-      movc.dCrs = 0;
-      // LOG_S(INFO)<< mov.getEulerAngles();
     
       THEN("check if Track attributes are correct")
       {
 
-        // LOG_S(INFO)<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE);
+
-        REQUIRE((pos2.getGeocentricPos() - mov.getPosition().getGeocentricPos()).norm() <= 0.1);
+        // REQUIRE((pos2.getGeocentricPos() - mov.getPosition()->getGeocentricPos()).norm() <= 0.1);
-        REQUIRE((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);
+        REQUIRE((pos2.getGeodesicPos() - pos1->getGeodesicPos()).norm() <= 0.1);
+
+        REQUIRE(round(mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT)) == 1);
+
+        REQUIRE(mov2.getCourse() == SimCore::UtilFunctions::DegToRad(course));
+        REQUIRE(mov2.getPitch() == SimCore::UtilFunctions::DegToRad(pitch));
+        REQUIRE(mov2.getSpeed() == speed);