public static Vector3 BodyRatesToEarthRates(Matrix3 dcm, Vector3 gyro)
{
//'''convert the angular velocities from body frame to
//earth frame.
//all inputs and outputs are in radians/s
//returns a earth rate vector
//'''
var p = gyro.x;
var q = gyro.y;
var r = gyro.z;
double phi = 0;
double theta = 0;
double psi = 0;
dcm.to_euler(ref phi, ref theta, ref psi);
var phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi));
var thetaDot = q * cos(phi) - r * sin(phi);
if (fabs(cos(theta)) < 1.0e-20)
{
theta += 1.0e-10;
}
var psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta);
return(new Vector3((phiDot), (thetaDot), (psiDot)));
}
public static Vector3 BodyRatesToEarthRates(Matrix3 dcm, Vector3 gyro)
{
//'''convert the angular velocities from body frame to
//earth frame.
//all inputs and outputs are in radians/s
//returns a earth rate vector
//'''
var p = gyro.x;
var q = gyro.y;
var r = gyro.z;
double phi = 0;
double theta = 0;
double psi = 0;
dcm.to_euler(ref phi, ref theta, ref psi);
var phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi));
var thetaDot = q * cos(phi) - r * sin(phi);
if (fabs(cos(theta)) < 1.0e-20)
theta += 1.0e-10;
var psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta);
return new Vector3((phiDot), (thetaDot), (psiDot));
}
public void set_yaw_degrees(double yaw_degrees)
{
double roll = 0, pitch = 0, yaw = 0;
//'''rotate to the given yaw'''
dcm.to_euler(ref roll, ref pitch, ref yaw);
yaw = (yaw_degrees * deg2rad);
self.dcm.from_euler(roll, pitch, yaw);
}