飞航导弹六自由度模型测试（无控)

飞航导弹非线性动力学模型的第一个可行版本，没有自动驾驶仪，后面会加，后续还会增加导弹种类，测试版本是一个近程地对地导弹。

下面是导弹类的头文件

#pragma once
#include "ISAmodel.h"
#include "Engine.h"
#include "AeroDynamics.h"
#include "PointData.h"
#include <memory>
#include <vector>

using namespace MissilePara;   
using namespace std;

// 导弹基类
class Missile
{
public:
	Missile(const double* x0);               
	virtual ~Missile();                       

public: 
	void OneStep(int n, double h);
	inline double gethight() const { return position(1); }
	inline shared_ptr<vector<shared_ptr<PointData>>> getTrajectory() const { return ptrPointDataList; }

private:
	friend double RK4(int n, const double* x, double* x_new, double h, const Missile& missile);                       
	friend double Gill4(int n, const double* x, double* x_new, double h, const Missile& missile);                       
	friend double RKF78(int n, const double* x, double* x_new, double h, const Missile& missile);

protected:
	virtual void DynFunction(int n, const double* x, double* x_dot) const;
	virtual void SaveIntostdvector();

protected:
	// 速度
	Vector velocity;

	// 角速度
	Vector omega;     

	// 位置
	Vector position;  

	// 欧拉角
	Vector Eulerangle;

	// 质量
	double mass;     

	// 时间
	double time;          

	// 标准大气模型
	shared_ptr<ISAmodel> ptrISAmodel;

	// 发动机
	shared_ptr<Engine> ptrEngine;

	// 空气动力
	shared_ptr<AeroDynamics> ptrAeroDynamics;

	// 弹道数据（轨迹）
	shared_ptr<vector<shared_ptr<PointData>>> ptrPointDataList;
};

// 下面是派生类
// 空空导弹
class air2airMissile : public Missile
{

};

// 空地导弹
class air2groundMissile : Missile
{

};

// 地空导弹
class ground2airMissile : public Missile
{

};


// 地地导弹
class g2gMissile : public Missile
{

};

#include <fstream>
#include <string>
#include "Missile.h"

void Initialize(double* x0)
{
    double V0 = 20;                     // 速度
    double theta0 = 35 * pi / 180.0;    // 弹道倾角
    double psaiv0 = 0;                  // 弹道偏角
    double wx0 = 0;                     // 角速度
    double wy0 = 0;                     // 角速度
    double wz0 = 0;                     // 角速度
    double xm0 = 0;                     // 射程
    double ym0 = 0;                     // 高度
    double zm0 = 0;                     // 侧向位置
    double fai0 = theta0;               // 俯仰角
    double psai0 = psaiv0;              // 偏航角
    double gamma0 = 0;                  // 滚转角
    double m0 = 266;                    // 质量
    double t0 = 0;                      // 时间
    x0[0] = V0;
    x0[1] = theta0;
    x0[2] = psaiv0;
    x0[3] = wx0;
    x0[4] = wy0;
    x0[5] = wz0;
    x0[6] = xm0;
    x0[7] = ym0; 
    x0[8] = zm0;
    x0[9] = fai0;
    x0[10] = psai0;
    x0[11] = gamma0;
    x0[12] = m0;
    x0[13] = t0;
    return;
};

void SaveDataIntoFile(const shared_ptr<vector<shared_ptr<PointData>>> ptrPointDataList)
{
    std::string FileName("./Missiledata/data.txt");
    std::ofstream outFile(FileName, std::ios::out);
    if (outFile.fail()) std::cerr << "打开失败！" << std::endl;

    //临时定义
    double V;
    double theta;
    double psaiv;
    double wx;
    double wy;
    double wz;
    double rx;
    double ry;
    double rz;
    double fai;
    double psai;
    double gamma;
    double m;
    double t;

    //依据设置保存数据
    std::cout << "正在保存数据，请稍后！" << std::endl;
    for (size_t i = 0; i < ptrPointDataList->size(); i++)
    {
        // 赋值，简化后面的代码
        V = ptrPointDataList->at(i)->V;
        theta = ptrPointDataList->at(i)->theta;
        psaiv = ptrPointDataList->at(i)->psaiv;
        wx = ptrPointDataList->at(i)->wx;
        wy = ptrPointDataList->at(i)->wy;
        wz = ptrPointDataList->at(i)->wz;
        rx = ptrPointDataList->at(i)->rx;
        ry = ptrPointDataList->at(i)->ry;
        rz = ptrPointDataList->at(i)->rz;
        fai = ptrPointDataList->at(i)->fai;
        psai = ptrPointDataList->at(i)->psai;
        gamma = ptrPointDataList->at(i)->gamma;
        m = ptrPointDataList->at(i)->m;
        t = ptrPointDataList->at(i)->t;

        outFile << t << ",  " << V << ",  " << theta << ",  " << psaiv << ",  " << wx << ",  " << wy << ",  " << wz << ",  "
            << rx << ",  " << ry << ",  " << rz << ",  " << fai << ",  " << psai << ",  " << gamma << ",  " << m <<  std::endl;
    }
    // 关闭文件     
    outFile.close();
    std::cout << "数据已保存，请查看！" << std::endl;
    return;
}

int main()
{
    /* 导弹初始化 */
    double x0[14];
    Initialize(x0);
    Missile missile(x0);  

    /* 测试 */
    while (missile.gethight() >= 0)  missile.OneStep(14, 0.01);

    /* 保存数据 */
    SaveDataIntoFile(missile.getTrajectory());
    return 0;
};               

下面是用matlab绘图可视化