ADD: added movement tests and updated the calcuation of the updated position in earth centered coordinates
This commit is contained in:
Henry Winkel
183
tests/MovementCalculationTryOuts.txt
Normal file
183
tests/MovementCalculationTryOuts.txt
Normal file
@@ -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;
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
@@ -74,36 +45,41 @@ SCENARIO("Testing the SimCore Sensor")
|
||||
GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2);
|
||||
LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
|
||||
|
||||
|
||||
Entities::Movement mov2(pos1);
|
||||
mov2.setCourse(0);//degree
|
||||
mov2.setSpeed(100);//m/s
|
||||
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 pitch = 0;
|
||||
double course = 360;
|
||||
double speed = 1;
|
||||
|
||||
double Vd = 100 * sin(45* M_PI / 180.0);
|
||||
double h = Vd*10;
|
||||
LOG_S(INFO)<<"höhe: "<< h;
|
||||
double i;
|
||||
|
||||
for ( i = 0; i <= 10; ) {
|
||||
mov2.updatePosition(500);
|
||||
i += 0.5;
|
||||
}
|
||||
Entities::Movement mov2(pos1,course,speed,pitch);
|
||||
|
||||
|
||||
MovementVec movc;
|
||||
movc.dSpd = 10;
|
||||
movc.dClb = 0;
|
||||
movc.dCrs = 0;
|
||||
// LOG_S(INFO)<< mov.getEulerAngles();
|
||||
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);
|
||||
|
||||
|
||||
|
||||
|
||||
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((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);
|
||||
|
||||
// REQUIRE((pos2.getGeocentricPos() - mov.getPosition()->getGeocentricPos()).norm() <= 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);
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user