从零开始手写vio-ch2-imu数据仿真_imu定位轨迹曲线绘制

一、利用欧拉积分和中指=值积分来计算imu数据，并绘制轨迹，与给定的轨迹方程进行比较

利用欧拉积分或者中值积分计算imu数据

核心代码imu.cpp源码如下：

//
// Created by hyj on 18-1-19.
//

#include <random>
#include "imu.h"
#include "utilities.h"

// euler2Rotation:   body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
    double yaw = eulerAngles(2);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
    double cy = cos(yaw); double sy = sin(yaw);

    //RIb=RbI^−1=RbI^T
    Eigen::Matrix3d RIb;//这里对应的是公式 是从i系到b系的旋转矩阵的转置
    //i系到b系的旋转矩阵的公式见ppt 就是Rbi=R(yaw,pith,roll)
    RIb<< cy*cp ,   cy*sp*sr - sy*cr,   sy*sr + cy* cr*sp,
            sy*cp,    cy *cr + sy*sr*sp,  sp*sy*cr - cy*sr,
            -sp,         cp*sr,           cp*cr;
    return RIb;
}
//变换矩阵--将欧拉角速度从I坐标变到body坐标
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
//公式见ppt
    Eigen::Matrix3d R;
    R<<  1,   0,    -sp,
            0,   cr,   sr*cp,
            0,   -sr,  cr*cp;

    return R;
}


IMU::IMU(Param p): param_(p)
{
    gyro_bias_ = Eigen::Vector3d::Zero();
    acc_bias_ = Eigen::Vector3d::Zero();
}

//加入噪声
void IMU::addIMUnoise(MotionData& data)
{
    std::random_device rd;
    std::default_random_engine generator_(rd());
    std::normal_distribution<double> noise(0.0, 1.0);

    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
    data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;

    Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
    data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

    // gyro_bias update
    Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
    gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
    data.imu_gyro_bias = gyro_bias_;

    // acc_bias update
    Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
    acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
    data.imu_acc_bias = acc_bias_;

}

得到body坐标系下的IMU测量值
MotionData IMU::MotionModel(double t)
{

    MotionData data;
    // param
    float ellipse_x = 15;//椭圆长轴
    float ellipse_y = 20;//椭圆短轴
    float z = 1;           // z轴做sin运动
    float K1 = 10;          // z轴的正弦频率是x，y的k1倍
    float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周

    // translation
    // twb:  body frame in world frame
    // position(t)=[ellipsex∗cos(K∗t)+5,ellipsey∗sin(K∗t)+5,z∗sin(K1∗K∗t)+5]
    //设定质量快运动轨迹方程（以例程中椭圆为例）
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);
    // 上面知道了位置矢量方程 一次求导 求出质量块速度方程
    Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));              // position导数　in world frame
    double K2 = K*K;//
    //对质量块速度方程再求一次导，得到质量块加速度方程
    Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));     // position二阶导数

    // Rotation
    double k_roll = 0.1;
    double k_pitch = 0.2;
    /**
     * 在东北天坐标系中，yaw，pitch，roll对应的是绕载体坐标系的z，y，x轴。在北东地坐标系中，yaw，pitch，roll对应的是绕载体坐标系的y, x, z轴。在IMU仿真程序中，我们采用东北天坐标系，欧拉角顺规为yaw，pitch，roll。具体的值为

ϑ \vartheta ϑ = （ k r o l l ∗ c o s ( t ) , k p i t c h ∗ s i n ( t ) , K ∗ t ） T （kroll * cos(t) , kpitch * sin(t) , K*t）^T （kroll∗cos(t),kpitch∗sin(t),K∗t）^T
     **/
    //质量块的欧拉角 是body系在不同时刻相对于惯性系所转角度
    Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t );   // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]
    // 上面的质量块欧拉角求导得到欧拉角速度
    Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);      // euler angles 的导数

//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen::Vector3d eulerAnglesRates(0.,0. , K);      // euler angles 的导数

    Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);  //得到从b坐标系到I坐标系的旋转矩阵(这里I坐标系就是W坐标系？？)   这里可以参考从零手写vio-ch2笔记    // body frame to world frame
    //eulerRates2bodyRates(eulerAngles)//b坐标系到I坐标系的角速度的变换矩阵 这里可以参考从零手写vio-ch2笔记
    Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

    Eigen::Vector3d gn (0,0,-9.81);     //东北天 导航系                              //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
    Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //   imu_acc是测量值  ddp是imu得到的值   Rbw * Rwn * gn = gs

    data.imu_gyro = imu_gyro;//角速度
    data.imu_acc = imu_acc;//加速度
    data.Rwb = Rwb;//旋转矩阵 从body 坐标系到world(I系？)
    data.twb = position;//位置，也可理解为平移
    data.imu_velocity = dp;//速度 速度就是轨迹方程的导数
    data.timestamp = t;//时间戳
    return data;

}

读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，比对积分出来的轨迹与自己设定出来的轨迹是不是一样的
用来验证数据以及模型的有效性。
void IMU::testImu(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);
//初始化
    double dt = param_.imu_timestep;//理解为离散的采样时间吧
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    //读取imu数据
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];
        /// imu 动力学模型 欧拉积分 加入bias
       /* //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half =  (imupose.imu_gyro-imupose.imu_gyro_bias) * dt /2.0;
        //这里的dq就是离散时间段的旋转小量
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();
        
        //  这一部分对应ppt欧拉法公式
        Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc-imupose.imu_acc_bias) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        Qwb = Qwb * dq;//imu的速度加上旋转
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;*/

        /// 中值积分 加入噪声
        Eigen::Quaterniond Qwbk,Qwbk1,dq;//dq是旋转小量
        Eigen::Vector3d midOmega;
        Eigen::Vector3d dtheta_half;
        midOmega=(imudata[i].imu_gyro-imudata[i-1].imu_gyro_bias+imudata[i-1].imu_gyro-imudata[i-1].imu_gyro_bias)/2;
        dtheta_half=midOmega*dt/2.0;//这就是公式的旋转小量的 1/2* w *dt
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();//将小量四元数单位化
        Qwbk=Qwb;
        Qwbk1=Qwbk*dq;//得到下一时刻的四元数-旋转
        Eigen::Vector3d acc_w =0.5*( Qwbk * (imudata[i-1].imu_acc-imudata[i-1].imu_acc_bias) + gw +Qwbk1 * (imupose.imu_acc-imudata[i-1].imu_acc_bias) + gw );
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;
        Qwb = Qwbk1;




        //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;

    }

    std::cout<<"test　end"<<std::endl;

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217

没有加噪声

欧拉法：
中值法：

加噪声

欧拉法：
中值法：

实验总结

在没有加入噪声之前，可以看出欧拉法的误差比较大，中值法基本与轨迹重合。
加入噪声后，有噪声的数据随着时间推移，误差越来越大，直到完全脱离轨迹。

二、Allan方差标定

参考博客：Allan