6DOF模块

功能	该模块中包括了简易的环境模型和基于四元数的刚体六自由度模型。简易环境模型模拟了重力对无人系统飞行产生的影响；刚体六自由度模型用于描述无人载具运动时的姿态和位置变化
参数
Init_PosE	3维向量，站心坐标系（常为NED）下的载具初始位置，单位米
Init_VelB	3维向量，机体坐标系下的载具xyz速度向量，单位m/s
Init_AngEuler	3维向量，机体坐标系下载具初始姿态，单位弧度（rad）
Init_RateB	3维向量，机体坐标系下的载具绕xyz轴的角速度向量，单位rad/s
Param_uavMass	载具质量，单位为kg
Param_uavJ	3*3的对角矩阵，载具的转动惯量矩阵，主对角线元素为机体系xyz三个主轴的转动惯量，单位kg.m^2
Param_GPSLatLong	2维向量，地图GPS原点的纬度和经度，单位度。
Param_envAltitude	地图GPS原点的海拔高度，竖直向下为正，高于海平面填负值，单位米。注意：由于Simulink使用的地球大气模型不支持海平面以下的输入，ModelParam_envAltitude取值必须为负。
输入
FM_body	6维向量，机体坐标系下载具所受合力与合力矩拼接在一起，由力和力矩模块输出
fColi	6维向量，机体坐标系下碰撞和摩擦产生的力和力矩拼接在一起，由碰撞模块和地面模块输出，若无对应模块则用0代替
Wind	3维向量，站心坐标系（常为NED）下的风速，由风干扰模块输出，若无对应模块则用0代替
输出
6DOF Bus	将简易环境模型和刚体六自由度模型的输出封装在总线中

1.六自由度机身子系统（内层）

刚体六自由度机身子系统

功能	刚体六自由度模型用于描述无人载具运动时的姿态和位置及其变化（包括速度和加速度）
参数
Init_PosE	3维向量，站心坐标系（常为NED）下的载具初始位置，单位米
Init_VelB	3维向量，机体坐标系下的载具xyz速度向量，单位m/s
Init_AngEuler	3维向量，机体坐标系下载具初始姿态，单位弧度（rad）
Init_RateB	3维向量，机体坐标系下的载具绕xyz轴的角速度向量，单位rad/s
输入
Fb	3维向量，机体坐标系下载具所受合力，单位N
Mb	3维向量，机体坐标系下载具所受合力矩，单位N.m
Param_uavMass	载具质量，单位为kg
dm	3*3的矩阵，载具的惯性变化率矩阵，默认为0矩阵，单位kg.m^2/s
Param_uavJ	3*3的对角矩阵，载具的转动惯量矩阵，主对角线元素为机体系xyz三个主轴的转动惯量，单位kg.m^2
输出
Ve	3维向量，
Xe	3维向量
Eular	3维向量
DCM	3*3的方向余弦矩阵
Vb	3维向量
wb	3维向量
dwb/dt	3维向量
AccB	3维向量

2.简易环境子系统（内层）

环境子系统

功能	该简易环境模型进行了载具位置的GPS坐标换算并模拟了重力对无人系统飞行产生的影响
参数
Param_GPSLatLong	2维向量，地图GPS原点的纬度和经度，单位度。
Param_envAltitude	地图GPS原点的海拔高度，竖直向下为正，高于海平面填负值，单位米。注意：由于Simulink使用的地球大气模型不支持海平面以下的输入，ModelParam_envAltitude取值必须为负。
输入
Xe	3维向量，站心坐标系（这里是NED）下的载具实时位置坐标，由六自由度机身子系统模块输出
输出
PosGPS	3维向量，大地坐标系（LLA，经纬高）下的载具实时位置坐标（单位：经纬度均为度，高度为m），结合地图GPS原点对NED下的载具实时位置做了一个逆投变换
gravity	3维向量，特定地点（PosGPS）的地球重力加速度（单位m/s^2），利用世界大地测量系统( WGS 84基准椭球体下建立的测度 )计算。