0. 内容
1. 旋转运动学
角速度的推导：
左=
ω
∧
\omega^{\wedge}
ω∧，而
ω
\omega
ω是在z轴方向运动，=
θ
′
[
0
,
0
,
1
]
T
\theta^{\prime}[0,0,1]^T
θ′[0,0,1]T
两边取模后得到结论： 线速度大小=半径 * 角速度大小
其中，对旋转矩阵求导根据第一章的结论：
还有绿色箭头的公式，下面的推导看的不是很明白
实际上是把R的导数变成求极限的形式，但是这个极限怎么求的我不是很理解，然后就是下面这个公式
R
ω
∧
=
(
R
ω
)
∧
R
R\omega^\wedge=(R\omega)^\wedge R
Rω∧=(Rω)∧R
a
=
R
i
b
∗
a
b
a = R_{ib}*a^b
a=Rib∗ab表示body下的加速度在
I
I
I系下的表示，仍是body下的加速度，只是表示在
I
I
I系。
知道了这个科氏力之后，测量出科氏力以及运动的速度就能知道角速度了，这就是gyro的一个基本原理。
2. IMU测量模型及运动模型
光纤陀螺仪一般较贵，原理是：光跑一圈路程是
2
π
r
2\pi r
2πr，但如果在旋转，那就是
2
π
r
+
x
2\pi r+x
2πr+x，测量出来这个x，用光速再进行相关计算就得到此时的角速度。
陀螺仪测角速度要两个轴：一个主动运动轴，一个敏感轴，敏感轴用于预测量科氏力
音叉振动陀螺
音叉两端左方向相反的正弦运动（什么叫正弦运动？音叉不是固定的吗？ ），同一时刻其速度相反，±v，受到的科氏力大小相同方向相反F，整个音叉收到向右的力为f，左右也都为f，把受到的力相减，抵消之后，就能测出两倍科氏力2F，同时，知道自然块运动的速度，就能算出科氏力。
科氏力为 2 v ∗ ω 2v*\omega 2v∗ω，
这个G-sensitivity是灵敏度系数，比如机械振动会不会对IMU测量数据造成影响，如果不那么灵敏，就不会影响，如果比较灵敏，就需要考虑IMU减震等。
加速度计是否需要考虑科氏力影响？
不需要。
因为
1.即使开始加速时质量块会加速，但最终会达到平衡，速度v=0，最终的科氏力为0，
2.加计不是主动驱动的高速运动，会很缓慢地动，最终速度为0。
3. IMU误差模型
3.1 误差模型
3.2 确定性误差
确定性误差有bias和scale，
- bias随着时间会累积，使得位姿的误差 p e r r p_{err} perr越来越大；
- scale可看成是原始物理
v
,
ω
v,\omega
v,ω与ADC后的传感器输出值之间的比值，需要标定。
scale是尺度，Misalignment是轴偏，如yz轴投影到x轴上的轴偏。
不考虑bias时，测量出的
l
a
x
=
s
x
x
∗
a
x
+
m
x
y
∗
a
y
+
m
x
z
∗
a
z
l_{ax}=s_{xx}*a_x + m_{xy}*a_y + m_{xz}*a_z
lax=sxx∗ax+mxy∗ay+mxz∗az，尺度轴偏矩阵主对角线为尺度，其他为轴偏。
其他确定性误差还有
- 运行误差(每次都不一样)，
- 温度相关误差（温度补偿或者标定方法）。
- 环境相关误差（高度，室内外等）
六面法标定bias和scale，分别将xzy三个轴朝上或者下放置，测出的应该是±g，但是会受到bias影响.于是 b = l u p + l d o w n 2 b = \frac{l^{up}+l^{down}}{2} b=2lup+ldown就是两倍bias的均值，反之，相减绝对值就是 2 g 2g 2g，一除就是尺度scale。
3.2.1 六面法标定acc
l
1
l_1
l1 ~
l
6
l_6
l6是加速度测量值，S，b是待标定的尺度轴偏矩阵和bias，
a
1
a_1
a1 ~
a
6
a_6
a6是加速度的理论值，其中
g
=
9.81
g=9.81
g=9.81是标量。如此可以标定出
S
S
S和
b
b
b。
L
=
S
[
a
1
,
a
2
,
a
3
,
a
4
,
a
5
,
a
6
]
+
b
L=S[a_1, a_2,a_3,a_4,a_5,a_6]+b
L=S[a1,a2,a3,a4,a5,a6]+b
最小二乘法能够求出S和b共12个元素。
3.2.2 六面法标定gyro
同理，标定gyro也可以使用这种方法，需要有一个角速度较为精确的转台，也用6面，相对加计，此时我们知道较为精确的角速度
ω
1
\omega_1
ω1~
ω
6
\omega_6
ω6，即可标出gyro的尺度、轴偏、bias。
3.2.3 温度标定
一般采用soak method，精度较高。
3.3 IMU随机误差
3.3.1 IMU随机误差的介绍
bias的导数满足高斯分布
n
(
t
)
n(t)
n(t)（注意是导数而不是bias本身），这个bias的分布被称为随机游走（random walk）。
ADC采样时间段内认为数据是常数，采集的数据=理想数据+bias和随机游走的噪声带来的部分，这个不是常数，所以需要进行积分
仅考虑高斯白噪声时（假设bias和n(t)是相互独立的）的协方差计算推导：
这里的
τ
\tau
τ实际上就是一个自变量，可以是
x
,
y
,
z
x,y,z
x,y,z等任意一个，是因为
t
t
t在这里要代表时间，所以使用了
τ
\tau
τ作为这里的自变量。本身高斯白噪声是满足高斯分布
n
(
t
)
n(t)
n(t)的，就如(13)式所定义。
协方差相关补充：
方差是协方差的特殊情况，方差实际上是对自身的协方差，即
D
(
X
)
=
C
o
v
(
X
,
X
)
D(X)=Cov(X,X)
D(X)=Cov(X,X)，也就是X的二阶原点矩。
补充对于一二阶矩的定义：（一阶矩是期望
E
(
X
)
E(X)
E(X)，二阶原点矩是
E
(
X
2
)
E(X^2)
E(X2)，二阶非原点矩
E
(
(
(
X
−
E
(
X
)
)
2
)
E(\quad((X-E(X))^2\quad)
E(((X−E(X))2)，平方是因为如果
E
(
X
)
E(X)
E(X)不为有0时，若出现了负值，则会使整体二阶矩偏大，如果加了平方就相当于加上了绝对值，就更能体现偏离均值的范围。
这里的协方差标准写法应该是
C
o
v
(
n
d
2
[
k
]
)
Cov(n_d^2[k])
Cov(nd2[k])，写法做了省略：
C
o
v
(
n
d
[
k
]
,
n
d
[
k
]
)
=
D
(
n
d
[
k
]
)
=
E
(
n
d
2
[
k
]
)
−
E
(
n
d
[
k
]
)
2
=
E
(
n
d
2
[
k
]
)
Cov(n_d[k],n_d[k])=D(n_d[k])=E(n_d^2[k])-E(n_d[k])^2=E(n_d^2[k])
Cov(nd[k],nd[k])=D(nd[k])=E(nd2[k])−E(nd[k])2=E(nd2[k])(因为这里
n
d
[
k
]
n_d[k]
nd[k]是均值为0的高斯分布，所以
E
(
n
d
[
k
]
)
=
0
\bm{E(n_d[k])=0}
E(nd[k])=0，方差=平方的期望-期望的平方)，不好理解，内部展开就是下面项目的相乘
假设高斯白噪声是独立的，
n
(
τ
)
n
(
t
)
n(\tau)n(t)
n(τ)n(t)只有特定项（时间相差为1时）相乘才会有值，是狄拉克函数
δ
(
t
1
−
t
2
)
=
1
(
当且仅当
t
1
−
t
2
=
1
时
)
\delta(t_1-t_2)=1(当且仅当t_1-t_2=1时)
δ(t1−t2)=1(当且仅当t1−t2=1时)
右因为前面有：
在
t
Δ
t
t~\Delta t
t Δt时间内只有一个时刻能使狄拉克函数为1，所以内层积分为1，外层积分为
(
t
+
Δ
t
)
−
Δ
t
=
Δ
t
(t+\Delta t)-\Delta t=\Delta t
(t+Δt)−Δt=Δt ，消掉分母即得协方差
σ
2
Δ
t
\frac{\sigma^2}{\Delta t}
Δtσ2。
同理，下面的协方差写法也是做了省略，省略过程见上。
看起来像是开方的，
σ
\sigma
σ那一项相当于是服从一个
N
(
0
,
1
)
N(0,1)
N(0,1)分布。
结论：bias随即游走噪声方差从连续到离散之间需要
∗
Δ
t
*\sqrt{\Delta t}
∗Δt。（这个推导最后的开放有些看不懂，具体更详细的需要看论文
3.3.2 IMU随机误差的标定
主要是标定协方差，用于IMU选型，确定使用什么灵敏度类型的IMU
random walk noise的标定
艾伦方差的标定的论文：
斜率-0.5处，t=1时的值是高斯白噪声方差的大小，斜率0.5，t=3处值是bias random walk的方差的大小，具体为什么，需要去看论文。（功率谱，靶向量？）
数据仿真部分：
（主要是标定Acc和Gyro的bias的random walk的方差，使用Kalibr_allan）
3.4 IMU数学模型
尺度因子如果标定的话会对精度有小幅提升（MSCKF什么黎明杨做的？VINS-MONO用过）
加计数学模型：加速度计的测量数据由尺度轴偏矩阵，重力分量，高斯白噪声，bias等构成。
陀螺仪数学模型：
下面这篇论文对MSCKF进行了非常详细的建模，如果有兴趣可以看。
4. 运动模型离散时间处理：欧拉法&中值法
如何使用这些数据获得pose：
这里需要强调，重力加速度
g
w
g^w
gw前面的符号是跟坐标系定义有关的，如果是东北天，那就是-，如果是北东地就是+，整体上自洽即可。
基于导数，四元数导数的
⊗
\bm{\otimes}
⊗是四元数乘法，可以积分计算位置，速度，旋转：
欧拉法进行近似，假设在每个积分间隔内，被积函数值保持不变，即k~k+1时刻的积分使用k时刻的值：
关于四元数的更新：
q
ω
b
k
+
1
=
q
ω
b
k
+
q
′
Δ
t
=
q
⊗
[
1
0
]
+
q
⊗
[
0
1
2
ω
Δ
t
]
（
q
提出来）
=
q
⊗
[
1
1
2
ω
Δ
t
]
\begin{align} q_{\omega b_{k+1}} &= q_{\omega b_k}+q^{\prime}\Delta t \\ &=q \otimes \begin{bmatrix} 1 \\ 0 \end{bmatrix}+ q \otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega \Delta t \end{bmatrix} \\ （q提出来）&=q \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\omega \Delta t \end{bmatrix} \\ \end{align}
qωbk+1（q提出来）=qωbk+q′Δt=q⊗[10]+q⊗[021ωΔt]=q⊗[121ωΔt]
相较于欧拉法，中值法使用了k&k+1时刻的积分的均值，稍微准一点。
由于k->k+1时间较短，可以使用第k时刻的bias来矫正第k+1时刻的acc和gyro的值
5. IMU数据仿真
两种方式产生IMU仿真数据：
- 指定轨迹方程，求一阶导，二阶导得加速度等IMU仿真数据。
- 已有pose轨迹，但不知道方程，使用B-Spline产生IMU仿真数据。
由于四元数导数
所以四元数更新如上。
SO(3)更新也如上，更直观的方式是转换成欧拉角，需要将body系下的角速度转换为欧拉角速度。
粗略的理解：
旋转顺序是固定的（先绕哪个轴，再绕哪个轴）
最后转的x轴，所以x轴的角速度就是
d
ψ
d
t
\frac{d\psi}{dt}
dtdψ，
绕y轴转完之后还要绕x轴转个
ψ
\psi
ψ，所以y轴的角速度要乘一个
R
(
ψ
)
R(\psi)
R(ψ)
绕z轴转完之后还绕yx分别转了
θ
\theta
θ和
ψ
\psi
ψ，所以z方向的角速度要多乘个
R
(
ψ
)
R
(
θ
)
R(\psi)R(\theta)
R(ψ)R(θ)
所以就得到了Inertial->body系下的角速度转换矩阵，求逆就得从body->Inernial下的转换。
6. 作业
Reference
1. MSCKF详细建模推导（国外硕士论文）
2. Allan方差曲线论文
3. 离散和连续的IMU随机误差方差推导
4. 6面法标定误差（自己之前看过abstract的）
2014 ICRA：A Robust and Easy to Implement Method for IMU Calibration without
External Equipments
5. IMU数据仿真时IMU body系下的角速度转化为欧拉角速度课件
6. 利用B Spline（B样条）和已有IMU数据你拟合方程，产生IMU数据
![论文解析[11] CAT: Cross Attention in Vision Transformer](https://img-blog.csdnimg.cn/5430cf7c7ac040dda1be2310141e623a.png)
![[数据集][VOC][目标检测]河道垃圾水面漂浮物数据集目标检测可用yolo训练-1304张介绍](https://i1.hdslb.com/bfs/archive/5c12f67ab09f89d659e4f816d5cd90be83a7b6ee.jpg@100w_100h_1c.png@57w_57h_1c.png)
