# INS_Project

**Repository Path**: null_130_5865/INS_Project

## Basic Information

- **Project Name**: INS_Project
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2024-11-10
- **Last Updated**: 2024-11-10

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 程序功能简介
本软件为惯性导航相关算法解算软件，通过已给的惯性导航数据源文件，可借助该程序进行导入数据并计算得到标定实验、初始对准实验、导航解算实验的解算结果数据，并将结果进行绘图。软件的主要功能包括数据的导入与存储、解算算法的选择、解算历元的范围选择，解算状态的显示和解算结果采样率设置与绘图。

## 数据载入
（1）加表六位置法标定文件类型：txt格式；

数据输入格式为： 

\1.	文件中共七列数据依次为：时标、X向陀螺输出、Y向陀螺输出、Z向陀螺输出、X向加速度计输出、Y向加速度计输出、Z向加速度计输出。

\2.	采样频率为200HZ即采样间隔为0.005s（也可以从时标上判断得到）。

\3.	陀螺和加速度计的输出均为采样间隔内的增量。

\4.	陀螺输出的单位为rad，加速度计输出的单位为 .	

\5.	数据采集时间约半小时，动作依次为：Z轴朝下静止5min，Z轴负向90度（速率10deg/s），Z轴正向旋转90度（速率10deg/s），Z轴朝上静止5min； X轴朝上静止5min，X轴正向旋转90度（速率10deg/s），X轴负向旋转90度（速率10deg/s），X轴朝下静止5min ；Y轴朝下静止5min，Y轴负向旋转90度（速率10deg/s），Y轴正向旋转90度（速率10deg/s），Y轴朝上静止5min。

（2）静态粗对准文件类型：txt格式；

数据输入格式为：

\1. GPS周秒，Gx，Gy，Gz，Ax，Ay，Az，（Gx：IMU的X轴陀螺输出，其它类推）陀螺和加速度计数据都为增量，单位分别是rad和m/s

（3）纯惯导解算文件类型：bin格式；

数据输入格式为：

1. 按照二进制存储，为双精度
2. IMU原始数据为7列；纯惯导参考数据为10列；
3. IMU的b-frame是前右下，数据文件格式为：
4. GPS周秒、Gx、Gy、Gz、Ax、Ay、Az （G代表陀螺，A加速度计）
5. 陀螺和加速度计数据均为增量形式，单位分别为rad和m/s


首先读取文件，提取一整行数据，再通过基本信息循环读入每个历元IMU输出原始的信息；
## 解算结果值的计算
程序先通过循环读取原始数据文件每个历元的数据，再根据功能选择的不同，使用不同的流程与算法进行相应算法计算。

加表六位置法标定首先使用标定数学模型，即传统误差模型，六位置标定法加速度计输入为输入矩阵M，使用六位置标定法传感器输出采集数据为输出矩阵D，以最小二乘法估计算法求解所建立数学模型，将采样点输出加速度数据与所得未考虑横向灵敏度六位置法标定参数代入公式，计算加速度计的理论输入，与实际输入值进行对比，计算两者绝对误差值，做出加速度计输出补偿前误差值与补偿后误差值对比结果。

静态粗对准功能在粗对准阶段,依靠重力矢量及地球自转角速度矢量的测量值,直接估算从机体系到导航系的姿态矩阵 (表示计算导航系);在精对准阶段,可通过处理惯性仪表的输出信号,估计出计算的导航坐标系与理想导航坐标系之间的小失准角。

纯惯导解算功能首先原始数据参数初始化，再通过积分比例积分项与重力/哥氏积分项，解算当前历元速度，并进行高程更新、纬度更新与经度更新，再更新历元位置与曲率半径，更新姿态四元，进行归一化处理，四元数转DCM姿态角，判断是否完成全部历元更新，并保存结果。

## 数据信息的显示与绘图
在导入数据后，程序会自动计算所有数据的总历元数目，并显示在UI上，点击解算后，UI会实时显示正在解算第多少个历元，并将当前历元计算得到的数据实时显示在UI上，进度条会显示当前解算位置，同时将这些信息存储在矩阵变量内，在点击绘图后，会自动以历元为横坐标，解算结果为纵坐标进行绘图。

程序具有显示导入的原始数据的功能，在解算完毕后，可通过“绘制原始数据”功能对导入的原始数据进行绘图并查看。

## 数据的保存
点击解算后，UI会实时显示正在解算第多少个历元，并将当前历元计算得到的改正数和改正后的坐标实时显示在UI上，同时将这些信息存储在矩阵变量内，在点击保存后，选择要保存的路径和要保存的文件名，程序会自动将矩阵中的内容输出到文件中。

对于加表六位置法标定算法，程序会自动以历元数+陀螺仪原始数据补偿前后数据差GX+陀螺仪原始数据补偿前后数据差GY+陀螺仪原始数据补偿前后数据差GZ+加速度仪原始数据补偿前后数据差X+加速度仪原始数据补偿前后数据差Y+加速度仪原始数据补偿前后数据差Z的格式将矩阵中的内容输出到文件中。

对于加静态粗对准算法，程序会自动将对准结果打印在UI上。

对于纯惯导解算算法程序会自动以历元数+时间+X位置误差+Y位置误差+Z位置误差+X速度误差+Y速度误差+Z速度误差+X加速度误差+Y加速度误差+Z加速度误差+X位置+Y位置+Z位置+X速度+Y速度+Z速度+X加速度+Y加速度+Z加速度的格式将矩阵中的内容输出到文件中。
# 主要算法设计与流程图
## 算法设计
此程序分为三部分，加速度计的六位置法可以标定出加速度计的零偏、比例因子、交轴耦合，标定要求加速度计的三个轴线分别垂直指上、指下并静止一段时间。陀螺的静态六位置法可以标定出陀螺的零偏。陀螺比例因子和交轴耦合的标定采用速率法，标定要求绕陀螺的三个轴线分别正转和反转过一定的已知角度，通过已知角度对IMU进行标定。

平台式惯导系统初始对准主要目的是将平台调整到预定的坐标系中，捷联惯导系统初始对准的主要目的是确定姿态矩阵，飞机上捷联惯导系统自主对准可分粗对准和精对准两步进行。在粗对准阶段,依靠重力矢量及地球自转角速度矢量的测量值,直接估算从机体系到导航系的姿态矩阵;在精对准阶段,可通过处理惯性仪表的输出信号,估计出计算的导航坐标系与理想导航坐标系之间的小失准角,粗对准采用解析式粗对准。

使用对IMU的原始数据进行积分并将计算的积分结果转换到对应的坐标系中，并在相应的坐标系中计算速度、位置等信息。 
# 程序主界面与结果绘图
![github](https://github.com/Wang-Jie-Lucid-Sheep/INS_Project/blob/main/Picture/Picture.png)

![github](https://github.com/Wang-Jie-Lucid-Sheep/INS_Project/blob/main/Picture/Result.png)