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)));
}
private static void printdcm(Matrix3 dcm)
{
double R = 0;
double P = 0;
double Y = 0;
dcm.to_euler(ref R, ref P, ref Y);
Console.WriteLine("{0} {1} {2}", R * MathHelper.rad2deg, P * MathHelper.rad2deg, Y * MathHelper.rad2deg);
}