# GNSS-INS-SIM
**GNSS-INS-SIM** is an GNSS/INS simulation project, which generates reference trajectories, IMU sensor output, GPS output, odometer output and magnetometer output. Users choose/set up the sensor model, define the waypoints and provide algorithms, and **gnss-ins-sim** can generate required data for the algorithms, run the algorithms, plot simulation results, save simulations results, and generate a brief summary.
# Contents
------
* [Requirements](#requirements)
* [Demos](#demos)
* [Get started](#get-started)
* [Step 1 Define the IMU model](#step-1-define-the-imu-model)
* [Step 2 Create a motion profile](#step-2-create-a-motion-profile)
* [Step 3 Create your algorithm](#step-3-create-your-algorithm)
* [Step 4 Run the simulation](#step-4-run-the-simulation)
* [Step 5 Show results](#step-5-show-results)
* [Acknowledgement](#Acknowledgement)
# Requirements
- Numpy ( version>1.10 )
- Matplotlib
# Demos
We provide the following demos to show how to use this tool:
| file name | description |
|---|---|
| demo_no_algo.py | A demo of generating data, saving generated data to files and plotting (2D/3D)interested data, no user specified algorithm. |
| demo_allan.py | A demo of Allan analysis of gyroscope and accelerometer data. The generated Allan deviation is shown in figures.|
| demo_free_integration.py | A demo of a simple strapdown system. The simulation runs for 1000 times. The statistics of the INS results of the 1000 simulations are generated.|
| demo_inclinometer_mahony.py | A demo of an dynamic inclinometer algorithm based on Mahony's theory. This demo shows how to generate error plot of interested data.|
| demo_aceinna_vg.py | A demo of DMU380 dynamic tilt algorithm. The algorithm is first compiled as a shared library. This demo shows how to call the shared library. This is the algorithm inside Aceinna's VG/MTLT products.|
| demo_aceinna_ins.py | A demo of DMU380 GNSS/INS fusion algorithm. The algorithm is first compiled as a shared library. This demo shows how to call the shared library. This is the algorithm inside Aceinna's INS products.|
| demo_multiple_algorithms.py | A demo of multiple algorithms in a simulation. This demo shows how to compare resutls of multiple algorithm.|
| demo_gen_data_from_files.py | This demo shows how to do simulation from logged data files.|
# Get started
## Step 1 Define the IMU model
### Step 1.1 Define the IMU error model
IMU error model can be specified in two ways:
#### Choose a built-in model
There are three built-in IMU models: 'low-accuracy', 'mid-accuracy' and 'high accuracy'.
#### Manually define the model
```python
imu_err = {
# gyro bias, deg/hr
'gyro_b': np.array([0.0, 0.0, 0.0]),
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0.25, 0.25, 0.25]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([3.5, 3.5, 3.5]),
# gyro bias instability correlation, sec.
# set this to 'inf' to use a random walk model
# set this to a positive real number to use a first-order Gauss-Markkov model
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# accelerometer bias, m/s^2
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
# accelerometer velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]),
# accelerometer bias instability, m/s^2
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]),
# accelerometer bias instability correlation, sec. Similar to gyro_b_corr
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# magnetometer noise std, uT
'mag_std': np.array([0.2, 0.2, 0.2])
}
```
### Step 1.2 Create an IMU object
```python
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
imu = imu_model.IMU(accuracy='low accuracy', axis=9, gps=True)
```
axis = 6 to generate only gyro and accelerometer data.
axis = 9 to generate magnetometer data besides gyro and accelerometer data.
gps = True to generate GPS data, gps = False not.
## Step 2 Create a motion profile
A motion profile specifies the initial states of the vehicle and motion command that drives the vehicle to move, as shown in the following table.
| Ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 32 | 120 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 200 | 1 |
| ... | ... | ... | ... | ... | ... | ... | ... | ... |
The initial position should be given in the LLA (latitude, longitude and altitude) form. The initial velocity is specified in the vehicle body frame. The initial attitude is represented by Euler angles of ZYX rotation sequence.
Motion commands define how the vehicle moves from its initial state. The simulation will generate true angular velocity, acceleration, magnetic field, position, velocity and attitude according to the commands. Combined with sensor error models, these true values are used to generate gyroscope, accelerometer, magnetometer and GPS output.
There is only one motion command in the above table. Indeed, you can add more motion commands to specify the attitude and velocity of the vehicle. You can also define GPS visibility of the vehicle for each command.
Five command types are supported, as listed below.
| Command type | Comment |
|---|---|
| 1 | Directly define the Euler angles change rate and body frame velocity change rate. The change rates are given by column 2~7. The units are deg/s and m/s/s. Column 8 gives how long the command will last. If you want to fully control execution time of each command by your own, you should always choose the motion type to be 1 |
| 2 | Define the absolute attitude and absolute velocity to reach. The target attitude and velocity are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. If actual executing time is less than max time, the remaining time will not be used and the next command will be executed immediately. If the command cannot be finished within max time, the next command will be executed after max time. |
| 3 | Define attitude change and velocity change. The attitude and velocity changes are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. |
| 4 | Define absolute attitude and velocity change. The absolute attitude and velocity change are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. |
| 5 | Define attitude change and absolute velocity. The attitude change and absolute velocity are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. |
### An example of motion profile
| Ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 32 | 120 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0| 0 | 0 | 0 | 0 | 0 | 200 | 1 |
| 5 | 0 | 45 | 0 | 10 | 0 | 0 | 250 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 10 | 1 |
| 3 | 90 | -45 | 0 | 0 | 0 | 0 | 25 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 50 | 1 |
| 3 | 180 | 0 | 0 | 0 | 0 | 0 | 25 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 50 | 1 |
| 3 | -180 | 0 | 0 | 0 | 0 | 0 | 25 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 50 | 1 |
| 3 | 180 | 0 | 0 | 0 | 0 | 0 | 25 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 50 | 1 |
| 3 | -180 | 0 | 0 | 0 | 0 | 0 | 25 | 1 |
| 1 | 0
没有合适的资源?快使用搜索试试~ 我知道了~
GNSS + 惯性导航、传感器融合模拟器。运动轨迹生成器、传感器模型和导航_Python _代码_相关文件_下载
共102个文件
py:46个
csv:28个
png:6个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
5星 · 超过95%的资源 8 下载量 100 浏览量
2022-07-13
00:44:53
上传
评论 3
收藏 2.07MB ZIP 举报
温馨提示
开源 GNSS + 惯性导航、传感器融合模拟器。运动轨迹发生器、传感器模型和导航 GNSS-INS-SIM是一个 GNSS/INS 仿真项目,可生成参考轨迹、IMU 传感器输出、GPS 输出、里程计输出和磁力计输出。用户选择/设置传感器模型,定义航点并提供算法,gnss-ins-sim可以生成算法所需的数据,运行算法,绘制仿真结果,保存仿真结果,并生成简要总结。 内容 要求 演示 开始使用 步骤 1 定义 IMU 模型 步骤 2 创建运动配置文件 步骤 3 创建您的算法 步骤 4 运行模拟 步骤 5 显示结果 更多详情、使用方法,请下载后阅读README.md文件
资源推荐
资源详情
资源评论
收起资源包目录
GNSS + 惯性导航、传感器融合模拟器。运动轨迹生成器、传感器模型和导航_Python
_代码_相关文件_下载 (102个子文件)
libmymath.a 31KB
MagCalibration.c 10KB
ekfSim_ins.cfg 2KB
WMM2010.COF 4KB
WMM.COF 4KB
motion_def-Komatsu_level_50m_0_0.csv 1.29MB
vib_psd.csv 305KB
motion_def-Komatsu_loaded_-5_0.csv 241KB
mag-0.csv 126KB
accel-0.csv 75KB
accel-0.csv 75KB
gyro-0.csv 75KB
gyro-0.csv 74KB
ref_pos.csv 73KB
ref_pos.csv 73KB
ref_vel.csv 73KB
ref_vel.csv 73KB
ref_att_euler.csv 73KB
ref_att_euler.csv 73KB
time.csv 24KB
time.csv 24KB
motion_def-0to100_sample_by_sample.csv 22KB
motion_def-long_drive.csv 1KB
motion_def-Holland_tunnel.csv 1KB
motion_def.csv 647B
motion_def-ins.csv 647B
motion_def_mag_cal.csv 534B
motion_def-3d.csv 440B
motion_def-90deg_turn.csv 351B
motion_def-100to0.csv 307B
motion_def-Allan.csv 305B
motion_def-static.csv 305B
motion_def-0to100.csv 304B
aceinna_ins-x64.dll 91KB
aceinna_ins-x86.dll 76KB
libsim_utilities-x64.dll 18KB
libsim_utilities-x86.dll 11KB
.gitignore 204B
mymath.h 16KB
MagCalibration.h 814B
template.kml 1KB
LICENSE 1KB
gnss-ins-sim-doc.md 19KB
README.md 19KB
motion_profile_demo.png 1.29MB
mag_si_hi.png 163KB
simulator.png 101KB
NED.png 40KB
ECEF.png 32KB
allan_deviation.png 30KB
ins_data_manager.py 42KB
ins_sim.py 39KB
attitude.py 33KB
pathgen.py 32KB
gui_ans.py 20KB
imu_model.py 16KB
aceinna_ins.py 13KB
geomag.py 11KB
sim_data.py 11KB
sim_data_plot.py 10KB
free_integration.py 8KB
free_integration_odo.py 7KB
ins_algo_manager.py 6KB
ins_loose.py 6KB
inclinometer_mahony.py 5KB
mag_calibrate.py 5KB
geoparams.py 5KB
kml_gen.py 4KB
demo_free_integration.py 3KB
demo_gen_data_from_files.py 3KB
demo_aceinna_ins.py 3KB
demo_free_integration_long_time.py 3KB
time_series_from_psd.py 2KB
ins_algo.py 2KB
demo_ins_loose.py 2KB
demo_inclinometer_mahony.py 2KB
demo_mag_cal.py 2KB
allan_analysis.py 2KB
demo_allan.py 2KB
inclinometer_acc.py 2KB
demo_free_integration_openimu.py 2KB
allan.py 2KB
demo_multiple_algorithms.py 2KB
demo_no_algo.py 1KB
demo_ui_ans.py 1KB
__init__.py 1015B
setup.py 953B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
__init__.py 0B
.pylintrc 484B
libmagcal.so 47KB
libsim_utilities.so 17KB
CMakeLists.txt 427B
共 102 条
- 1
- 2
快撑死的鱼
- 粉丝: 1w+
- 资源: 9156
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
- 1
- 2
- 3
- 4
前往页