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;
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user