多传感器融合定位（3-惯性导航原理及误差分析）8-惯性导航误差分析
本次作业摘自 张松鹏大哥的优秀作业
eg.
在现实中 高精度惯导中用等效旋转矢量，低精度惯导中，用角增量；一般中低精度的导航里面，提高的比例都在10%以内，没有太高；实际系统中可以做个实验对比一下，看能提高多少，来决定要不要使用等效旋转矢量的方法解算
我们先来回顾一下姿态更新的框架图
两种姿态解算方法：
1.使用theta t 时间内角增量做积分中值处理
2.使用角增量求解等效旋转矢量的方法
注：表征姿态变化，一般使用四元数较为方便
张松鹏的优秀作业代码github地址
这里对松鹏大神的部分代码进行自己理解记录和一些修改
仿真数据集准备
这里使用的imu 生成仿真软件为 gnss-ins-sim
自定义运动场景 imu_def5.csv
自定义场景
生成仿真数据 sim_imu.py
运行 如下指令即可生成仿真数据，注意生成文件的路径
python sim_imu.py
import numpy as np
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 10.0 # IMU sample frequency
fs_gps = 10.0 # GPS sample frequency
fs_mag = fs # magnetometer sample frequency, not used for now
def create_sim_imu(): #生成 imu 方针imu数据
### Customized IMU model parameters, typical for IMU381
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
'accel_b_stability': np.array([1e-5, 1e-5, 1e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# 'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def6.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('/home/kaho/chapter3/kaho_chapter3/gnss-ins-sim/imu_data/data6') # save data files
if __name__ == '__main__':
create_sim_imu()
resloving.cpp 姿态解算 部分代码片
读取gnss-ins-sim数据集
// 读取gnss-ins-sim生成的仿真数据
bool ReadData(const std::vector<std::string> &path,
std::vector<double> &stamps,
std::vector<Eigen::Vector3d> &accs,
std::vector<Eigen::Vector3d> &gyros,
std::vector<Eigen::Vector3d> &gpses,
std::vector<Eigen::Vector3d> &ref_poses,
std::vector<Eigen::Quaterniond> &ref_att_quats)
{
stamps.clear();
accs.clear();
gyros.clear();
gpses.clear();
ref_poses.clear();
ref_att_quats.clear();
std::vector<std::ifstream> reads;
// int count = 0;
for (int i = 0; i < 6; ++i)
{
reads.push_back(std::ifstream(path[i]));
}
bool init = false;
while (true)
{
if (!init)
{
init = true;
for (int i = 0; i < 6; ++i)
{
std::string strs;
std::getline(reads[i], strs);
}
}
else
{
double time;
{
std::string strs;
if (std::getline(reads[0], strs))
{
// count++;
// count = count % 2;
// if (count != 0)
// {
// continue;
// }
time = std::stod(strs);
}
else
{
break;
}
}
{
std::string strs;
std::string temp;
strs = "";
std::getline(reads[1], strs);
temp = "";
std::vector<double> acc;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
acc.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
acc.push_back(std::stod(temp));
strs = "";
std::getline(reads[2], strs);
temp = "";
std::vector<double> gyro;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gyro.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gyro.push_back(std::stod(temp));
strs = "";
std::getline(reads[3], strs);
temp = "";
std::vector<double> gps;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gps.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gps.push_back(std::stod(temp));
strs = "";
std::getline(reads[4], strs);
temp = "";
std::vector<double> ref_pos;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_pos.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_pos.push_back(std::stod(temp));
strs = "";
std::getline(reads[5], strs);
temp = "";
std::vector<double> ref_att_quat;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_att_quat.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_att_quat.push_back(std::stod(temp));
// std::cout << time << std::endl;
// std::cout << (Eigen::Vector3d(acc[0], acc[1], acc[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gyro[0], gyro[1], gyro[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gps[0], gps[1], gps[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])).transpose() << std::endl;
// std::cout << (Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])).coeffs().transpose() << std::endl;
// throw;
stamps.push_back(time);
accs.push_back(Eigen::Vector3d(acc[0], acc[1], acc[2]));
gyros.push_back(Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R));
// 四元数(4x1)(Eigen::Quaterniond 旋转向量(3x1):Eigen::AngleAxisd 平移向量(3x1):Eigen::Vector3d
Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) * //以(0,0,1)做旋转轴，旋转90度
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
q = q.inverse(); // 初始化四元数
gpses.push_back(Eigen::Vector3d(gps[0], gps[1], gps[2]));
// std::cout << Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// std::cout << q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// throw;
ref_poses.push_back(q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])); // 存储 reference pose
ref_att_quats.push_back(q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])); // 存储 reference quaterniond
}
}
}
}
输入文件路径
// 输入路径，读取数据
path.push_back("../../gnss-ins-sim/imu_data/data6/time.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/accel-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/gyro-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/gps-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/ref_pos.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/ref_att_quat.csv");
ReadData(path, stamps, accs, gyros, gpses, ref_poses, ref_att_quats);
生成 gt.txt ins.txt 供使用kitti evo 评估工具进行数据评估
// groundtruth
std::ofstream gt;
gt.open("../../gnss-ins-sim/imu_data/data6/gt.txt", std::fstream::out);
// 解算后的姿态
std::ofstream pose;
pose.open("../../gnss-ins-sim/imu_data/data6/ins.txt", std::fstream::out);
初始化旋转量和一些地球参数常量
Eigen::Vector3d g = Eigen::Vector3d(0, 0, -9.79484197226504); // 地球重力矢量
/*旋转矩阵*/
Eigen::Quaterniond c_nm_bm(1, 0, 0, 0);
Eigen::Quaterniond c_nm_1_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm(1, 0, 0, 0);
Eigen::Vector3d p_e_nm_e(0, 0, 0);
Eigen::Vector3d p_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_e(0, 0, 0);
Eigen::Vector3d v_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_nm(0, 0, 0);
double w_ie = 360.0 /24.0 / 3600.0 * D2R ; // 地球自转角速度
double rm = 6353346.18315; //极半径 (短半轴)
double rn = 6384140.52699; //赤道半径(长半轴)
double L = 32 * D2R; //L为纬度
double h =0 ;
角增量姿态解算
/*角增量姿态解算*/
if (method == 0 )
{
phi = 0.5*dt *(gyros[i-1] + gyros[i]); //积分中值定理
angle = phi.norm(); //求解等效旋转角度
if (angle == 0)
{
direction = Eigen::Vector3d(0,0,0);
}else
{
direction = phi / angle ; //单位化取方向矢量
direction = direction * std::sin(angle/2.0); // 轴角 转换为 四元数表示
}
c_bm_1_bm = Eigen::Quaterniond(std::cos(angle / 2.0), direction[0], direction[1], direction[2]);
}
eg：轴角与四元数的转换公式
参考博客 ：
四元数和旋转轴及旋转角度之间的转换理解实例
四元数笔记（3）—— 轴角表示法与四元数表示的区别
x=ax∗sin(θ/2)
y = a y ∗ s i n ( θ / 2 ) y = ay * sin(\theta/2)y=ay∗sin(θ/2)
z = a z ∗ s i n ( θ / 2 ) z = az * sin(\theta/2)z=az∗sin(θ/2)
w = c o s ( θ / 2 ) w = cos(\theta/2)w=cos(θ/2)
其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度
等效旋转矢量姿态解算
公式先导
部分代码解析：
求解 T 和 2T时刻的等效旋转矢量
求解 当前时刻相对于上一时刻的旋转
求解旋转矩阵
速度解算，位置解算
eg：位置其实算的是相对于e系的，所以要用到相对于e系的旋转，不过机器人领域，忽略了e跟n系的相对旋转，直接看成一个坐标系了,忽略n相对于e的旋转
//速度解算
/*位置其实算的是相对于e系的，所以要用到相对于e系的旋转，不过机器人领域，忽略了e跟n系的相对旋转，直接看成一个坐标系了,忽略n相对于e的旋转***/
v_e_nm_e = v_e_nm_1_e + dt * (0.5 * (c_nm_1_bm_1 * accs[i - 1] + c_nm_bm * accs[i]) + g);
v_e_nm_nm = c_nm_bm.inverse()*v_e_nm_e;
p_e_nm_e = p_e_nm_1_e + 0.5 * dt * (v_e_nm_1_e + v_e_nm_e);
// 更新 旋转矩阵
c_i_bm_1 = c_i_bm;
c_i_nm_1 = c_i_nm;
/*********************/
c_e_nm_1 = c_e_nm;
c_e_bm_1 = c_e_bm;
/********************/
c_nm_1_bm_1 = c_nm_bm;
v_e_nm_1_e = v_e_nm_e;
p_e_nm_1_e = p_e_nm_e;
解算结果
在两个不同的终端，分别运行
./resolving 0
./resolving 1
使用evo 里程计评估工具进行评估
evo_ape tum gt.txt ins.txt --plot
角增量解算
等效旋转矢量解算
对比发现两种方法误差相同，运行1分钟后整体误差均达到30米左右。由于运动是绕定轴转动，计算结
果相同。
为了对比两者的区别尝试让运动更剧烈且更久一些角增量和旋转矢量的轨迹精度如下所示
ps：来自张松鹏总结
全部代码
代码下载地址https://github.com/rainbowrooster/Multi—sensor-fusion-and-positioning/tree/main/chapter3/Q4
resolving.cpp
#include <vector>
#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <Eigen/Geometry>
#define D2R 0.017453292519943295 //degree2radius
// 读取gnss-ins-sim生成的仿真数据
bool ReadData(const std::vector<std::string> &path,
std::vector<double> &stamps,
std::vector<Eigen::Vector3d> &accs,
std::vector<Eigen::Vector3d> &gyros,
std::vector<Eigen::Vector3d> &gpses,
std::vector<Eigen::Vector3d> &ref_poses,
std::vector<Eigen::Quaterniond> &ref_att_quats)
{
stamps.clear();
accs.clear();
gyros.clear();
gpses.clear();
ref_poses.clear();
ref_att_quats.clear();
std::vector<std::ifstream> reads;
// int count = 0;
for (int i = 0; i < 6; ++i)
{
reads.push_back(std::ifstream(path[i]));
}
bool init = false;
while (true)
{
if (!init)
{
init = true;
for (int i = 0; i < 6; ++i)
{
std::string strs;
std::getline(reads[i], strs);
}
}
else
{
double time;
{
std::string strs;
if (std::getline(reads[0], strs))
{
// count++;
// count = count % 2;
// if (count != 0)
// {
// continue;
// }
time = std::stod(strs);
}
else
{
break;
}
}
{
std::string strs;
std::string temp;
strs = "";
std::getline(reads[1], strs);
temp = "";
std::vector<double> acc;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
acc.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
acc.push_back(std::stod(temp));
strs = "";
std::getline(reads[2], strs);
temp = "";
std::vector<double> gyro;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gyro.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gyro.push_back(std::stod(temp));
strs = "";
std::getline(reads[3], strs);
temp = "";
std::vector<double> gps;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gps.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gps.push_back(std::stod(temp));
strs = "";
std::getline(reads[4], strs);
temp = "";
std::vector<double> ref_pos;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_pos.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_pos.push_back(std::stod(temp));
strs = "";
std::getline(reads[5], strs);
temp = "";
std::vector<double> ref_att_quat;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_att_quat.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_att_quat.push_back(std::stod(temp));
// std::cout << time << std::endl;
// std::cout << (Eigen::Vector3d(acc[0], acc[1], acc[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gyro[0], gyro[1], gyro[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gps[0], gps[1], gps[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])).transpose() << std::endl;
// std::cout << (Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])).coeffs().transpose() << std::endl;
// throw;
stamps.push_back(time);
accs.push_back(Eigen::Vector3d(acc[0], acc[1], acc[2]));
gyros.push_back(Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R));
// 四元数(4x1)(Eigen::Quaterniond 旋转向量(3x1):Eigen::AngleAxisd 平移向量(3x1):Eigen::Vector3d
Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) * //以(0,0,1)做旋转轴，旋转90度
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
q = q.inverse(); // 初始化四元数
gpses.push_back(Eigen::Vector3d(gps[0], gps[1], gps[2]));
// std::cout << Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// std::cout << q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// throw;
ref_poses.push_back(q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])); // 存储 reference pose
ref_att_quats.push_back(q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])); // 存储 reference quaterniond
}
}
}
}
int main(int argc, char const *argv[])
{
/* code */
int method = std::atoi(argv[1]); // method: 0 角动量解算 method:1 旋转矢量解算
std::vector<double>stamps; //时间戳
std::vector<Eigen::Vector3d> accs; //加速度
std::vector<Eigen::Vector3d> gyros; //陀螺仪
std::vector<Eigen::Vector3d> gpses; //gps
std::vector<Eigen::Vector3d>ref_poses; //位置真值
std::vector<Eigen::Quaterniond>ref_att_quats; //姿态真值（四元数法表示）
std::vector<std::string> path;
// 输入路径，读取数据
path.push_back("../../gnss-ins-sim/imu_data/data5/time.csv");
path.push_back("../../gnss-ins-sim/imu_data/data5/accel-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data5/gyro-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data5/gps-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data5/ref_pos.csv");
path.push_back("../../gnss-ins-sim/imu_data/data5/ref_att_quat.csv");
ReadData(path, stamps, accs, gyros, gpses, ref_poses, ref_att_quats);
// groundtruth
std::ofstream gt;
gt.open("../../gnss-ins-sim/imu_data/data5/gt.txt", std::fstream::out);
// 解算后的姿态
std::ofstream pose;
pose.open("../../gnss-ins-sim/imu_data/data5/ins.txt", std::fstream::out);
Eigen::Vector3d g = Eigen::Vector3d(0, 0, -9.79484197226504); // 地球重力矢量
/*旋转矩阵*/
Eigen::Quaterniond c_nm_bm(1, 0, 0, 0);
Eigen::Quaterniond c_nm_1_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm(1, 0, 0, 0);
Eigen::Vector3d p_e_nm_e(0, 0, 0);
Eigen::Vector3d p_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_e(0, 0, 0);
Eigen::Vector3d v_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_nm(0, 0, 0);
double w_ie = 360.0 /24.0 / 3600.0 * D2R ; // 地球自转角速度
double rm = 6353346.18315; //极半径 (短半轴)
double rn = 6384140.52699; //赤道半径(长半轴)
double L = 32 * D2R; //L为纬度
double h =0 ;
for (int i = 1; i < stamps.size(); ++i)
{
double dt = stamps[i] - stamps[i - 1];
Eigen::Vector3d w_ie_n = Eigen::Vector3d(0, w_ie * std::cos(L), w_ie * std::sin(L)); // w_ie_n 地球自转引起的角速度
Eigen::Vector3d w_en_n = Eigen::Vector3d(-v_e_nm_nm[1] / (rm + h), v_e_nm_nm[0] / (rn + h), v_e_nm_nm[0] / (rn + h) * std::tan(L)); //w_en_n 表示载体在地球表面运动时，绕地球旋转形成的角速度
Eigen::Vector3d phi; //角增量
double angle; // 等效旋转角度
Eigen::Vector3d direction; //绕轴转动方向矢量
Eigen::Quaterniond c_bm_1_bm; //当前时刻 相对于 上一时刻的旋转
/*角增量姿态解算*/
if (method == 0 )
{
phi = 0.5*dt *(gyros[i-1] + gyros[i]); //积分中值定理
angle = phi.norm(); //求解等效旋转角度
if (angle == 0)
{
direction = Eigen::Vector3d(0,0,0);
}else
{
direction = phi / angle ; //单位化取方向矢量
direction = direction * std::sin(angle/2.0); // 轴角 转换为 四元数表示
}
c_bm_1_bm = Eigen::Quaterniond(std::cos(angle / 2.0), direction[0], direction[1], direction[2]);
}
/*等效旋转矢量姿态解算*/
else
{
Eigen::Vector3d theta1;
if (i==1)
{
theta1 = 0.5 *dt * (gyros[i-1] + gyros[i]); // 积分中值定理求解角增量
}else
{
theta1 = 0.5*dt * (gyros[i-2] + gyros[i-1]); // 上上一时刻角增量
}
Eigen::Vector3d theta2 = 0.5 * dt *(gyros[i-1] + gyros[i]) ; // 上一时刻角增量
Eigen::Vector3d phi1 = theta1 + 1.0/12*theta1.cross(theta2); // 周期T内的等效旋转矢量
Eigen::Vector3d phi2 = theta1 + theta2 + 2.0 / 3.0* theta1.cross(theta2); // 周期2T内的等效旋转矢量
double angle1 = phi1.norm() ; //等效旋转角
double angle2 = phi2.norm() ;
Eigen::Vector3d direction1;
Eigen::Vector3d direction2;
if (angle1 == 0)
{
direction1 = Eigen::Vector3d(0,0,0);
}else
{
direction1 = phi1/ angle1 ; // 轴的矢量
direction1 = direction1 * std::sin(angle1 / 2.0); // 四元数中的 xyz
}
if (angle2 == 0)
{
direction2 = Eigen::Vector3d(0,0,0);
}else
{
direction2 = phi2 / angle2;
direction2 = direction2 * std::sin(angle2 / 2.0); // 四元数中的xyz
}
Eigen::Quaterniond temp1 = Eigen::Quaterniond(std::cos(angle1 / 2.0), direction1[0], direction1[1], direction1[2]); // 等效旋转矢量 转化为 四元数
Eigen::Quaterniond temp2 = Eigen::Quaterniond(std::cos(angle2 / 2.0), direction2[0] ,direction2[1], direction2[2] );
c_bm_1_bm = temp1.inverse()*temp2; // 得到当前时刻相对于上一时刻的旋转
}
c_i_bm = c_i_bm_1* c_bm_1_bm;
phi = (w_ie_n + w_en_n)*dt;
angle = phi.norm();
if (angle == 0)
{
direction = Eigen::Vector3d(0, 0, 0);
}else
{
direction = phi / angle;
direction = direction*std::sin(angle / 2.0);
}
Eigen::Quaterniond c_nm_1_nm(std::cos(angle / 2.0),direction[0],direction[1],direction[2]);
c_i_nm = c_i_nm_1 * c_nm_1_nm;
c_nm_bm = c_i_nm.inverse()*c_i_bm;
/************鹏哥使用的方法，考虑了地球自转的影响，求解n系相对于e系的矩阵***********/
// phi = w_en_n * dt;
// angle = phi.norm();
// if (angle == 0)
// {
// direction = Eigen::Vector3d(0, 0, 0);
// }
// else
// {
// direction = phi / angle;
// direction = direction * std::sin(angle / 2.0);
// }
// Eigen::Quaterniond temp(std::cos(angle / 2.0), direction[0], direction[1], direction[2]);
// c_e_nm = c_e_nm_1 * temp;
// c_e_bm = c_e_nm * c_nm_bm;
// v_e_nm_e = v_e_nm_1_e + dt * (0.5 * (c_e_bm_1 * accs[i - 1] + c_e_bm * accs[i]) + g);
// v_e_nm_nm = c_e_nm.inverse() * v_e_nm_e;
// p_e_nm_e = p_e_nm_1_e + 0.5 * dt * (v_e_nm_1_e + v_e_nm_e);
/***************************************************************************************/
//速度解算
/*位置其实算的是相对于e系的，所以要用到相对于e系的旋转，不过机器人领域，忽略了e跟n系的相对旋转，直接看成一个坐标系了,忽略n相对于e的旋转***/
v_e_nm_e = v_e_nm_1_e + dt * (0.5 * (c_nm_1_bm_1 * accs[i - 1] + c_nm_bm * accs[i]) + g);
v_e_nm_nm = c_nm_bm.inverse()*v_e_nm_e;
p_e_nm_e = p_e_nm_1_e + 0.5 * dt * (v_e_nm_1_e + v_e_nm_e);
// 更新 旋转矩阵
c_i_bm_1 = c_i_bm;
c_i_nm_1 = c_i_nm;
/*********************/
c_e_nm_1 = c_e_nm;
c_e_bm_1 = c_e_bm;
/********************/
c_nm_1_bm_1 = c_nm_bm;
v_e_nm_1_e = v_e_nm_e;
p_e_nm_1_e = p_e_nm_e;
gt << std::fixed << stamps[i] << " " << ref_poses[i][0] - ref_poses[0][0] << " "
<< ref_poses[i](1) - ref_poses[0][1] << " " << ref_poses[i](2) - ref_poses[0][2] << " "
<< ref_att_quats[i].coeffs()(0) << " " << ref_att_quats[i].coeffs()(1) << " "
<< ref_att_quats[i].coeffs()(2) << " " << ref_att_quats[i].coeffs()(3) << std::endl;
pose << std::fixed << stamps[i] << " " << p_e_nm_1_e(0) << " "
<< p_e_nm_1_e(1) << " " << p_e_nm_1_e(2) << " "
<< c_nm_bm.coeffs()(0) << " " << c_nm_bm.coeffs()(1) << " "
<< c_nm_bm.coeffs()(2) << " " << c_nm_bm.coeffs()(3) << std::endl;
}
return 0;
}
CMakeLists.txt
project(ins)
cmake_minimum_required(VERSION 3.10)
include_directories(/usr/include/eigen3)
add_executable(resolving resolving.cpp)
#target_link_libraries( ins ${IMU_TK_LIBS})
sim_imu.py
import numpy as np
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 10.0 # IMU sample frequency
fs_gps = 10.0 # GPS sample frequency
fs_mag = fs # magnetometer sample frequency, not used for now
def create_sim_imu(): #生成 imu 方针imu数据
### Customized IMU model parameters, typical for IMU381
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
'accel_b_stability': np.array([1e-5, 1e-5, 1e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# 'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def6.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('/home/kaho/chapter3/kaho_chapter3/gnss-ins-sim/imu_data/data6') # save data files
if __name__ == '__main__':
create_sim_imu()
2909
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
