imu绘制轨迹

本文作者感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台。

首先声明，仅仅是实现，实际应用意义不大

这套算法利用EKF更新误差并且补偿到状态更新，如果没有gps信号时，利用imu信息运行捷联惯导算法进行短时间的状态更新，这个短时间由自己的imu质量高低决定

在这套算法中存在gps信号判断，为了实现单独imu轨迹绘制，将这个判断始终设置为Faule，也就是只运行捷联惯导算法，但这里存在一个问题，就是捷联惯导算法也会使用gps信息，主要用来计算重力和地球向心加速度，为了彻底避免使用gps信息，可以将重力设置为常量9.8，其余gps计算得出的相关量全部设置为零。

修改src/kf-gins/gi_engine.cpp，将

int res = isToUpdate(imupre_.time, imucur_.time, updatetime);

修改为

int res = 0;

下面是修改后的imesh.cpp文件

#include "common/earth.h"
#include "common/rotation.h"

#include "insmech.h"

void INSMech::insMech(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {

    // perform velocity update, position updata and attitude update in sequence, irreversible order
    // 依次进行速度更新、位置更新、姿态更新, 不可调换顺序
    std::cout<<imucur.time<<std::endl;
    std::cout<<imucur.dtheta<<std::endl;
    std::cout<<imucur.dvel<<std::endl;
    std::cout<<imucur.dt<<std::endl;
    velUpdate(pvapre, pvacur, imupre, imucur);
    posUpdate(pvapre, pvacur, imupre, imucur);
    attUpdate(pvapre, pvacur, imupre, imucur);
}

void INSMech::velUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {

    Eigen::Vector3d d_vfb, d_vfn, d_vgn, gl, midvel, midpos;
    Eigen::Vector3d temp1, temp2, temp3;
    Eigen::Matrix3d cnn, I33 = Eigen::Matrix3d::Identity();
    Eigen::Quaterniond qne, qee, qnn, qbb, q1, q2;

    Eigen::Vector2d rmrn = Earth::meridianPrimeVerticalRadius(pvapre.pos(0));
    Eigen::Vector3d wie_n, wen_n;
    wie_n << 0, 0, 0;
    wen_n << 0, 0, 0;
    double gravity = 9.8;
    temp1 = imucur.dtheta.cross(imucur.dvel) / 2;
    temp2 = imupre.dtheta.cross(imucur.dvel) / 12;
    temp3 = imupre.dvel.cross(imucur.dtheta) / 12;
    d_vfb = imucur.dvel + temp1 + temp2 + temp3;
    temp1 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp1);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;
    gl << 0, 0, gravity;
    d_vgn = (gl - (2 * wie_n + wen_n).cross(pvapre.vel)) * imucur.dt;
    midvel = pvapre.vel + (d_vfn + d_vgn) / 2;
    temp3 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp3);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;
    gl << 0, 0, gravity;
    d_vgn = (gl - (2 * wie_n + wen_n).cross(midvel)) * imucur.dt;
    pvacur.vel = pvapre.vel + d_vfn + d_vgn;
}

void INSMech::posUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {

    Eigen::Vector3d temp1, temp2, midvel, midpos;
    Eigen::Quaterniond qne, qee, qnn;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvacur.vel + pvapre.vel) / 2;
    midpos = pvapre.pos + Earth::DRi(pvapre.pos) * midvel * imucur.dt / 2;

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 重新计算 k时刻到k-1时刻 n系旋转矢量
    // recompute n-frame rotation vector (n(k) with respect to n(k-1)-frame)
    temp1 = (wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);
    // e系转动等效旋转矢量 (k-1时刻k时刻，所以取负号)
    // e-frame rotation vector (e(k-1) with respect to e(k)-frame)
    temp2 << 0, 0, -WGS84_WIE * imucur.dt;
    qee = Rotation::rotvec2quaternion(temp2);

    // 位置更新完成
    // position update finish
    qne           = Earth::qne(pvapre.pos);
    qne           = qee * qne * qnn;
    pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt;
    pvacur.pos    = Earth::blh(qne, pvacur.pos[2]);
}

void INSMech::attUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {

    Eigen::Quaterniond qne_pre, qne_cur, qne_mid, qnn, qbb;
    Eigen::Vector3d temp1, midpos, midvel;
    Eigen::Vector3d wie_n, wen_n;
    wie_n << 0.000, 0.000, 0.000;
    wen_n << 0.000, 0.000, 0.000;
    temp1 = -(wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);
    temp1 = imucur.dtheta + imupre.dtheta.cross(imucur.dtheta) / 12;
    qbb   = Rotation::rotvec2quaternion(temp1);
    pvacur.att.qbn   = qnn * pvapre.att.qbn * qbb;
    pvacur.att.cbn   = Rotation::quaternion2matrix(pvacur.att.qbn);
    pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}

Kf-gins更新的状态为三轴速度，位置（经纬度高程），姿态（欧拉角），仅依赖三轴速度就可以绘制轨迹，下面是代码

import numpy as np
import matplotlib.pyplot as plt



#打开结果文件
with open("/home/workspace/KF-GINS/dataset/KF_GINS_Navresult.nav","r") as f:
    datas = f.readlines()

 #先把速度提出来
dv = []
for data in datas:
    data_split = data.split(" ")
    data = [float(i) for i in data_split if i != "" and i != "\n"]
    dv.append([data[1],data[5],data[6],data[7]])
dv = np.array(dv)

x = np.zeros(3,)
sx = []
sy = []
sz = []
for i in range(len(dv)-1):
    pre_data = dv[i]
    cur_data = dv[i+1]
    v0 = pre_data[1:]
    dt = cur_data[0] - pre_data[0]
    if dt == 0:
        a = 0
    else:
        a = (cur_data[1:]-v0)/dt
    dx = v0 * dt + 0.5 * a *dt **2
    x += dx
    sx.append(x[0])
    sy.append(x[1])
    sz.append(x[2])

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.scatter(sx,sy,sz)
ax.set_xlabel("X Label")
ax.set_ylabel("Y Label")
ax.set_zlabel("Z Label")
ax.set_title("3D scatter plot")
plt.show()

---

先用KF-gins自带数据进行测试，第一幅图为groundtruth，第二幅图为经过修改的算法绘制的轨迹，时长为100s，也就是不到100s就开始漂，而且z轴方向漂的很厉害（单位m)

---

使用自己数据绘制轨迹，俯视图，z轴方向偏的很大。三个轴运动超过三秒轨迹直接起飞


另外，输入数据需要注意的是方向对齐，Kf-gins的数据格式为

IMU坐标系：原点为IMU测量位置，轴向为前-右-下方向
导航参考坐标系：原点和IMU坐标系一致，轴向为北-东-地方向

调整方式为

挪动imu，当三个角（rpy）输出为零的时候，x轴的方向为前，y轴的方向为右，z轴指向下，此时x-y-z轴应该分别指向北-东-地，下载一个指南针，就可以知道xy轴指向的是不是北和东，如果不是，就交换xyz顺序或者加正负号使其变成北东然后输入到Kf-gins算法中