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数据
1433
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
