参考目录^1234567

惯性导航中的一些问题

四元数的表示方法是什么？

可以这样表示：
$\dot{q}_{e}^{b}(t)=\frac{1}{2}\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right] q_{e}^{b}(t)$
在MEKF算法的差分化之后，有这样的结果：
$\boldsymbol{q}(t+\Delta t)=\boldsymbol{q}(t) \otimes e^{\frac{1}{2} \boldsymbol{q}_{\omega} \Delta t}$
这里还要参考一下VIO中预积分的做法。

科式加速度的表示

参考；

科式加速度主要用来描述参考系与惯性系之间的关系。

我们来逐项分析上面这个式子。第一项中 αα 为 {B} 的角加速度，所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度，但在惯性系 {A} 下表达——类似于 vrvr，定义相对加速度 arar。第三项比较特殊，为 {B} 的旋转运动与 P 相对 {B} 的平移运动耦合产生的加速度，称为「科氏加速度」。可以看到，除了第四项外，另外三项都和 {B} 的旋转有关。

什么时候能够用到科式加速度呢？

在MSCKF1中，考虑的是参考系（b），因此需要考虑；但在MSCKF2中，考虑的是惯性系（a），所以就不需要考虑科式加速度了。

这里存在一个问题：既然不考虑科式加速度又简便也符合逻辑，那么为什么一些文献中采用考虑科式加速度的做法呢？

在IMU中如何确定协方差矩阵？有没有一个简单的理解？或者通用的解释？

这个分析过程需要随机过程的知识，主要参考博客与博客的内容。

这里已经给出了比较合理的解释，可以根据这个关系去确定Q矩阵与R矩阵。上面的noise是白噪声，bias代表随机游走偏差，为白噪声的积分。

这里的问题是：这个部分到底是不是为了确定Q的大小？

• 在之前的一个项目里是这么做的

$Q_{d}=Q_{c} \Delta t \quad Q_{c}=\text {blkdiag}\left(\sigma_{a c c}^{2}\right)$

我觉得这个和加速度计的固有性质相关。

• 博客的内容

计算方法如下：但是这里是白噪声还是随机游走？理论上应该是白噪声。
$\frac{Q_{c}}{\Delta t} *{\Delta t}^{2}=\frac{b l k d i a g\left(\sigma_{g y r o}^{2}\right)}{\Delta t} *{\Delta t^{2}}$

• 在博客中，提到一种ESKF的姿态角度估计方法，对于Q的求解，给出下面的结果：

$\begin{array}{l} \boldsymbol{V}_{i}=\sigma_{a_{n}}^{2} \Delta t^{2} \boldsymbol{I} \\ \mathbf{\Theta}_{i}=\sigma_{\omega_{n}}^{2} \Delta t^{2} \boldsymbol{I} \\ \boldsymbol{A}_{i}=\sigma_{a_{\omega}}^{2} \Delta t \boldsymbol{I} \\ \boldsymbol{\Omega}_{i}=\sigma_{\omega_{\omega}}^{2} \Delta t \boldsymbol{I} \end{array}$

但是这里面的加速度计于陀螺的表示都是一样的，怎么解释？

为什么是这样的呢？

显然与上面的矛盾，这怎么处理呢？

如何确定协方差矩阵的更新？

要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量 之间的线性递推关系。协方差矩阵可以这样推导：这个方差的积累公式需要注意一下，实际上状态估计大多都是这么做的。
$\boldsymbol{\Sigma}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\Sigma}_{i k-1} \mathbf{F}_{k-1}^{\top}+\mathbf{G}_{k-1} \boldsymbol{\Sigma}_{\mathbf{n}} \mathbf{G}_{k-1}^{\top}$
其中，$Σ_n$ 是测量噪声的协方差矩阵，方差从 i 时刻开始进行递推，$Σ_{ii} = 0$。

但是在传统的卡尔曼滤波器的递推公式中，有下面的结果：

在这里更新的过程并没有考虑到控制量u的雅可比矩阵，这是为什么呢？

这里Q代表的过程误差方差矩阵，对应的维度是状态量。利用控制量与运动学方程，确实可以求出来状态量的误差方差矩阵。相当于状态量的方差有一部分是由控制量决定的。但是如果系统中没有控制量，只有状态量，那么就需要直接给出Q矩阵的值。实际上这种系统也是比较多的。

在一个开源项目中是这样确定Q的大小的：

sGPS     = 0.5*8.8*dt**2  # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
sCourse  = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle
sVelocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
sYaw     = 1.0*dt # assume 1.0rad/s2 as the maximum turn rate acceleration for the vehicleQ = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])

可以看到这里的Q是用来描述连续微分运动学方程中的微分量的。

这里是不是两种方法的区别？

如何将角度积分差分化表示？

在VIO中，对于IMU的处理流程是：预积分–预积分的离散形式–推导预积分的方差。

类别	方法
博主	差分化–>离散化
VIO	离散化–>差分化
	实际上就是两个部分：离散化，差分化。虽然都不是 什么复杂的数学理论，但是常常让人感到confused

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UIpYfX4W-1592795714724)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200619081139836.png)]

实际上IMU组件给系统系统的运动学方程，在状态估计中可以认为是一种约束。同时测量方程也可以认为是另外一种约束。

在优化系统中，将这两种约束都写入到优化函数中，并且保证惩罚函数是线性的，这样就可以利用梯度下降法简单求解了。

类别	方法
基于一阶泰勒展开误差递推方程：方法1	离散化–>差分化
基于误差随时间变化的递推方程：方法2	差分化–>离散化
备注	实际上就是两个部分：离散化，差分化。虽然都不是 什么复杂的数学理论，但是常常让人感到confused 另外，差分化就是将其写成误差状态的形式

但是因为方法2中需要知道状态量的连续导数关系（随时间变化的关系），很多系统都没办法求解这部分（在PVQ系统中可以做）。方法1中的离散形式比较符合对EKF的解释和EKF中协方差更新的方法，因此更加常用。

下面是详细定义：

于是有下面的结果，这里的变化很重要：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HPGtvYph-1592795714770)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200619092726511.png)]

但是最后一步是怎么推导的？

在上面的两种方法中，第一种是符合EKF的状态方程的递推关系的；第二种是目前VIO算法中的主流做法。应该是殊途同归的两种做法，但是第一种更为通用。

下面的推导是怎么完成的？
$\begin{array}{l} \dot{\delta \mathbf{v}}=\mathbf{R} \delta \mathbf{a}^{b}+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times}\left(\mathbf{a}^{b}+\delta \mathbf{a}^{b}\right)+\delta \mathbf{g} \\ \dot{\delta \mathbf{v}}=\mathbf{R} \delta \mathbf{a}^{\mathbf{b}}-\mathbf{R}\left[\mathbf{a}^{b}\right]_{\times} \delta \boldsymbol{\theta}+\delta \mathbf{g} \end{array}$

唯一的解释就是ab与delta_sita平行，叉乘为0。但是与逻辑不符合。

为什么要用误差状态量去表示呢？

这部分参考Quaternion kinematics for the error-state Kalman filte和博客。

• 定向误差状态是最小的，避免了与过度参数化相关的问题，以及相关协方差矩阵奇异性的风险，这通常是由强制约束引起的
• 误差状态系统总是在接近原始系统的情况下运行，因此远离可能的参数奇点、万向锁问题等，从而保证线性化的有效性始终保持不变
• 错误状态总是很小，这意味着所有二阶部分都可以忽略不计。这使得雅可比矩阵的计算变得非常简单和快速。有些雅可比数甚至可能是常数或等于可用状态量。
• 误差动力学是缓慢的，因为所有的大信号动力学都已集成到标称状态。这意味着我们可以以低于预测的速度应用KF修正

是不是用来描述振动变化，有着更为出色的性能？如果是的话，这也是一个可以发论文的点。

是如何将误差状态与控制量相结合的？

角速度积分表示：
$\begin{aligned} _{I}^{G} \dot{\boldsymbol{R}} &=_{I}^{G} \boldsymbol{R}\left[\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}-\boldsymbol{n}_{w}\right] \times \\ \dot{\boldsymbol{b}}_{g} &=\mathbf{0}+\boldsymbol{n}_{g} \end{aligned}$
下面是旋转矩阵的积分过程：
$\frac{\mathrm{d} \mathbf{R}}{\mathrm{d} t}=\mathbf{R}[\boldsymbol{\omega}]_{\times}$
得到离散化之后的：
$\mathbf{R}(t+\Delta t)=\mathbf{R}(t) e^{[\omega]_{\times} \Delta t}$

参考EKF-Based IMU Orientation Estimation，有下面的运动学方程：
$\begin{aligned} \delta \boldsymbol{\theta} &=\operatorname{Exp}\left[\left(\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}\right) \Delta t\right]^{T} \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g} \Delta t+\boldsymbol{\theta}_{i} \\ \delta \boldsymbol{b}_{g} &=\delta \boldsymbol{b}_{g}+\boldsymbol{\omega}_{i} \end{aligned}$
表示为：
$\left[\begin{array}{c} \delta \boldsymbol{\theta} \\ \delta \boldsymbol{b}_{g} \end{array}\right]=\underbrace{\left[\begin{array}{cc} \operatorname{Exp}\left[\left(\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}\right) \Delta t\right]^{T} & -\boldsymbol{I} \Delta t \\ \boldsymbol{0} & \boldsymbol{I} \end{array}\right]}_{\boldsymbol{F}_{\boldsymbol{x}}}\left[\begin{array}{c} \boldsymbol{\delta} \boldsymbol{\theta} \\ \delta \boldsymbol{b}_{g} \end{array}\right]+\underbrace{\left[\begin{array}{cc} \boldsymbol{I} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{I} \end{array}\right]}_{\boldsymbol{F}_{i}}\left[\begin{array}{c} \boldsymbol{\theta}_{i} \\ \boldsymbol{\omega}_{i} \end{array}\right]$

这里其实就说明了：sita与delta_sita的方差的对应关系。

四元数的运算是什么样的？

四元数相乘可以写成：
$\mathbf{q}_{1} \otimes \mathbf{q}_{2}=[\mathbf{q}]_{L} \mathbf{q}_{2}=[\mathbf{q}]_{R} \mathbf{q}$
其中
$[\mathbf{q}]_{L}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & -q_{z} & q_{y} \\ q_{y} & q_{z} & q_{w} & -q_{x} \\ q_{z} & -q_{y} & q_{x} & q_{w} \end{array}\right]=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\mathrm{T}} \\ \mathbf{q}_{v} & {\left[\mathbf{q}_{v}\right]_{\times}} \end{array}\right]$

最后一点：四元数与李代数之间的关系？

1. 首先是李代数的部分：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Jwg0MMbf-1592795714771)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200622104950178.png)]

2. 其次是罗德里格斯旋转公式：

$\mathbf{R}=\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2}$

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SdGm8KNn-1592795714775)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200622105144838.png)]

3. 描述旋转群与四元数之间的关系：参考文献1的 4.2.2

ESKF总结陈述

首先是运动学方程

The error-state Jacobian and perturbation matrices 中给出下面：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-FlqmaWN2-1592795714777)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200622084611877.png)]

其次是测量方程：

测量方程求取雅可比矩阵采用链式法则。

此外有：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-o4oWr5Mz-1592795714778)(/Users/chenshiming/Library/Application%20Support/typora-user-images/image-20200622084626045.png)]
$\left.\mathbf{X}_{\delta \mathbf{x}} \triangleq \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left[\begin{array}{ccc} \mathbf{I}_{6} & 0 & 0 \\ 0 & \mathbf{Q}_{\delta \boldsymbol{\theta}} & 0 \\ 0 & 0 & \mathbf{I}_{9} \end{array}\right]$
有下面的重要性质：

---

1. Quaternion kinematics for the error-state Kalman Filter.pdf ↩︎

2. ESKF Attitude Algorithm.pdf ↩︎

3. https://github.com/ydsf16/IMUOrientationEstimator ↩︎

4. https://github.com/yuzhou42/ESKF-Attitude-Estimation ↩︎

5. https://www.cnblogs.com/MerakXuan/p/12148013.html ↩︎

6. 开源SLAM项目：MSEKF ↩︎

7. 从头手写VIO ---- 高、贺 ↩︎