henry/EntityLibrary
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Henry Winkel
2023-11-02 11:07:00 +01:00
parent def866a82b
commit 75efaad6ef
6 changed files with 520 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
src/Entities/Movement.cpp
View File

@@ -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);
@@ -23,12 +24,12 @@ namespace Entities
ownPosition_->setGeodesicPos(0,0,0);
}
Movement::Movement(std::shared_ptr<SimCore::Position> pos):ownPosition_(pos)
{
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();
// 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_;
auto ecefVelocityVector = this->ToECEFFromENU();
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_.y() += accelerationVec_.y() * dt / 1000.0;
kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
// kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
// kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
// kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
ownPosition_->setGeocentricPos(X, Y, Z);
c.notify_one();
// c.notify_one();
}
@@ -124,51 +145,118 @@ 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;
RotationMatrix << -sin(lon), -sin(lat)*cos(lon), cos(lat)*cos(lon),
cos(lon), -sin(lat)*sin(lon), cos(lat)*sin(lon),
0 , cos(lat), sin(lat);
return RotationMatrix;
if (!position.isValid()) {
return Eigen::Vector3d(0,0,0);
}
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;
}
void Movement::calKinematicVector()
Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle)
{
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());
Eigen::Vector3d Movement::ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position)
{
// 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);
double lat = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LATITUDE)); // in radians
double lon = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LONGITUDE)); // in radians
// Create NED velocity vector using Eigen vector
Vector3d Vn2(Vn, Ve, -Vd);
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);
// Create transformation matrix from NED to ENU frame using Eigen matrix
Matrix3d T;
T << 0, 1, 0,
1, 0, 0,
0, 0, -1;
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);
}
// Transform NED velocity vector to ENU velocity vector using Eigen matrix multiplication
Vector3d Venu = T * Vn2;
kinematicVec_ = Venu;
}
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_;
}