基于VS2019编程实现的导弹惯性导航仿真导弹 - 码农知识堂

基于VS2019编程实现的导弹惯性导航仿真导弹
题目内容及要求
导弹随飞机升空，在[30ºN，100ºE，1000m]的位置，以1200m/s的对地速度沿北向发射，保持匀速水平状态飞行100s，攻角、侧滑和滚转角均为零。假设导弹安装捷联式惯性导航系统，导航系统三轴指向和飞机本体一致，即捷联式惯性导航系统本体坐标系和导弹本体坐标系保持一致。惯性器件的采样周期为0.01s，，采用GNSS辅助INS，，输出周期为1s，试计算GNSS/INS组合导航输出数值。
A 导弹本体坐标系采用前上右体制，导航坐标系定义北天东坐标系。敏感测量数值包括比力、绝对角速度信息。导航输出飞行位置（经度、纬度、高度）、速度（北向、天向、东向）、姿态（俯仰、偏航和滚转）。
B导弹本体坐标系采用前上右体制，导航坐标系和惯性坐标系均定义为发射时刻的地面发射坐标系（该坐标系发射后处于静止状态），假设发射瞬间地球坐标系和地心惯性坐标系坐标轴重合。敏感测量数值包括比力、绝对角速度信息。导航输出飞行位置、速度（地面发射坐标系内的直角坐标和速度）、姿态（俯仰、偏航和滚转，本体系相对发射坐标系）。
1 要求：
a 基于捷联惯性系统，推导飞行器本体坐标系相对于导航坐标系的比力方程，建立比力和导航坐标系内运动加速度之间的关系模型；
b 基于捷联惯性系统，推导飞行器本体坐标系相对于导航坐标系的相对旋转角速度模型，建立陀螺仪测量的绝对旋转角速度和相对旋转角速度的关系模型；
c 建立基于捷联惯性系统的比力、绝对旋转角速度的线运动导航算法模型、角运动导航算法模型
d 算法可以自行更改飞行器初始飞行参数，比如初始姿态、位置和速度信息。
e 组合算法可采用输出或反馈修正，可自行确定。
f 完成基于位置组合、位置/速度组合的GNSS/INS卡尔曼滤波算法设计。算法至少包括四个模块，第一个模块输出标准运动信息，第二个模块模拟惯性器件测量误差和相应的捷联惯性系统计算，第三个模块输出模拟的GNSS测量信息，第四个模块完成组合/滤波计算。
g 编程完成仿真计算，输出三轴加速度计输出信息（比力信息）、三轴陀螺仪输出信息（绝对角速度信息）、INS导航信息、组合导航信息。

2部分模型参数：
A 地球椭球模型WGS-84椭球，长半轴6378137m，扁率298.257223563，地球自转角速度7.292115×10-5 rad/s；重力加速度取恒定值9.8m/s2；
B 姿态坐标旋转顺序可采用但不限定231体制，只要算法封闭正确即可。
C 假设惯性器件测量误差包括一阶马尔科夫过程和白噪声，加速度计对应的一阶马尔科夫过程均方差为10-5g0（1σ）（此处单位不正确，但暂时按此考虑），时间常数为1h，对应白噪声均方差为10-6g0（1σ）；本文转载自http://www.biyezuopin.vip/onews.asp?id=16769陀螺仪对应的一阶马尔科夫过程均方差为1º/h（1σ）（此处单位不正确，但暂时按此考虑），时间常数为1h，对应白噪声均方差为0.1 º/h（1σ）。GNSS的位置、速度测量精度分别为10m（1σ），0.1m/s（1σ）。INS初始位置、速度和姿态对准精度为10m（1σ），0.1m/s（1σ），0.1º（1σ）。
目录
1. 题目内容及要求 1
  2部分模型参数： 2
2. 模型建立 3
  2.1 坐标系基准确定 3
  2.2 比力方程推导 5
  2.2.3 说明 5
  2.3 姿态旋转角速度关系推导 5
  2.3.3 说明 6
  2.4 导航算法和定姿计算方法 6
  2.5卡尔曼滤波滤波参数和矩阵 7
3. 惯性仿真计算 10
  3.1 仿真流程 10
  3.2 SISN和EKF算法编排框图 10
  3.3 仿真初始条件和参数 11
  3.4 仿真图形 12
  3.4.1 三轴比力模拟值如图6所示 12
  3.4.2 三轴陀螺仪测量模拟值 12
  3.4.2 纯惯性导航计算的位置、速度和姿态（姿态矩阵） 13
  3.4.3 EKF导航计算的位置、速度和姿态（姿态矩阵） 14
  3.5 仿真结果说明 15
4. 附件 15
```
#pragma once
#include 
#include 
#include 
#include "math.h"
#include 						//随机数
#include 					//C++ 矩阵计算工具
#include 
#include 
#include 

using namespace std;
using namespace Eigen;
using Eigen::Matrix3d;					//三维double矩阵
using Eigen::Vector3d;					//三维double向量
using Eigen::Vector4d;
using Eigen::Quaterniond;
using Eigen::MatrixXd;					//自定义维度向量

typedef Matrix Vector6d;
typedef Matrix Matrix6d;
typedef Matrix Matrix9_6d;
typedef Matrix Vector9d;
typedef Matrix Vector15d;
typedef Matrix Matrix15d;
typedef Matrix Matrix15_6d;
typedef Matrix Matrix15_3d;
typedef Matrix Matrix6_15d;
typedef Matrix Matrix3_15d;


#define Omega_i_e	7.292115e-5			//地球自转角速度  unit: rad/s
#define g			9.8					//重力加速度  unit: m/s^2
#define a			6378137				//地球长半轴  unit: m 地球椭球模型WGS-84椭球
#define f			1/298.257223563		//地球曲率
#define lambda_0	100*RAD				//初始经度  unit:angle
#define L_0			30*RAD				//初始纬度  unit:angle
#define h_0			1000				//初始经度  unit:m
#define vN_0		1200				//初始速度  unit:m/s
#define PI			3.1415926535897932  //pi
#define RAD         0.0174532925199432  //角度转换为弧度
#define DEG         57.295779513082323  //弧度转换为角度
#define Flight_Time		100				//飞行时间
#define INS_UPDATE_TIME 0.01			//惯性器件更新时间 unit: s
#define GNSS_UPDATE_TIME 1				//GNSS数据更新时间 unit: s
#define N				 10000			//迭代次数，下面代码并没有用Flight/Update_Time表示此处

#define Acc_Markov_Variance			(10e-5)*g		//加计马尔科夫过程方差 unit:m/s^2
#define Acc_Markov_T_Constant		3600			//加计马尔科夫过程时间常数 unit: s
#define Acc_White_Variance			(10e-6)*g		//加计驱动白噪声
#define Gyro_Markov_Variance		1 * RAD /3600	//陀螺仪马尔科夫过程方差 unit: rad/s
#define Gyro_Markov_T_Constant		3600			//陀螺仪马尔科夫过程时间常数 unit: s
#define Gyro_White_Variance			0.1 * RAD /3600 //陀螺仪驱动白噪声

#define GNSS_Position_Variance		10				//定位系统位置方差 unit:m
#define GNSS_Velocity_Variance		0.1				//定位系统速度方差 unit:m/s
#define INS_Position_Variance		10				//惯性系统初始对准位置方差 unit:m
#define INS_Velocity_Variance		0.1				//惯性系统初始对准速度方差 unit:m/s
#define INS_Attitude_Variance		0.1*RAD			//惯性系统初始对准姿态方差 unit:°


typedef struct							//导航坐标系(NUE)下飞行状态量
{
	double t;									//飞行时间
	Vector3d position, velocity, attitude;		//位置、速度、姿态
	//Matrix3d Dcm;								//方向余弦矩阵
	//Quaterniond q;							//四元数

}FlightState;

typedef struct									//传感器数据
{
	double t;									//飞行时间
	Vector3d f_b;								//体系下比力
	Vector3d omega_b;							//体系下陀螺仪输出

	int	N_amount;								//迭代次数

}SensorData;



typedef struct							//体系下的惯性器件噪声
{
	double t;							//飞行时间
	Vector3d acc, gyro;					//加速度计 陀螺仪
	Vector3d GNSS_position;				//位置噪声
	Vector3d GNSS_velocity;				//速度噪声
	int N_amount;						//次数统计

}SensorNoise;

typedef struct							//GNSS系统信息
{
	double t;
	Vector3d position;					//位置
	Vector3d velocity;					//速度
	int N_amount;
}GNSS_INFO;

typedef struct							//卡尔曼滤波状态-误差
{
	double t;							//飞行时间
	Vector15d X;						//15个状态量
	Vector15d X_erro;					//15个状态误差量——为真实的在EKF中更新的量
	int N_amount;

}EKF_STATE;


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
```
相关阅读:
Unity类银河恶魔城学习记录13-1 p142 Save system源代码
 ElasticSearch7.3学习(二)----内部基于_version乐观锁控制机制
 计算机网络原理复习汇总
 Mapper层注解讲解
 【业务功能篇 131】23种设计模式介绍
 使用新的 NVIDIA Isaac Foundation 模型和工作流程创建、设计和部署机器人应用程序
 JAVA安全之Shrio550-721漏洞原理及复现
 【广州华锐互动】屠宰场生猪检疫VR模拟演练系统
 配置logback日志
 【毕业设计】深度学习 opencv python 实现中国交通标志识别
原文地址：https://blog.csdn.net/sheziqiong/article/details/126946027