NED坐标系转机体坐标系函数

下面是一个简单的NED坐标系转机体坐标系函数的C语言代码：

void NED2Body(double N, double E, double D, double psi, double theta, double phi, double *x, double *y, double *z) {
    double c_psi = cos(psi);
    double s_psi = sin(psi);
    double c_theta = cos(theta);
    double s_theta = sin(theta);
    double c_phi = cos(phi);
    double s_phi = sin(phi);
    
    // NED -> ENU
    double tmp = N;
    N = E;
    E = tmp;

    // NED -> NED'
    double n = -D;
    double e = E;
    double d = -N;

    // NED' -> Body
    *x = c_theta * c_psi * n + (s_phi * s_theta * c_psi - c_phi * s_psi) * e + (c_phi * s_theta * c_psi + s_phi * s_psi) * d;
    *y = c_theta * s_psi * n + (s_phi * s_theta * s_psi + c_phi * c_psi) * e + (c_phi * s_theta * s_psi - s_phi * c_psi) * d;
    *z = -s_theta * n + s_phi * c_theta * e + c_phi * c_theta * d;
}

该函数的输入参数为NED坐标系下的N，E，D三个方向上的数值，以及机体坐标系下的psi，theta，phi三个欧拉角，输出结果为机体坐标系下的x，y，z三个方向上的数值。

请注意以上回答并不代表作者的观点，仅仅是 ChatGPT AI 扮演者的角色回答问题。