多传感器融合定位（3-惯性导航原理及误差分析）8-惯性导航解算验证


多传感器融合定位（3-惯性导航原理及误差分析）8-惯性导航误差分析

本次作业摘自 张松鹏大哥的优秀作业

eg.
在现实中 高精度惯导中用等效旋转矢量，低精度惯导中，用角增量；一般中低精度的导航里面，提高的比例都在10%以内，没有太高；实际系统中可以做个实验对比一下，看能提高多少，来决定要不要使用等效旋转矢量的方法解算

我们先来回顾一下姿态更新的框架图
两种姿态解算方法：
1.使用theta t 时间内角增量做积分中值处理
2.使用角增量求解等效旋转矢量的方法

注：表征姿态变化，一般使用四元数较为方便
在这里插入图片描述
张松鹏的优秀作业代码github地址

这里对松鹏大神的部分代码进行自己理解记录和一些修改

仿真数据集准备

这里使用的imu 生成仿真软件为 gnss-ins-sim
自定义运动场景 imu_def5.csv

自定义场景

在这里插入图片描述
在这里插入图片描述

生成仿真数据 sim_imu.py

运行 如下指令即可生成仿真数据，注意生成文件的路径

1
python sim_imu.py
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
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数据集

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
// 读取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
            }
        }
    }
}

输入文件路径

1
2
3
4
5
6
7
8
 //  输入路径，读取数据
    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 评估工具进行数据评估

1
2
3
4
5
6
    // 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);

初始化旋转量和一些地球参数常量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
    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 ;

角增量姿态解算

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
        /*角增量姿态解算*/
        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）—— 轴角表示法与四元数表示的区别

1
2
3
4
5
6
7
8
9
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的旋转

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
        //速度解算
        /*位置其实算的是相对于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;

解算结果

在两个不同的终端，分别运行

1
2
./resolving 0
./resolving 1

使用evo 里程计评估工具进行评估

1
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

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
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
#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

1
2
3
4
5
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

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
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()