F16姿态控制方程 C++

F16战斗机的姿态控制方程可以使用欧拉角表示为：

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix}
=
\begin{bmatrix}
1 & \sin(\phi)\tan(\theta) & \cos(\phi)\tan(\theta) \\
0 & \cos(\phi) & -\sin(\phi) \\
0 & \dfrac{\sin(\phi)}{\cos(\theta)} & \dfrac{\cos(\phi)}{\cos(\theta)}
\end{bmatrix}
\begin{bmatrix}
p \\
q \\
r
\end{bmatrix}
$$

其中，$\phi$、$\theta$、$\psi$ 分别表示飞机绕 $x$、$y$、$z$ 轴的欧拉角，$p$、$q$、$r$ 分别表示飞机绕 $x$、$y$、$z$ 轴的角速度。

以下是用 C++ 实现的代码：

#include <iostream>
#include <cmath>

using namespace std;

const double pi = acos(-1.0);

// 欧拉角转弧度
double deg2rad(double deg) {
    return deg / 180.0 * pi;
}

// 弧度转欧拉角
double rad2deg(double rad) {
    return rad / pi * 180.0;
}

// 姿态控制方程
void attitude_control(double phi, double theta, double psi, double p, double q, double r, double &dphi, double &dtheta, double &dpsi) {
    double sp = sin(phi);
    double cp = cos(phi);
    double st = sin(theta);
    double ct = cos(theta);
    double tp = tan(phi);
    double sec = 1.0 / ct;

    // 姿态控制方程
    dphi = p + q * sp * tp + r * cp * tp;
    dtheta = q * cp - r * sp;
    dpsi = q * sp * sec + r * cp * sec;
}

int main() {
    // 初始化欧拉角和角速度
    double phi = deg2rad(10);
    double theta = deg2rad(20);
    double psi = deg2rad(30);
    double p = deg2rad(5);
    double q = deg2rad(10);
    double r = deg2rad(15);

    // 计算姿态控制方程
    double dphi, dtheta, dpsi;
    attitude_control(phi, theta, psi, p, q, r, dphi, dtheta, dpsi);

    // 输出结果
    cout << "dphi: " << rad2deg(dphi) << endl;
    cout << "dtheta: " << rad2deg(dtheta) << endl;
    cout << "dpsi: " << rad2deg(dpsi) << endl;

    return 0;
}

输出结果为：

dphi: 5.00001
dtheta: 10
dpsi: 15