正经夜光杯

这个屌丝很懒，什么也没留下！

热门标签

IMU加速度、角速度、零偏Bias、高斯白噪声_imu主要参数

作者：正经夜光杯 | 2024-07-27 20:47:02

踩

imu主要参数

ORB_SLAM3优势

IMU(Inertial Measurement Unit),惯性测量单元
• 典型 6 轴 IMU 以较高频率(≥ 100Hz)返回被测量物体的角速度与加速度
• 受自身温度、零偏、振动等因素干扰,积分得到的平移和旋转容易漂移

六自由度 IMU 本身由一个陀螺仪和一个加速度计组成,分别测量自身的角速度和加速度。

IMU 适合计算短时间、快速的运动;

快速响应,不受成像质量影响,角速度普遍比较准确,可估计绝对尺度

存在零偏,低精度 IMU 积分位姿发散,高精度价格昂贵

可利用视觉定位信息来估计 IMU 的零偏,减少 IMU 由零偏导,致的发散和累积误差;

视觉 Visual Odometry
• 以图像形式记录数据,频率较低(15 − 60Hz 居多)
• 通过图像特征点或像素推断相机运动

视觉适合计算长时间、慢速的运动。

不产生漂移,直接测量旋转与平移

受图像遮挡,运动物体干扰,单目视觉无法测量尺度,单目纯旋转运动无法估计,快速运动时易丢失

IMU 可以为视觉提供快速运动时的定位

单目视觉或 IMU 不具备估计 Pose 的能力，视觉存在尺度不确定性、IMU 存在零偏导致漂移。
松耦合中：视觉内部 BA 没有 IMU 的信息在整体层面来看不是最优的。
紧耦合中：可以一次性建模所有的运动和测量信息更容易达到最优。

在这里插入图片描述

单纯凭(单目)视觉或 IMU 都不具备估计 Pose 的能力:视觉存,在尺度不确定性、IMU 存在零偏导致漂移;
• 松耦合中,视觉内部 BA 没有 IMU 的信息,在整体层面来看不是最优的。
• 紧耦合可以一次性建模所有的运动和测量信息,更容易达到最优。

IMU的加速度

六自由度 IMU 本身由加速度计测量自身的加速度。

IMU的角速度

六自由度 IMU 本身由一个陀螺仪测量自身的角速度。

IMU的零偏(Bias)

https://www.zhihu.com/question/352293998
在这里插入图片描述

在陀螺静止时仍会以规定时间内测得的输出量平均值等效输入角速率单位为°/h，°/s。
理想状态下该数值应为地球自转角速度的分量。
在角速度输入为零时陀螺仪的输出是一条复合白噪声信号缓慢变化的曲线，曲线的平均值就是零偏值。

在这里插入图片描述

IMU的高斯白噪声

https://www.cnblogs.com/wt869054461/p/7169072.html
在这里插入图片描述

IMU的Scale

在这里插入图片描述

VIO 中的 IMU 模型

在这里插入图片描述

ORB_SLAM3相机和IMU位置姿态描述

在这里插入图片描述
如 I 到 W 系的变换矩阵为:T W I,右乘一个 I 系下(齐次)坐标,将得到该点 W 系下坐标

在这里插入图片描述

质量块是我们测量IMU的一部分，是真执行计算的部分，二者分别处于不同的坐标系。

从零手写VIO|第二节——imu.cpp代码解析

用欧拉角表示body坐标系到惯性系的旋转,下方公式表示的是用欧拉角表示从惯性系到body系的转换：
在这里插入图片描述

// euler2Rotation:   body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
    double yaw = eulerAngles(2);
<span class="token keyword">double</span> cr <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sr <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">double</span> cp <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sp <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">double</span> cy <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>yaw<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sy <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>yaw<span class="token punctuation">)</span><span class="token punctuation">;</span>

Eigen<span class="token double-colon punctuation">::</span>Matrix3d RIb<span class="token punctuation">;</span>
RIb<span class="token operator">&lt;&lt;</span> cy<span class="token operator">*</span>cp <span class="token punctuation">,</span>   cy<span class="token operator">*</span>sp<span class="token operator">*</span>sr <span class="token operator">-</span> sy<span class="token operator">*</span>cr<span class="token punctuation">,</span>   sy<span class="token operator">*</span>sr <span class="token operator">+</span> cy<span class="token operator">*</span> cr<span class="token operator">*</span>sp<span class="token punctuation">,</span>
        sy<span class="token operator">*</span>cp<span class="token punctuation">,</span>    cy <span class="token operator">*</span>cr <span class="token operator">+</span> sy<span class="token operator">*</span>sr<span class="token operator">*</span>sp<span class="token punctuation">,</span>  sp<span class="token operator">*</span>sy<span class="token operator">*</span>cr <span class="token operator">-</span> cy<span class="token operator">*</span>sr<span class="token punctuation">,</span>
        <span class="token operator">-</span>sp<span class="token punctuation">,</span>         cp<span class="token operator">*</span>sr<span class="token punctuation">,</span>           cp<span class="token operator">*</span>cr<span class="token punctuation">;</span>
<span class="token keyword">return</span> RIb<span class="token punctuation">;</span>
1
2
3
4
5
6
7
8
9
1
2
3
4
5
6

}

惯性系下的欧拉角速度转换到body坐标系
在这里插入图片描述

Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
<span class="token keyword">double</span> cr <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sr <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">double</span> cp <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sp <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span>

Eigen<span class="token double-colon punctuation">::</span>Matrix3d R<span class="token punctuation">;</span>
R<span class="token operator">&lt;&lt;</span>  <span class="token number">1</span><span class="token punctuation">,</span>   <span class="token number">0</span><span class="token punctuation">,</span>    <span class="token operator">-</span>sp<span class="token punctuation">,</span>
        <span class="token number">0</span><span class="token punctuation">,</span>   cr<span class="token punctuation">,</span>   sr<span class="token operator">*</span>cp<span class="token punctuation">,</span>
        <span class="token number">0</span><span class="token punctuation">,</span>   <span class="token operator">-</span>sr<span class="token punctuation">,</span>  cr<span class="token operator">*</span>cp<span class="token punctuation">;</span>

<span class="token keyword">return</span> R<span class="token punctuation">;</span>
1
2
3
4
5
6
7
8
9
1
2
3
4

}

运动模型的离散积分-欧拉法

运动学模型模型线性化-离散化
https://zhuanlan.zhihu.com/p/156822207
在这里插入图片描述

运动模型的离散积分－中值法

在这里插入图片描述

B-Spline 产生 IMU数据

B样条曲线（B-spline Curves）
Bezier曲线有以下几个不足点，所以导致出现了B-spline算法：
一旦确定特征多边形，就确定了曲线的阶次
Bezier曲线拼接复杂（需要满足几何连续性，参数连续性等）
Bezier曲线不能作局部修改（只能整体修改）
B-spline算法是整条曲线用一段一段的曲线连接而成，采用分段连续多段式生成
[从零手写VIO|第二节]从已有轨迹生成imu数据推导
 从零手写VIO|第二节——imu.cpp代码解析

imusim代码实战教程

imu和cam数据仿真，用于vio算法测试。

创建工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
编译工作空间
cd ~/catkin_ws/
catkin_make
当前终端生效的工作空间环境变量配置方法
source  ~/catkin_ws/devel/setup.bash
然后把代码放到src目录下面，再次编译

 
 
 
 1
2
3
4
5
6
7
8
9
10
1
2
3
4
5
6
7
8
9
10

中值积分和欧拉积分
在这里插入图片描述

在这里插入图片描述

坐标系

Body frame: imu坐标系
Cam frame: 相机坐标系
World frame: imu坐标系的第一帧位置

Navigation frame: NED（北东地）or ENU（东北天），本代码采用的是ENU，重力向量在该坐标系下为

     (
    
    
     0
    
    
     ,
    
    
     0
    
    
     ,
    
    
     −
    
    
     9.81
    
    
     )
    
   
   
    (0,0,-9.81)
   
  
 </span><span class="katex-html"><span class="base"><span class="strut" style="height: 1em; vertical-align: -0.25em;"></span><span class="mopen">(</span><span class="mord">0</span><span class="mpunct">,</span><span class="mspace" style="margin-right: 0.166667em;"></span><span class="mord">0</span><span class="mpunct">,</span><span class="mspace" style="margin-right: 0.166667em;"></span><span class="mord">−</span><span class="mord">9</span><span class="mord">.</span><span class="mord">8</span><span class="mord">1</span><span class="mclose">)</span></span></span></span></span></li></ul> 
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

目前，imu的z轴向上，xy平面内做椭圆运动，z轴做正弦运动，x轴沿着圆周向外。

外参数Tbc将相机坐标旋转，使得相机朝向特征点。

代码结构

main/gener_alldata.cpp : 用于生成imu数据，相机轨迹，特征点像素坐标，特征点的3d坐标

src/paramc.h：imu噪声参数，imu频率，相机内参数等等

src/camera_model.cpp：相机模型，调用的svo，目前代码里这个文件删掉了

python_tool/：文件夹里为可视化工具，draw_points.py就是动态绘制相机轨迹和观测到的特征点。如果是ubuntu不需额外安装，windows需要安装python matplot等依赖项

数据存储的格式

特征点

x，y，z，1，u，v

每个特征出现在文件里的顺序，就是他们独立的id，可用来检索特征匹配

imu data

timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3)

cam data

timestamp (1)，cam quaternion(4)，cam position(3)，imu gyro(3)，imu acc(3)

注意，由于imu和cam的存储采用的是同一个函数，所以cam也会存储一些gyro,acc这些数据，但是没用，是多余存储的。

void CreatePointsLines(Points& points, Lines& lines)

using Point = Eigen::Vector4d;

 
 
 
 1
1

C++11使用using定义别名（替代typedef）

using Points = std::vector<Point, Eigen::aligned_allocator<Point> >;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 
 
 
 1
2
1
2

从Eigen向量化谈内存对齐

C++,vector模板类的问题？ std::vector<PointT, Eigen::aligned_allocator > points；

using Line = std::pair<Eigen::Vector4d, Eigen::Vector4d>;

 
 
 
 1
1

STL pair的使用

using Lines = std::vector<Line, Eigen::aligned_allocator<Line> >;

 
 
 
 1
1

std::vector使用总结

Eigen::Vector4d pt1( x, y, z, 1 );
lines.emplace_back(pt0, pt1);

 
 
 
 1
2
1
2

生成两个Eigen::Vector4d的点，两个点连成一条线。

emplace_back()和push_back()的区别

emplace_back() 和 push_back() 的区别，就在于底层实现的机制不同。push_back() 向容器尾部添加元素时，首先会创建这个元素，然后再将这个元素拷贝或者移动到容器中（如果是拷贝的话，事后会自行销毁先前创建的这个元素）；而 emplace_back() 在实现时，则是直接在容器尾部创建这个元素，省去了拷贝或移动元素的过程。

C++ STL vector添加元素（push_back()和emplace_back()）详解

Eigen::Quaterniond

三维空间的旋转可以用欧拉角，旋转向量，旋转矩阵，四元数来表示。
欧拉角表示法，我们可以用绕某个轴旋转来表示。
旋转向量就是用一个旋转轴和一个旋转角来表示旋转。
旋转矩阵用一个矩阵来表示空间中的旋转变换关系。
四元数用４个变量来表示旋转增加一个维度，避免万向锁。

Eigen库中各种形式的表示如下
Eigen::AngleAxisd //旋转向量（3*1）
Eigen::Matrix3d //旋转矩阵（3*3）
Eigen::Quaterniond //四元数（4*1）
Eigen::Vector3d //平移向量或欧拉角（3*1）
Eigen::Isometry3d //欧式变换矩阵（4*4）
Eigen::Affine3d      //仿射变换矩阵（4*4）
Eigen::Projective3d  //射影变换矩阵（4*4）
Eigen::Quaterniond q1(w, x, y, z);// 第一种方式,实部是前面的w
Eigen::Quaterniond q2(Vector4d(x, y, z, w));// 第二种方式,实部是后面的w
Eigen::Quaterniond q2(Matrix3d(R));// 第三种方式由旋旋转矩阵构造四元数
Eigen::Quaterniond Qwb;
Qwb.setIdentity();// 表示单位旋转的四元数
Eigen::Quaterniond.coeffs().transpose()
coeffs的顺序是(x,y,z,w) w为实部，前三者为虚部，输出四元数的每一个值。
从零开始手写vio作业-ch1-编程实现旋转的更新(分别用旋转矩阵和四元数实现)
https://blog.csdn.net/joun772/article/details/110133708

 
 
 
 1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

Eigen::MatrixXd和VectorXd的用法注意

//方式一：用matrix()
//方式二：用toRotationMatrix()
Eigen::Quaterniond.toRotationMatrix()

 
 
 
 1
2
3
1
2
3

将四元数转换为旋转矩阵

时间戳

double dt_tmp = 0.005;
for (double i = 0; i < 20.; i += dt_tmp) {

 
 
 
 1
2
1
2

浅显理解激光雷达的时间同步。

《The swirlds hashgraph consensus algorithm: Fair, fast, byzantine fault tolerance》Hashgraph论文的学习

采用一致的时间戳(当前时间的一个数字序列)，每一个区块（实际上是事件，下文会谈到）以及区块里的每一笔交易都有顺序

无论是相机还是IMU如果同时启动，那么在这个时间就会产生两个数据一个是图像一个是IMU输出的加速度和角速度，假设每一个数据没有时间戳去标记的化，那么再次读取的时候我们怎么确定先后顺序呢？也没办法找到图片输出的那一时刻IMU的数据，所以在去中心化系统中唯一的标识就是时间戳。例如git每一个提交的版本的就是一个时间戳，只不过形式是哈希值，当然我们也可以自己定义规则去生成数字序列。

mkdir("keyframe", 0777);
// 头文件：sys/types.h, sys/stat.h
// 函数：int mkdir(const char *pathname, mode_t mode)
1
2
3

函数：int rmdir(const char *_Path)
头文件：sys/types.h, sys/stat.h
功能：删除文件夹，成功返回0，否则-1。

功能：创建文件夹，返回0为创建成功，否则返回-1。

Linux中创建时会有权限要求，该权限参数可以去了解Linux对于文件权限的设置相关内容，例如：0777表示对文件具有完全的权限。

构造函数

class Param{
public:
    Param();

 
 
 
 1
2
3
1
2
3

构造函数（Constructor）名字和类名相同，没有返回值。

如果没有构造函数要通过成员函数 setname()、setage()、setscore() 分别为成员变量 name、age、score 赋值，这样做虽然有效，但显得有点麻烦。

构造函数Student(char *, int, float)，它的作用是给三个 private 属性的成员变量赋值。要想调用该构造函数，就得在创建对象的同时传递实参，并且实参由( )包围，和普通的函数调用非常类似。

不管是构造函数还是普通函数都是定义的时候是形式参数，传递的是实际参数，

构造函数和析构函数的由来

类的数据成员不能在类的声明时候初始化,为了解决这个问题? 使用构造函数处理对对象的初始化。构造函数是一种特殊的成员函数,与其他函数不同,不需要用户调用它,而是创建对象的时候自动调用。析构函数是对象不再使用的时候,需要清理资源的时候调用。

构造函数和析构函数的基本语法

构造函数

C++中的类需要定义与类名相同的特殊成员函数时,这种与类名相同的成员函数叫做构造函数;
构造函数可以在定义的时候有参数;
构造函数没有任何返回类型。
构造函数的调用: 一般情况下,C++编译器会自动的调用构造函数。特殊情况下,需要手工的调用构造函数。

析构函数

C++中的类可以定义一个特殊的成员函数清理对象,这个特殊的函数是析构函数;
析构函数没有参数和没有任何返回类型;
析构函数在对象销毁的时候自动调用;
析构函数调用机制: C++编译器自动调用。

C++类成员的访问权限以及类的封装

C++构造函数详解

形式参数和实际参数

形参（形式参数）
在函数定义中出现的参数可以看做是一个占位符，它没有数据，只能等到函数被调用时接收传递进来的数据，所以称为形式参数，简称形参。

这里按自己的理解为什么说形参看作一个占位符，后文还说到在调用之前并没有给他分配内存，看上去这两段是有些矛盾的，其实机器在时间上的运行顺序这么说没毛病；

本人理解是在程序编译过程中，程序计数器不断加以程序顺序执行，执行到调用函数位置时，会将之前的数据压栈就像中断一样，调用前要传递实参值对应的内存位置并没有开辟，所以可以说形参再调用前不占内存；

但是这里没有开辟不是说实参会传到随机的位置，这是不会发生的，程序在编译好之后，每执行一条指令，数据都会有个确切的内存位置对应（这是在编译器编译的时候就已经确定好的），所以这么说来，形参确实是一个占位符（占有一个内存地址的位置）；

只是调用前别的参数也可以占用这个位置，但编译器编译的时候肯定不会让一个全局变量的内存地址和分配给形参的内存地址是同一个地址，原因大家应该通过上面的解释也明白编译器在编译时不会让两个没有关联的变量，同时服务于一个地址这样会造成数据的混乱。

就像两个服务员负责同一桌客人时，当客人需要一瓶饮料时，两个服务员分别在不同时刻听到，可能就会造成给客人拿两瓶情况。

实参（实际参数）
函数被调用时给出的参数包含了实实在在的数据，会被函数内部的代码使用，所以称为实际参数，简称实参。

形参和实参的功能是传递数据，发生函数调用时，实参的值会传递给形参。

形参和实参的区别和联系

形参变量只有在函数被调用时才会分配内存，调用结束后，立刻释放内存，所以形参变量只有在函数内部有效，不能在函数外部使用。
实参可以是常量、变量、表达式、函数等，无论实参是何种类型的数据，在进行函数调用时，它们都必须有确定的值，以便把这些值传送给形参，所以应该提前用赋值、输入等办法使实参获得确定值。
实参和形参在数量上、类型上、顺序上必须严格一致，否则会发生“类型不匹配”的错误。当然，如果能够进行自动类型转换，或者进行了强制类型转换，那么实参类型也可以不同于形参类型。
函数调用中发生的数据传递是单向的，只能把实参的值传递给形参，而不能把形参的值反向地传递给实参；换句话说，一旦完成数据的传递，实参和形参就再也没有瓜葛了，所以，在函数调用过程中，形参的值发生改变并不会影响实参。
形参和实参虽然可以同名，但它们之间是相互独立的，互不影响，因为实参在函数外部有效，而形参在函数内部有效。

源地址：https://www.cnblogs.com/zhj868/p/14180848.html

MotionData IMU::MotionModel(double t)

椭圆函数

在这里插入图片描述

圆和椭圆的参数方程是函数. x=acosθ ， y=bsinθ

一.圆锥曲线的参数方程：

圆锥曲线的参数方程与三角换元思想联系紧密。

譬如x2/4 +y2/3=1，把左边两项看成同角正余弦平方之和，列出x2/4=（cosa）2、y2/3=（sina）2

再反解出x与y，此时不需考虑正负号，因为一个角cos与sin可以取遍【-1，1】

最后用大括号将解出的x与y括起来，记得用小括号标注a（你用的那个角）为参数。

双曲线同理。个人推荐题主掌握三角函数的相关思想，这对求取值范围、最值等问题十分有帮助！

二.圆锥曲线的极坐标方程：

用极坐标的定义直接变形。

在以x轴非负半轴为极轴的极坐标系中，所有点都满足x=pcosa、y=psina。

故将上面两式直接带入圆锥曲线方程，即得到了圆锥曲线的极坐标方程。

关于极坐标

极坐标就是：用角度和长度描述位置的坐标系

// 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
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);

 
 
 
 1
2
3
4
5
6
7
8
9
1
2
3
4
5
6
7
8
9

在这里插入图片描述

对称中心不在原点的椭圆

在这里插入图片描述

Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);

 
 
 
 1
1

所以这里就是定义一个对称中心不在原点的椭圆，用极坐标表示。

Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));  

 
 
 
 1
1

一阶导数

Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));

 
 
 
 1
1

二阶导数

float K1 = 10;          // z轴的正弦频率是x，y的k1倍

 
 
 
 1
1

正弦型函数是形如y=Asin(ωx+φ)+k的函数

A称为振幅，ω称为圆频率或角频率，φ称为初相位或初相角。

往复振动一次所需要的时间T=2π/ω，它叫做振动的周期。

单位时间内往复振动的次数f=1/T=ω/2π，它叫做振动的频率。

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]

 
 
 
 1
1

https://blog.csdn.net/qq_21950671/article/details/100080945

euler2Rotation

 
 
 
 1
1

欧拉角和旋转矩阵的转换
在这里插入图片描述

在这里插入图片描述

Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0)：旋转矩阵R用欧拉角euler_angles表示。 
//旋转向量使用AngleAxis，运算可以当做矩阵沿着Z轴旋转45°
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0,0,1));   
1
2
3

Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double yaw = eulerAngles(2);

<span class="token keyword">double</span> cr <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sr <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>roll<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">double</span> cp <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sp <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>pitch<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">double</span> cy <span class="token operator">=</span> <span class="token function">cos</span><span class="token punctuation">(</span>yaw<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">double</span> sy <span class="token operator">=</span> <span class="token function">sin</span><span class="token punctuation">(</span>yaw<span class="token punctuation">)</span><span class="token punctuation">;</span>

Eigen<span class="token double-colon punctuation">::</span>Matrix3d RIb<span class="token punctuation">;</span>
RIb<span class="token operator">&lt;&lt;</span> cy<span class="token operator">*</span>cp <span class="token punctuation">,</span>   cy<span class="token operator">*</span>sp<span class="token operator">*</span>sr <span class="token operator">-</span> sy<span class="token operator">*</span>cr<span class="token punctuation">,</span>   sy<span class="token operator">*</span>sr <span class="token operator">+</span> cy<span class="token operator">*</span> cr<span class="token operator">*</span>sp<span class="token punctuation">,</span>
        sy<span class="token operator">*</span>cp<span class="token punctuation">,</span>    cy <span class="token operator">*</span>cr <span class="token operator">+</span> sy<span class="token operator">*</span>sr<span class="token operator">*</span>sp<span class="token punctuation">,</span>  sp<span class="token operator">*</span>sy<span class="token operator">*</span>cr <span class="token operator">-</span> cy<span class="token operator">*</span>sr<span class="token punctuation">,</span>
        <span class="token operator">-</span>sp<span class="token punctuation">,</span>         cp<span class="token operator">*</span>sr<span class="token punctuation">,</span>           cp<span class="token operator">*</span>cr<span class="token punctuation">;</span>
<span class="token keyword">return</span> RIb<span class="token punctuation">;</span>
1
2
3
4
5
6
7
8
9

欧拉角转旋转矩阵

Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
        1,       0,              0,
        0,       cos(theta[0]),   -sin(theta[0]),
        0,       sin(theta[0]),   cos(theta[0])
    );
    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
        cos(theta[1]),    0,      sin(theta[1]),
        0,               1,      0,
        -sin(theta[1]),   0,      cos(theta[1])
    );
    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
        cos(theta[2]),    -sin(theta[2]),      0,
        sin(theta[2]),    cos(theta[2]),       0,
        0,               0,                  1
    );
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;
    return R;
}
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    #if 1
    x = x*180.0f/3.141592653589793f;
    y = y*180.0f/3.141592653589793f;
    z = z*180.0f/3.141592653589793f;
    #endif
    return Vec3f(x, y, z);
}

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

旋转矩阵到欧拉角
在这里插入图片描述

//由旋转矩阵计算欧拉角
        private double[] rotationMatrixToEulerAngles(double[] M)
        {
            double R00 = M[0], R01 = M[1], R02 = M[2];
            double R10 = M[4], R11 = M[5], R12 = M[6];
            double R20 = M[8], R21 = M[9], R22 = M[10];
        double sy = Math.Sqrt(R00 * R00 + R10 * R10);

        bool singular = sy &lt; 1e-6; // If

        double x, y, z;
        if (!singular)
        {
            x = Math.Atan2(R21, R22);
            y = Math.Atan2(-R20, sy);
            z = Math.Atan2(R10, R00);
        }
        else
        {
            x = Math.Atan2(-R12, R11);
            y = Math.Atan2(-R20, sy);
            z = 0;
        }
        x = x * 180.0 / Math.PI;
        y = y * 180.0 / Math.PI;
        z = z * 180.0 / Math.PI;
        double[] angle = new double[3] { x, y, z };
        return angle;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
1
2
3
4
5
6

从两帧IMU数据中获得当前位姿的预测思路非常简单，

无非是求出当前时刻

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/正经夜光杯/article/detail/891618

IMU加速度、角速度、零偏Bias、高斯白噪声_imu主要参数

文章目录

ORB_SLAM3优势

IMU的加速度

IMU的角速度

IMU的零偏(Bias)

IMU的高斯白噪声

IMU的Scale

VIO 中的 IMU 模型

ORB_SLAM3相机和IMU位置姿态描述

运动模型的离散积分-欧拉法

运动模型的离散积分－中值法

B-Spline 产生 IMU数据

imusim代码实战教程

坐标系

代码结构

数据存储的格式

imu data

cam data

void CreatePointsLines(Points& points, Lines& lines)

Eigen::Quaterniond

时间戳

构造函数

形式参数和实际参数

MotionData IMU::MotionModel(double t)