---

1. IMU运动学方程和状态表示

1.1. 旋转状态的表达方式

对于旋转状态有两种表示方式，分别是四元数 $q$ 和旋转矩阵 $R$。可以使用这两种中的任意一种表示姿态，比如 VINS-Mono 中用的是四元数，而 FAST-LIO 中用的是旋转矩阵。它们的导数和积分如下计算：

$$\begin{array}{r}\dot{q}=q\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\omega \end{array}\right]q=q\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\end{array}$$

$$\begin{array}{r}\dot{R}=R\cdot {\omega }_{\times }R=R\cdot exp({\omega }_{\times }\delta t)\end{array}$$

1.2. IMU运动学方程

$$\begin{array}{r}\{\begin{array}{l}{\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+{\iint }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)d{t}^2\\ {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+{\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt\\ {\mathbf{q}}_{b_{k+1}}^w={\int }_{t_i\in \left[t_k,t_{k+1}\right]}{\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right)\end{array}\right]dt\\ {\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{b_a}\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{b_{\omega }}\end{array}\end{array}$$
其中，各个变量定义如下：

• ${\mathbf{a}}_m$ 和 ${\mathbf{\omega }}_m$——IMU传感器的加速度和角速度测量值读数；
• ${\mathbf{b}}_a$ 和 ${\mathbf{b}}_{\omega }$——加速度和角速度的零偏，随机游走过程，导数服从高斯分布，即${\mathbf{n}}_{{\mathbf{b}}_a}\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_{b_a}^2\right),{\mathbf{n}}_{{\mathbf{b}}_{\omega }}\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_{b_{\omega }}^2\right)$
• ${\mathbf{n}}_a$ 和 ${\mathbf{n}}_{\omega }$——加速度和角速度的测量高斯白噪声，即 ${\mathbf{n}}_a\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_a^2\right)$ 和 ${\mathbf{n}}_{\omega }\sim \left(0,{\sigma }_{\omega }^2\right)$
• ${\mathbf{R}}_{b_i}^w$ ——$t_i$ 时刻IMU的姿态，是从IMU坐标系到世界坐标系的旋转；
• ${\mathbf{p}}_k^w$ 和 ${\mathbf{v}}_k^w$——$k$ 时刻的位置和速度，在世界坐标系下表示的向量；
• ${\mathbf{g}}^w$ ——世界坐标系下表示的重力加速度矢量；
• $\mathbf{\Omega }(\mathbf{\omega })$——表示把$\mathbf{\omega }$转成四元数的右乘矩阵。

值得注意的是，虽然下标 $k$ 表示第 $k$ 个时刻的状态，$k+1$ 是第 $k+1$ 时刻的状态，但这仍然是 连续 的运动学方程。因为后面我们是用积分算的，也就是用 $k$ 到 $k+1$ 这个时间段内的每个时刻的IMU值，而不是单纯使用 $k$ 或 $k+1$ 时刻的IMU值。

1.3. 名义状态、名义IMU值 和 真实状态、真实IMU值

1.3.1. 名义状态 $\hat{\mathbf{x}}$ 和 名义IMU值 $\hat{\mathbf{a}}$ / $\hat{\mathbf{\omega }}$

名义状态：我们知道的值，我们估计的值，我们能够算出来的值。它可以是先验（IMU运动学方程上一次的积分结果），也可以是后验（Kalman Filter的上一次矫正后的结果），总之它是你能够 真正算出来的东西。

名义加速度 和 名义角速度：由于我们后面建立状态空间方程的时候选择的状态变量中包括加速度和角速度的零偏 ${\mathbf{b}}_a$ 和 ${\mathbf{b}}_{\omega }$，也就是说零偏的真值我们是不知道的，我们用于计算的只是我们估计出来的 零偏的估计值，所以计算时候用的是 $\widehat{{\mathbf{b}}_a}$ 和 $\widehat{{\mathbf{b}}_{\omega }}$。同理由于实际上每次测量的加速度和陀螺仪的噪声我们都是没有办法知道的，我们只知道噪声服从均值为0的高斯分布，所以这里就只能 把噪声简化为0。这样我们 实际知道、计算使用的名义加速度和名义角速度为：

$$\begin{array}{r}\hat{\mathbf{a}}={\mathbf{a}}_m-\widehat{{\mathbf{b}}_a}\\ \hat{\mathbf{\omega }}={\mathbf{\omega }}_m-\widehat{{\mathbf{b}}_{\omega }}\end{array}$$

然后把 $\hat{\mathbf{a}}$ 和 $\hat{\mathbf{\omega }}$ 分别 替换运动学方程中的$\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)$和$\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right)$，这样在运动学方程中 计算得到 的状态值就是我们的名义状态值$\hat{\mathbf{x}}$，比如 ${\hat{\mathbf{p}}}_k^w$、${\hat{\mathbf{v}}}_k^w$。

1.3.2. 真实状态 $\mathbf{x}$ 和 真实IMU值 $\mathbf{a}$ / $\mathbf{\omega }$

真实状态：这个是我们永远都无法知道的值。但是我们可以对它进行建模。比如上面零偏不准确，噪声不知道是多少，但是我可以对零偏误差和噪声建模，这样带入系统运动学方程中我就能 表达出来 真实状态和我的零偏误差、噪声的关系，这样可以用来 评估 我们计算出来的名义状态有多不准确。

真实加速度 和 真实角速度：真实状态就是把所有东西都考虑进去，用于对系统的真实状态进行建模。由于我们估计的IMU的零偏不准确，所以在真值状态中就把估计的误差 $\delta {\mathbf{b}}_a$ 和 $\delta {\mathbf{b}}_{\omega }$ 也加进来。然后名义状态由于不知道噪声值所以简化为了 0，这里把噪声值也建模到真实值中，所以把IMU的测量噪声 ${\mathbf{n}}_a$ 和 ${\mathbf{n}}_{\omega }$ 也考虑进来。这样就得到了 表达真实状态 使用的真实加速度和真实角速度为：

$$\begin{array}{r}\mathbf{a}={\mathbf{a}}_m-(\widehat{{\mathbf{b}}_a}+\delta {\mathbf{b}}_{\mathbf{a}})-{\mathbf{n}}_a=\hat{\mathbf{a}}-\delta {\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_a\\ \mathbf{\omega }={\mathbf{\omega }}_m-(\widehat{{\mathbf{b}}_{\omega }}+\delta {\mathbf{b}}_{\mathbf{\omega }})-{\mathbf{n}}_{\omega }=\hat{\mathbf{\omega }}-\delta {\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\omega }\end{array}$$

然后把 $\mathbf{a}$ 和 $\mathbf{\omega }$ 分别 替换 运动学方程中的 $\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)$ 和 $\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right)$，这样带入运动学方程中得到的 状态值的表达式 就是我们的真实状态值 $\mathbf{x}$，比如 ${\mathbf{p}}_k^w$、${\mathbf{v}}_k^w$。

1.4. 离散时间下的运动学方程

连续时间的运动学方程为：

$$\begin{array}{r}\{\begin{array}{l}{\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+{\iint }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)d{t}^2\\ {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+{\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt\\ {\mathbf{q}}_{b_{k+1}}^w={\int }_{t_i\in \left[t_k,t_{k+1}\right]}{\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right)\end{array}\right]dt\\ {\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{b_a}\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{b_{\omega }}\end{array}\end{array}$$

1.4.1. 欧拉积分

欧拉积分就是在 $[t_k,t_{k+1}]$ 这段时间内的IMU测量值用 $t_k$ 时刻的测量值来代替，由于加速度值要乘以姿态，所以旋转矩阵也用 $t_k$ 时刻的旋转矩阵。此外IMU测量值的噪声也用 $t_k$ 时刻的噪声代替。从而得到欧拉积分的离散运动学方程如下：

$$\begin{array}{r}\{\begin{array}{l}{\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+\frac{1}{2}\mathbf{a}\Delta t_k^2\\ {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t_k\\ {\mathbf{q}}_{b_{k+1}}^w={\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\Delta t_k\end{array}\right]\\ {\mathbf{b}}_{a_{k+1}}={\mathbf{b}}_{a_k}+{\mathbf{n}}_{b_{ak}}\Delta t_k\\ {\mathbf{b}}_{{\omega }_{k+1}}={\mathbf{b}}_{{\omega }_k}+{\mathbf{n}}_{b_{\omega k}}\Delta t_k\end{array}\end{array}$$

其中使用的加速度和角速度值分别为：

$$\begin{array}{r}\mathbf{a}={\mathbf{R}}_{b_k}^w\left({\mathbf{a}}_{m_k}-{\mathbf{b}}_{a_k}-{\mathbf{n}}_{a_k}\right)-{\mathbf{g}}^w\\ \mathbf{\omega }={\mathbf{\omega }}_{m_k}-{\mathbf{b}}_{{\omega }_k}-{\mathbf{n}}_{{\omega }_k}\end{array}$$

1.4.2. 中值积分

中值积分就是在 $[t_k,t_{k+1}]$ 这段时间内的IMU测量值用 $t_k$ 时刻和 $t_{k+1}$ 时刻的测量值平均值来代替，但是由于加速度值要乘以姿态，所以两个加速度需要分别乘以各自时刻下的旋转矩阵。此外IMU测量值的噪声也用 $t_k$ 时刻和 $t_{k+1}$ 时刻的平均值代替。从而得到中值积分的离散运动学方程如下：

$$\begin{array}{r}\{\begin{array}{l}{\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+\frac{1}{2}\mathbf{a}\Delta t_k^2\\ {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t_k\\ {\mathbf{q}}_{b_{k+1}}^w={\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\Delta t_k\end{array}\right]\\ {\mathbf{b}}_{a_{k+1}}={\mathbf{b}}_{a_k}+\frac{1}{2}({\mathbf{n}}_{b_{ak}}+{\mathbf{n}}_{b_{ak+1}})\Delta t_k\\ {\mathbf{b}}_{{\omega }_{k+1}}={\mathbf{b}}_{{\omega }_k}+\frac{1}{2}({\mathbf{n}}_{b_{\omega k}}+{\mathbf{n}}_{b_{\omega k+1}})\Delta t_k\end{array}\end{array}$$

其中使用的加速度和角速度值分别为：

$$\begin{array}{r}\mathbf{a}=\frac{1}{2}\left({\mathbf{R}}_{b_k}^w\left({\mathbf{a}}_{m_k}-{\mathbf{b}}_{a_k}-{\mathbf{n}}_{a_k}\right)+{\mathbf{R}}_{b_{k+1}}^w\left({\mathbf{a}}_{m_{k+1}}-{\mathbf{b}}_{a_{k+1}}-{\mathbf{n}}_{a_{k+1}}\right)\right){\mathbf{g}}^w\\ \mathbf{\omega }=\frac{1}{2}\left(\left({\mathbf{\omega }}_{m_k}-{\mathbf{b}}_{{\omega }_k}-{\mathbf{n}}_{{\omega }_k}\right)+\left({\mathbf{\omega }}_{m_{k+1}}-{\mathbf{b}}_{{\omega }_{k+1}}-{\mathbf{n}}_{{\omega }_{k+1}}\right)\right)\end{array}$$

1.4.3. 注意

不论是欧拉积分还是中值积分，由于IMU的零偏是随机游走，在IMU的相邻两次测量时间内不会改变，所以无论欧拉积分还是中值积分都是使用$t_k$时刻的IMU零偏。而相邻两次的测量噪声一般是不同的，所以中值积分中我们要使用两个时刻各自的噪声。

因此，后面建立离散的状态空间方程的时候，有两种状态变量的选择方式：

• 使用 欧拉积分 的离散运动学方程建立状态空间方程，状态变量和噪声变量分别是 ：
  $$\begin{array}{r}{\mathbf{x}}_{\mathbf{k}}=\left[\begin{array}{c}{\mathbf{P}}_{\mathbf{k}}\\ {\mathbf{V}}_{\mathbf{k}}\\ {\mathbf{Q}}_{\mathbf{k}}\\ {\mathbf{b}}_{{\mathbf{a}}_{\mathbf{k}}}\\ {\mathbf{b}}_{{\mathbf{\omega }}_{\mathbf{k}}}\end{array}\right]{\mathbf{w}}_{\mathbf{k}}=\left[\begin{array}{c}{\mathbf{n}}_{{\mathbf{a}}_{\mathbf{k}}}\\ {\mathbf{n}}_{{\mathbf{\omega }}_{\mathbf{k}}}\\ {\mathbf{n}}_{{\mathbf{b}}_{\mathbf{ak}}}\\ {\mathbf{n}}_{{\mathbf{b}}_{{\mathbf{\omega }}_{\mathbf{k}}}}\end{array}\right]\end{array}$$

• 使用 中值积分 的离散运动学方程建立状态空间方程，状态变量和噪声分别是：
  $$\begin{array}{r}{\mathbf{x}}_{\mathbf{k}}=\left[\begin{array}{c}{\mathbf{P}}_{\mathbf{k}}\\ {\mathbf{V}}_{\mathbf{k}}\\ {\mathbf{Q}}_{\mathbf{k}}\\ {\mathbf{b}}_{{\mathbf{a}}_{\mathbf{k}}}\\ {\mathbf{b}}_{{\mathbf{\omega }}_{\mathbf{k}}}\end{array}\right]{\mathbf{w}}_{\mathbf{k}}=\left[\begin{array}{c}{\mathbf{n}}_{{\mathbf{a}}_{\mathbf{k}}}\\ {\mathbf{n}}_{{\mathbf{\omega }}_{\mathbf{k}}}\\ {\mathbf{n}}_{{\mathbf{a}}_{\mathbf{k}+1}}\\ {\mathbf{n}}_{{\mathbf{\omega }}_{\mathbf{k}+1}}\\ {\mathbf{n}}_{{\mathbf{b}}_{\mathbf{ak}}}\\ {\mathbf{n}}_{{\mathbf{b}}_{{\mathbf{\omega }}_{\mathbf{k}}}}\end{array}\right]\end{array}$$

2. 非线性连续系统的线性化 推导 状态空间矩阵方程

2.1. 为什么要推导状态空间方程？

为什么要推导状态空间方程呢？根据运动学方程，每当有新的IMU测量值输入，我们不就可以计算出新的状态变量值了吗？原因有两个：

（1） 最重要的原因：我们后面想进行数据融合，无论是Kalman Filter还是非线性优化，我们都需要提供一个 由IMU的运动学方程得到的状态协方差矩阵（即置信度）。而协方差矩阵的传播必须是基于状态空间矩阵方程的，所以必须推导状态空间方程。如果不用计算协方差矩阵而只需要计算状态话，完全没有必要推导状态空间方程。

（2） 如果我们选择的状态变量不是运动学方程中的状态值（比如ErKF中选的是状态的误差），那么虽然此时用运动学方程也能通过 真实状态$-$名义状态 计算状态的误差，但是并不如直接使用状态空间方程进行矩阵计算来的方便了。

2.2. 状态向量中的旋转如何表示？

要想表示旋转，可以用欧拉角、角轴（旋转向量）、四元数、旋转矩阵。前面两个都有周期性，并且欧拉角还存在万向节死锁的严重问题。后面两个没有周期性，但是又存在过参数化的问题。这里对比后选择 角轴（旋转向量） 作为状态向量，原因有两个：

（1） 我们建立状态空间方程最重要的原因就是想推导状态变量的协方差矩阵，如果选择后面两个作为状态变量的话，他们一个4个参数、一个9个参数，结果协方差矩阵分别是4x4和9x9，很奇怪（实际上工业界使用过四元数作为状态向量，用卡尔曼滤波精度还可以接受）。但是选择角轴是3x3的矩阵，符合旋转3DOF的情况。所以使用角轴能够避免误差协方差矩阵 过参数化 的情况。

（2） 注意我们使用状态空间方程推导协方差矩阵，要求状态向量必须是 向量，而从数学上来说四元数和旋转矩阵都不是向量，只有角轴是向量，所以它更符合状态空间方程的要求。

2.3. 连续时间运动学方程求导建立 连续时间 状态空间方程

首先选择状态变量，状态变量就是我们想要知道的状态量，我们感兴趣的值。对IMU运动学方程来说，状态变量的选择一般都是五个：$P$, $V$, $Q$, $b_a$, $b_{\omega }$。

状态空间方程就是描述 状态变量的导数和状态变量之间的关系。因此我们要根据1中的运动学方程，将我们选择的状态变量对 时间 求导，从而得到状态的导数和状态之间的关系。并且由于我们建立状态空间方程的目的是对协方差矩阵进行传播，所以要求状态空间方程一定要是 线性 的，即它要写成 $\dot{x}=Ax+Bu$ 的形式。如果最后有状态变量分离不出来，即写不成 $Ax$ 这种形式，那么我们就必须进行线性化。

2.3.1. 先来看一个倒立摆的例子

一阶倒立摆的运动学方程如下：

$$\begin{array}{r}\left[\begin{array}{cc}M+m& ml\cos \theta \\ ml\cos \theta & I+m{l}^2\end{array}\right]\left[\begin{array}{c}\ddot{x}\\ \ddot{\theta }\end{array}\right]+\left[\begin{array}{cc}0& -ml\sin \theta \dot{\theta }\\ 0& 0\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}u\\ mgl\sin \theta \end{array}\right]\end{array}$$

可以看到最右边有一个 $sin(\theta )$ 项，这部分是没办法写成 $a\ast \theta$ 的这种线性的形式的，因此我们就要把它在某个点附近线性化，比如在 ${\theta }_0$ 的地方线性化，那么就可以写成 $sin({\theta }_0)+cos({\theta }_0)\ast (\theta -{\theta }_0)$，其中 $cos({\theta }_0)$ 是这个点的导数值，这样我们就把它化成线性的了，就能写出状态空间方程了，如下：
$$\begin{array}{r}\begin{array}{c}\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& \frac{-m^{2}g{l}^2}{I(M+m)+Mm{l}^2}& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{mgl(M+m)}{I(M+m)+Mm{l}^2}& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{I+m{l}^2}{I(M+m)+Mm{l}^2}\\ 0\\ \frac{-ml}{I(M+m)+Mm{l}^2}\end{array}\right]u\\ y=x=\left[\begin{array}{llll}1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]\end{array}\end{array}$$

2.3.2. 连续时间力学方程求导建立 连续时间 状态空间方程

连续时间的运动学方程如下，状态变量是$P$, $V$, $Q$, $b_a$, $b_{\omega }$。

$$\begin{array}{r}\{\begin{array}{l}{\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+{\iint }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)d{t}^2\\ {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+{\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt\\ {\mathbf{q}}_{b_{k+1}}^w={\int }_{t_i\in \left[t_k,t_{k+1}\right]}{\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right)\end{array}\right]dt\\ {\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{b_a}\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{b_{\omega }}\end{array}\end{array}$$

注意：求导是对 时间$t$ 求导。方程中$k$时刻是一个确定的时刻，所以它的状态值都是常量，而 $k+1$ 时刻的状态量是待求的变量。所以对时间求导的时候，右边 $p_k$ $v_k$ 都是常量，对时间的导数是0。

（1） 先看 ${\mathbf{p}}_{\mathbf{k}+1}$ 对时间求导：
$$\begin{array}{r}\frac{\mathrm{d}{\mathbf{p}}_{\mathbf{k}+1}}{\mathrm{d}t}={\mathbf{v}}_k^w+{\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt={\mathbf{v}}_{k+1}^w\end{array}$$

再次说明，这里 $k+1$ 并不代表某个确定的时刻，而是一个变量，所以这里省略下标写成 $\mathbf{p}$ 更好，代表它是一个 状态变量，它的导数就是 $\mathbf{v}$。后面同理。所以我们得到了第一个状态变量和其他五个状态变量之间的关系是 $\dot{\mathbf{p}}=\mathbf{v}$。

（2） 再看 ${\mathbf{v}}_{\mathbf{k}+1}$ 对时间求导：

$$\begin{array}{r}\frac{\mathrm{d}{\mathbf{v}}_{\mathbf{k}+1}}{\mathrm{d}t}={\mathbf{R}}_{b_i}^w\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w={\mathbf{R}}_{b_i}^w{\mathbf{a}}_m-{\mathbf{R}}_{b_i}^w{\mathbf{b}}_a-{\mathbf{R}}_{b_i}^w{\mathbf{n}}_a-{\mathbf{g}}^w\end{array}$$

可见其中 ${\mathbf{R}}_{b_i}^w{\mathbf{b}}_a和{\mathbf{R}}_{b_i}^w{\mathbf{n}}_a$ 都写成了关于状态变量 ${\mathbf{b}}_{\mathbf{a}}$ 和 ${\mathbf{n}}_{\mathbf{a}}$ 的线性项，${\mathbf{g}}^w$ 是常数项没影响。但是其中一项变量 ${\mathbf{R}}_{b_i}^w{\mathbf{a}}_m$ 却没有办法写成关于状态量 $P$, $V$, $Q$, $b_a$, $b_{\omega }$ 的形式，注意这里面的 ${\mathbf{a}}_{\mathbf{m}}$ 是IMU的测量值，我们每次都是知道的，所以这就是个系数向量。

而 ${\mathbf{R}}_{b_i}^w$ 就是 $Q$，就是系统的状态变量。但是前面我们已经讨论了，旋转部分的状态向量我们用 角轴（旋转向量） 来表示。而旋转矩阵和角轴直接的对应关系是罗德里格斯公式：
$$\begin{array}{r}\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }\end{array}$$

可见他们之间不是单纯的 $\mathbf{R}=\mathbf{K}\ast \theta$ 的关系（其中 $\mathbf{K}$ 是一个矩阵），而是包含了 $cos(\theta )$ 项，所以 旋转这里具有高度的非线性。因此旋转部分无法表示为 角轴 的线性函数，就好比现在我们得到的是 $sin(\theta )$，我没办法把他表示成 $k\ast \theta$ 这种线性的形式，自然也就没办法写出来状态空间方程了（状态变量 $\theta$分离不出来）。

所以由于旋转的非线性必须对其在 某个姿态下（即某个旋转向量下） 进行线性化，从而得到状态转移矩阵。参考深蓝学院VIO中的PPT求导公式（对旋转施加右扰动）：

$$\begin{array}{r}\begin{array}{rl}\frac{\partial (\mathbf{Rp})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\exp \left({\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\mathbf{Rp}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\left(\mathbf{I}+{\phi }^{\wedge }\right)\mathbf{p}-\mathbf{Rp}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\phi }^{\wedge }\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{\phi }}{\phi }=-\mathbf{R}{\mathbf{p}}^{\wedge }.\end{array}\end{array}$$

假设线性展开点的角轴是 ${\mathbf{\theta }}_0$，则可求得这个点的状态变量 ${\mathbf{R}}_{b_i}^w{\mathbf{a}}_m$ 对 $\mathbf{\theta }$ 的导数为 $-{\mathbf{R}}_0{\mathbf{a}}_m^{\wedge }$，其中 ${\mathbf{R}}_0$ 是旋转向量 ${\mathbf{\theta }}_0$ 对应的旋转矩阵。所以线性展开结果就是 ${\mathbf{R}}_{b_i}^w{\mathbf{a}}_m={\mathbf{R}}_0^w{\mathbf{a}}_m-{\mathbf{R}}_0^w{\mathbf{a}}_m^{\wedge }(\mathbf{\theta }-{\mathbf{\theta }}_0)$，从而表示成了关于我们的旋转状态变量 $\mathbf{\theta }$ 的线性形式。

（3） 旋转状态变量的求导更加麻烦，这里不赘述，理解上面线性化的思想即可。

（4） 关于两个零偏，运动学方程建模的时候就已经有了它们关于时间的导数。并且它们俩由于是IMU内部本身的变化，和IMU的运动状态无关，因此是 相对独立的。

2.3.3. 状态空间方程为什么写成 $\dot{x}=Fx+Ww$ 的形式？

给出VINS中关于 预积分 的连续时间状态空间方程如下，注意VINS中是关于状态变量是预积分，而不是我们上面说的PVQ。

$$\begin{array}{r}\begin{array}{rl}\left[\begin{array}{c}\delta {\dot{\mathbf{\alpha }}}_t^{b_k}\\ \delta {\dot{\mathbf{\beta }}}_t^{b_k}\\ \delta {\dot{\mathbf{\theta }}}_t^{b_k}\\ \delta {\dot{\mathbf{b}}}_{a_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]=& \left[\begin{array}{ccccc}0& \mathbf{I}& 0& 0& 0\\ 0& 0& -{\mathbf{R}}_t^{b_k}{\lfloor {\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}\rfloor }_{\times }& -{\mathbf{R}}_t^{b_k}& 0\\ 0& 0& -[{\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{w_t}\rfloor \times & 0& -\mathbf{I}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{b}}_{a_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]\\ & +\left[\begin{array}{cccc}0& 0& 0& 0\\ -{\mathbf{R}}_t^{b_k}& 0& 0& 0\\ 0& -\mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]\end{array}\end{array}$$

（1） 状态空间方程为什么写成了这种形式，即 $\dot{x}=Fx+Ww$这种形式？IMU的$a,\omega $ 不应该作为系统输入 $u$ 吗？为什么不写成 $\dot{x}=Fx+Bu+Ww$ 的形式？

这里是和系统有关的，因为系统运动学方程推导状态空间方程的结果就是IMU的输入$u$ 包含在了 $F$ 矩阵中，而不存在对关于 $u$ 的线性相加项。

（2） 另外为什么要把和噪声有关的 $Ww$ 这一项单独摘出来作为一个状态变量矩阵相乘的项，而不是把它放到 $Fx$ 中扩充成一个维度更大的 $Fx$ 矩阵呢？原因是里面的 $w$ 项，也就是noise的实际值我们始终都不知道，$Ww$ 这一项只是我们考虑到运动学方程中存在过程噪声而加入的，而实际我们计算状态变量的值的时候只能用 $Fx$ 来计算，而 把噪声项 $Ww$ 简化为0。也就是我们只能计算一个名义值，如果我们知道了噪音那么就能计算真实值了，那也就用不上卡尔曼滤波或者非线性优化了。所以后面的 $Ww$ 这一项是为了计算我状态空间方程计算结果的协方差矩阵而存在的。

（3） 这里也可以发现，明明在IMU中的测量噪声只是 $w$，但是到这里却不是 $+w$，而变成了 $+Ww$。其实这就是因为我们的运动学方程是 $\dot{x}=f(x,w)$的形式，而不是 $\dot{x}=f(x)+w$ 的形式。也就是噪声$w$被传入我们的非线性的运动学模型中之后，它被放大了。而由于 $f$ 是非线性的，放大出来的噪声不再服从高斯分布了，这样就没办法再利用状态空间方程计算状态量的协方差矩阵了，所以我们才对状态空间方程进行了线性化，而得到的矩阵 $W$ 恰恰就是我们线性化的模型对噪声的放大倍数。（所以这里也解释了B站DR_CAN讲的 EKF 中为什么把 $w$ 放在了 $f$ 里面而不是外面，因为过程噪声不知道是在哪里被加入的，所以必须放在 $f$ 里面，这样才具有一般性）

2.3.4. 连续时间运动学方程求导建立 连续时间 误差状态空间方程

上面我们推导的是直接基于 状态 的 状态空间方程，但是在IMU的状态空间方程中，更常用的是一种基于 误差状态 (Error-State) 的误差状态空间方程。

（1） 先看多元微分学中的 全增量 的概念：多元函数 $z=f(x,y)$ 在点 $P(x,y)$ 的某邻域内有定义，$P^{\prime }(x+\Delta x,y+\Delta y)$ 是该邻域内任意一点，则称 $\Delta z=f(x+\Delta x,y+\Delta y)-f(x,y)$ 为函数在点 $P(x,y)$ 处的 全增量，并且满足

$$\begin{array}{r}\Delta z=f(x+\Delta x,y+\Delta y)-f(x,y)=\frac{\partial z}{\partial x}\Delta x+\frac{\partial z}{\partial y}\Delta y+o(\rho )\end{array}$$

其实全增量进一步扩展，即当自变量的增量足够小的时候，就变成了 全微分，即

$$\begin{array}{r}dz=\frac{\partial z}{\partial x}dx+\frac{\partial z}{\partial y}dy\end{array}$$

观察上面的全增量公式，可以发现：把每个自变量都给一个微小的增量，然后求出由于所有自变量都变化导致的函数的全增量。而全增量中每一项前面的系数，恰恰就是 原来的函数对这个变量的偏导。所以当我们想求多个变量的每一个偏导的时候，可以使用全增量公式来求。

（2） 现在把上面我们对连续时间求导得到的连续时间的 状态关于状态的变化方程（旋转矩阵可以用罗德里格斯公式写成关于旋转向量的非线性函数）写成一个抽象的非线性函数 $f$，其中 $\mathbf{x}$ 是状态变量，$\mathbf{w}$ 是过程噪声，也就是IMU传感器的测量噪声。

$$\begin{array}{r}\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{w})\end{array}$$

由于这个函数是一个非线性的函数，根据上面的讨论，我们想把它展开成关于状态变量 $\mathbf{x}$ 的线性函数。假设我们把 $f$ 在 $\mathbf{x}$ 处展开，如下所示：

$$\begin{array}{r}\dot{\mathbf{x}}+\dot{\mathbf{\delta x}}=f(\mathbf{x}+\delta \mathbf{x},\mathbf{w}+\delta \mathbf{\omega })\approx f(\mathbf{x},\mathbf{w})+\frac{\partial f}{\partial \mathbf{x}}\mathbf{\delta x}+\frac{\partial f}{\partial \mathbf{w}}\mathbf{\delta w}\end{array}$$

其中，由于我们的等号左侧实际上是 $\dot{\mathbf{x}}$，所以当自变量 $\mathbf{x}$ 变成是 $\mathbf{x}+\delta \mathbf{x}$ 之后，左边的函数值自然也就变成了

$$\begin{array}{r}\frac{\mathrm{d}(\mathbf{x}+\mathbf{\delta x})}{\mathrm{d}t}=\dot{\mathbf{x}}+\dot{\mathbf{\delta x}}\end{array}$$

而 $\approx$ 右边的是泰勒展开。由于 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{w})$，所以上面的公式化为：

$$\begin{array}{r}\dot{\mathbf{\delta x}}=\frac{\partial f}{\partial \mathbf{x}}\mathbf{\delta x}+\frac{\partial f}{\partial \mathbf{w}}\mathbf{\delta w}\end{array}$$

可见这就是上面说的 全增量公式，只不过由于我们的函数左侧是 $\dot{\mathbf{x}}$ 的形式看起来有点别扭，其实实际上是一样的。

（3） 回到误差状态空间方程的推导上

现在我们想推导误差状态空间方程，先看几种状态的定义和关系：

• 名义状态 ： $\hat{\mathbf{x}}$
• 真实状态 ： $\mathbf{x}$
• 误差状态 ：$\delta \mathbf{x}$

关系 ： $\mathbf{x}=\hat{\mathbf{x}}+\delta \mathbf{x}$， 即 真实状态 $=$ 名义状态 $+$ 误差状态。

在线性化上面的非线性状态方程的时候，我们只能在名义状态的地方进行线性化，因为名义状态是我们 唯一可以真实计算得到的状态，是我们可以真正操作的状态。而真实状态我们永远不知道，它只能对误差和噪声建模，然后带入公式中来表示我们的真实状态应该是多少，从而评估我们计算的名义状态误差有多大。所以利用上面的公式（28），在 名义状态 的地方展开，可以得到：

$$\dot{\hat{\mathbf{x}}}+\dot{\mathbf{\delta x}}=f(\hat{\mathbf{x}}+\delta \mathbf{x},\mathbf{w}+\delta \mathbf{\omega })\approx f(\hat{\mathbf{x}},\mathbf{\omega })+{\frac{\partial f}{\partial \mathbf{x}}|}_{\mathbf{x}=\hat{\mathbf{x}}}\mathbf{\delta x}+\frac{\partial f}{\partial \mathbf{w}}\mathbf{\delta w}$$

然后再由公式（30）就可以得到：

$$\begin{array}{r}\dot{\mathbf{\delta x}}={\frac{\partial f}{\partial \mathbf{x}}|}_{\mathbf{x}=\hat{\mathbf{x}}}\mathbf{\delta x}+\frac{\partial f}{\partial \mathbf{w}}\mathbf{\delta w}\end{array}$$

其实如果是推导基于状态变量的状态空间方程的话，我们直接按照 （2） 中在某个我们知道的状态点处展开就行了，而用不到上面的全增量公式。但是有一个需要注意的点是我们在计算名义状态值 $f(\hat{\mathbf{x}},\mathbf{\omega })$ 的时候噪声 $\mathbf{\omega }$ 是 0，因为名义状态下并不知道噪声，我们只能简化为0。

注意：我们这里是对函数 $f$ (这个函数 $f$ 是 运动学方程对时间求导得到的函数) 求它关于 各个状态变量 的偏导，而不是对 系统的运动学方程 求它关于各个状态变量的偏导。但是一个看深蓝学院VIO的推导里，他们都是直接就在对 离散运动学方程 上求了偏导，这是为什么呢？其实这里是两个求导的自变量不一样的原因，因为 运动学方程是对时间求全导，公式（31）是对 状态变量求偏导，两个求导并不影响。因此可见当我使用公式（31）对状态变量求偏导的时候，函数$f$如果再对时间进行一次积分或二次积分，就相当于我 在运动学方程上直接对状态变量求偏导，实际上最后的结果就相当于差了一个和时间 $t$ 有关的倍数。而这个和时间有关的倍数就是因为运动学方程是随着时间变化的，我们求得的 $\dot{\mathbf{x}}$ 表示了状态随时间变化的关系，所以下一个时刻的状态肯定是和经过的时间有关的，这就是为什么在运动学方程上直接对状态变量求导多了一个和时间 $t$ 有关的倍数。实际上直接在运动学方程上对状态变量求导的结果相当于省去了 状态根据状态空间方程随时间变化 的这个过程，直接得到了 随时间变化的状态 关于 状态变量 的偏导。

2.3.5. 状态空间方程和误差状态空间方程的协方差矩阵一致吗？

在 VINS 中，IMU使用的是误差状态空间方程，然后在非线性优化中直接利用误差的协方差矩阵作为IMU预积分值（状态变量）的协方差矩阵。为什么可以用 误差状态的协方差矩阵 代替 状态的协方差矩阵 呢？

TODO: 公式标号有问题：

实际上还是可以从公式（24）和（25）中看出来。协方差矩阵是否一样关键就是他们的状态空间方程的系数矩阵是否一样，对比公式（24）和（25）可以看到，无论是状态空间方程还是误差状态空间方程，他们的系数矩阵 $F$ 和 $W$ 都是 函数$f$对状态变量求偏导，在名义状态处取值 的结果。也就是状态空间方程和误差状态空间方程的 系数矩阵是完全一致的，因此协方差矩阵也是一致的。

2.4. 连续状态空间矩阵方程的离散化

上面我们做的是对连续的运动学方程进行线性化得到连续的状态空间方程，但是实际上我们的系统是离散的，不论IMU测量值还是我们进行计算的值都是离散的。所以还需要对上面的连续时间的状态空间方程进行离散化，这里根据系统运动学离散积分时选择方法的不同，连续状态空间方程进行离散化的方法也不同。

2.4.1. 欧拉积分的离散化（对应欧拉积分的运动学方程）

这里我们都是用误差状态空间方程来分析，因为IMU中用的基本都是误差状态空间方程。

假设前面我们推导得到的连续的误差状态空间方程为：

$$\begin{array}{r}\dot{\delta x}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}{\mathbf{n}}_{k-1}\end{array}$$

则离散的误差状态空间方程为：

$$\begin{array}{r}\begin{array}{rl}\delta {\mathbf{x}}_k& =\delta {\mathbf{x}}_{k-1}+\delta {\mathbf{x}}_{k-1}\Delta t\\ \to \delta {\mathbf{x}}_k& =(\mathbf{I}+\mathbf{A}\Delta t)\delta {\mathbf{x}}_{k-1}+\mathbf{B}\Delta t{\mathbf{n}}_{k-1}\end{array}\end{array}$$

因为我们用的是欧拉积分，所以 $\delta \mathbf{x}$ 直接取上一个时刻的值即可，测量值和噪声也都是只取上一个时刻的。

2.4.2. 中值积分的离散化（对应中值积分的运动学方程）：

这个要复杂一些，可以把误差状态空间方程矩阵拆开来看，分解成五个方程来计算。因为实际推导就是把五个方程从矩阵里分离出来，然后对每个方程进行推导的。

（1） 以预积分方程中关于 $\mathbf{\theta }$ 的连续时间方程来看：

$$\begin{array}{r}\delta {\dot{\mathbf{\theta }}}_t^{b_k}=-{[{\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{w_t}\rfloor }_{\times }\delta {\mathbf{\theta }}_t^{b_k}-\delta {\mathbf{b}}_{w_t}-{\mathbf{n}}_w\end{array}$$

对于中值积分对应的离散状态空间方程来说，所有的 状态量值和测量值 都需要用相邻 两个时刻 的值来表示。而IMU的零偏和噪声对状态量没有依赖关系，所以它们不用考虑对状态变量的以来关系。但是状态变量PVQ中依赖关系是 P→V→Q，所以在更新状态变量的时候的顺序是 Q→V→P。比如上面这个关于旋转的连续时间方程，其中的角速度值就要用前后两个时刻的，角速度噪声要用前后两个时刻的。这样很轻松就可以推得下一个时刻的旋转向量。

（2） 而速度的连续时间方程如下：

$$\begin{array}{r}\delta \dot{\beta }=-R_t{\left({\hat{a}}_t-b_{a_t}\right)}^{\wedge }\delta \theta -R_t\delta b_{a_t}-R_t{n}_a\end{array}$$

可以看出来里面和状态有关的是姿态，所以这个姿态就要使用我们上一次的姿态 $R_k$ 和这一次刚刚算出来的姿态 $R_{k+1}$ 来计算。但是这里还不是简单的平均关系，因为 $R$ 是作用在加速度上的，所以实际上我们应该把不同时刻的姿态乘以不同时刻的加速度，然后求平均作为加速度的平均值。位置P的计算类推，不再赘述。

所以这里能看出来，推导这个离散形式的 $F$ 和 $G$ 矩阵的时候，如果我们想写出来公式，实际上是很麻烦的，因为要按照 Q→V→P 的顺序依次迭代公式，最后才能得到P和V完整的离散状态空间方程的系数矩阵。但是这个在编程实现中却不困难，因为我们可以定义一个变量存储Q的系数矩阵，在V中计算的时候直接带入这个矩阵，然后计算得到V的系数矩阵，再存储这个系数矩阵，在P中计算的时候带入Q和V的系数矩阵。

3. 非线性离散系统的线性化 推导 状态空间矩阵方程

3.1. 离散状态空间方程的另一种推导方法（以R2Live为例）

上一章中的的方法是对连续的运动学方程先求导，得到连续的状态空间方程，然后再把连续的状态空间方程进行离散化。这样其中多了一个步骤，就是求连续的状态空间方程这里。从公式推导的角度来看，这样很麻烦，因为多了一个求连续状态空间方程的过程，而且离散化的时候如果用中值积分离散的话，存在Q→V→P顺序的系数矩阵的带入，最后推导公式结果会非常麻烦。但是这个方法的好处是在编程实现的时候非常容易，甚至编程实现的时候只需要推导得到连续的状态空间方程即可，离散的状态空间用系数矩阵带入即可。

现在用一种 方便公式推导 的方法：先把运动学方程离散化，然后直接对离散的运动学方程推导离散的状态空间方程。这个方法在公式推导的时候很好用，在 R2Live 的论文中优势表现非常明显，因为整个推导过程思路很清晰。这里的推导方便用的两个技巧是：

（1） 使用 全增量 的方式，一次把某一个状态对所有状态的偏导都求出来。将离散运动学方程在名义值处的 计算公式 值作为名义状态值，在真实状态处的 计算公式 值作为真实状态值，二者相减等号左侧就是误差状态值，等号右边就是 带入公式 得到的误差状态值的具体 表达式。如下所示：

$$\begin{array}{r}\begin{array}{rl}\delta {\hat{\mathbf{x}}}_{i+1}& ={\mathbf{x}}_{i+1}\boxminus {\hat{\mathbf{x}}}_{i+1}\\ & =\left({\mathbf{x}}_i⊞\left(\Delta t\cdot \mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)\right)\boxminus \left({\hat{\mathbf{x}}}_i⊞\left(\Delta t\cdot \mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right)\right)\\ & \approx {\mathbf{F}}_{\delta \hat{\mathbf{x}}}\delta \hat{\mathbf{x}}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_i\\ & \sim \mathcal{N}\left(0_{21\times 1},{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_{i+1}}\right)\end{array}\end{array}$$

其中：

${\mathbf{x}}_{i+1}$ 是下一刻真实状态，它的计算方法是带入上一刻的 真实状态 ${\mathbf{x}}_i={\hat{\mathbf{x}}}_i⊞\delta \hat{\mathbf{x}}$、加速度测量的真实值 ${\mathbf{a}}_i={\hat{\mathbf{a}}}_i-\delta {\hat{\mathbf{b}}}_{{\mathbf{a}}_i}-{\mathbf{n}}_{{\mathbf{a}}_i}$ 和 角速度测量的真实值 ${\mathbf{\omega }}_i={\hat{\mathbf{\omega }}}_i-\delta {\hat{\mathbf{b}}}_{{\mathbf{g}}_i}-{\mathbf{n}}_{{\mathbf{g}}_i}$ 进行计算。

$\widehat{{\mathbf{x}}_{i+1}}$ 是下一刻名义状态，它的计算方法是带入 上一刻的名义状态 ${\hat{\mathbf{x}}}_i$、加速度测量的名义值 ${\hat{\mathbf{a}}}_i={\mathbf{a}}_{m_i}-{\hat{\mathbf{b}}}_{{\mathbf{a}}_i}$ 和 角速度测量的名义值 ${\hat{\mathbf{\omega }}}_i={\mathbf{\omega }}_{m_i}-{\hat{\mathbf{b}}}_{{\mathbf{g}}_i}$ 进行计算。

比如其中最复杂的 旋转状态误差 的推导如下：

（2） 姿态采用旋转矩阵表示，然后旋转矩阵求导使用李群和李代数，相比四元数更加方便。求真实姿态和名义姿态之间的误差的时候，就是名义姿态矩阵乘以真实姿态矩阵的逆，然后结果再取对数在李代数上应该接近零向量，也就是旋转向量的误差，非常方便。但是这其中用到了几个李群李代数的公式，比如右雅克比、BCH近似、旋转矩阵的伴随性质等等，这些公式在视觉SLAM14讲中都有，所以仔细看并不难。

3.2. R2Live论文公式的疑惑解答

如下公式，最后写的是 $F_{\delta x}$ 和 $F_w$ 都是误差的导数在误差为0时候的值，这个看着不太好明白。

$$\begin{array}{r}\begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{i+1}={\mathbf{x}}_{i+1}\square {\hat{\mathbf{x}}}_{i+1}\\ & =\left({\mathbf{x}}_i⊞\left(\Delta t\cdot \mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)\right)\square \left({\hat{\mathbf{x}}}_i\boxminus \left(\Delta t\cdot \mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right)\right)\\ & \approx {\mathbf{F}}_{\delta \hat{\mathbf{x}}}\delta \hat{\mathbf{x}}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_i\\ & \sim \mathcal{N}\left(0_{21\times 1},{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_{i+1}}\right)\\ & \text{ where: }\\ & {\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_{i+1}}={\mathbf{F}}_{\delta \hat{\mathbf{x}}}{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_i}{\mathbf{F}}_{\delta \hat{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T\\ & {\mathbf{F}}_{\delta \hat{\mathbf{x}}}={\frac{\partial \left(\delta {\hat{\mathbf{x}}}_{i+1}\right)}{\partial \delta {\hat{\mathbf{x}}}_i}|}_{\delta {\hat{\mathbf{x}}}_i=0,{\mathbf{w}}_i=0},{\mathbf{F}}_{\mathbf{w}}={\frac{\partial \left(\delta {\hat{\mathbf{x}}}_{i+1}\right)}{\partial {\mathbf{w}}_i}|}_{\delta {\hat{\mathbf{x}}}_i=0,{\mathbf{w}}_i=0}\end{array}\end{array}$$

其实看之前我们推导的公式：

$$\begin{array}{r}\dot{\mathbf{\delta x}}={\frac{\partial f}{\partial \mathbf{x}}|}_{\mathbf{x}=\hat{\mathbf{x}}}\mathbf{\delta x}+\frac{\partial f}{\partial \mathbf{w}}\mathbf{\delta w}\end{array}$$

实际上这个误差方程关于 $\delta x$ 是线性的，所以 $\frac{\partial \left(\delta {\hat{\mathbf{x}}}_{i+1}\right)}{\partial \delta {\hat{\mathbf{x}}}_i}$ 的值与 $\delta x$ 没有关系，它一直都是原来的状态空间方程在名义值处的导数。只不过写成这里的形式是说我们只有名义值，而且每次我们给出的名义值都是我们能够给出的最精确的值，所以是无偏的，也就是每次的 $\delta x$ 都是 0。

4. IMU 积分的误差状态空间方程推导

参见我的另一篇博客：IMU 积分的误差状态空间方程推导

---