系统误差状态的维护

线性系统的误差状态的维护

在基于滤波方法的状态估计问题中，需要实时维护系统的误差状态（也就是系统变量的协方差矩阵），本文主要对系统的误差状态进行分析。

• 对于线性系统，从k时刻到k+1时刻的状态传播转移

\[ \hat{\boldsymbol{x}}_{k+1}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k}+ \boldsymbol{B}_k \boldsymbol{u}_{k} \]

其中，\(\hat{x}_{k+1}\)是k+1时刻的传播估计，\(A_k\)是系统线性转移矩阵，\(u_k\)是系统输入，\(B_k\)是相对于输入的线性转移矩阵。

• 则系统的误差状态传播为

\[ \tilde{x}_{k+1} = A_k \tilde{x}_{k} + B_k n_k \]

其中，\(\tilde{x}_k\)是k时刻的系统状态误差，\(n_k\)是系统输入的噪声（误差）。

• 系统的状态误差的协方差矩阵传播为

\[ P_{k+1} = A_k P_k A_k^T + B_k Q_k B_k^T \]

其中，\(P_k\)是k时刻系统的误差协方差矩阵，\(Q_k\)是系统输入的噪声的协方差矩阵。

线性系统中假设传播转移估计的状态变量\(\hat{x}_k\)其实是近似满足高斯分布的，因此状态误差\(\tilde{x}_k\)就近似满足零均值高斯分布，而系统中对误差分布的表示是使用协方差矩阵（方差，因此需要计算平方均值）的形式。可以发现在第三个公式中，\(B_k\)刚好将\(m\times m\)（m为输入维度）的协方差矩阵转换为\(n \times n\)（n为系统状态维度）的协方差矩阵。

非线性系统-imu积分传播示例

和EKF的原理一致，对于非线性系统的处理就是对其进行线性展开，使用雅克比矩阵近似线性转移矩阵（因此此处仅仅是大致估计误差状态的转移，而一个系统如果没有发散，其误差一般是比较小的）。

离散时间imu状态传播

• 将姿态估计从k时刻传播到k+1时刻：

\[ { }_{G}^{I_{k+1}} \hat{\bar{q}}=\exp \left(\frac{1}{2} \boldsymbol{\Omega}\left(\boldsymbol{\omega}_{m, k}-\hat{\mathbf{b}}_{g, k}\right) \Delta t\right) {}_{G}^{I_{k}} \hat{\bar{q}} \]

• 将速度估计从k时刻传播到k+1时刻：

\[ {}^{G} \hat{v}_{I_{k+1}} = {}^{G} \hat{v}_{I_k} - {}^{G}g \Delta t + {}_{G}^{I_{k}} \hat{R}^{T} \left( a_{m,k} - \hat{b}_{a.k} \right) \Delta t \]

• 将位置估计从k时刻传播到k+1时刻：

\[ {}^{G} \hat{p}_{I_{k+1}} = {}^{G} \hat{p}_{I_k} + {}^{G} \hat{v}_{I_k} \Delta t - \frac{1}{2} {}^{G}g \Delta t^2 + \frac{1}{2} {}^{I_k}_G \hat{R}^T \left( a_{m,k} - \hat{b}_{a,k} \right) {\Delta t} ^ {2} \]

• 将bias估计从k时刻传播到k+1时刻：

\[ \hat{b}_{g,k+1} = \hat{b}_{g,k} \\ \hat{b}_{a,k+1} = \hat{b}_{a,k} \]

离散时间imu的误差状态传播

由上面可知，对于imu积分这样一个非线性系统，我们需要计算两个雅克比，一个是k+1时刻系统状态对k时刻系统状态的雅克比，一个是k+1时刻系统状态对输入噪声的雅克比。

• 首先明确需要构造的系统误差状态传播方程：

\[ \tilde{x}_{I_{k+1}} = {\Phi}_k \tilde{x}_{I_{k}} + G_k n_k \]

其中x为系统状态\(q,v,p,b_g,b_a\)，n为输入噪声\(n_{m,a},n_{m,g},n_{b,a},n_{b,g}\)（分别是测量噪声与bias的随机游走噪声）。\(\Phi\)是k+1时刻系统状态相对于k时刻系统状态的雅克比。\(G\)是k+1时刻系统状态相对与输入噪声的雅克比。（因为噪声一般很小，所以上式近似成立）

• 系统协方差矩阵的状态传播方程：

\[ P_{k+1} = {\Phi}_k P_k {\Phi}_k^T + G_k Q_d G_k^T \]

其中，\(P_k\)是k时刻系统误差状态的协方差矩阵，\(Q_d\)是系统输入噪声的协方差矩阵。使用前一般会对imu进行标定，假设测量噪声\(n_{m,a},n_{m,g}\)的标准差分别为\({\sigma}_{m,a,c}，{\sigma}_{m,g,c}\)（c表示连续信号），则离散信号的测量噪声的协方差矩阵为： \[ \mathbf{Q}_{\text {meas }}=\left[\begin{array}{cc} \frac{1}{\Delta t} \sigma_{m,g,c}^{2} \mathbf{I}_{3} && \mathbf{0}_{3} \\ \mathbf{0}_{3} && \frac{1}{\Delta t} \sigma_{m,a,c}^{2} \mathbf{I}_{3} \end{array}\right] \] 同理，离散信号的bias随机游走噪声的协方差矩阵为： \[ \mathbf{Q}_{\text {bias }}=\left[\begin{array}{cc} {\Delta t} \sigma_{b,g,c}^{2} \mathbf{I}_{3} && \mathbf{0}_{3} \\ \mathbf{0}_{3} && {\Delta t} \sigma_{b,a,c}^{2} \mathbf{I}_{3} \end{array}\right] \] 所以整个系统输入的噪声协方差矩阵为： \[ \mathbf{Q}_{\text {d}}=\left[\begin{array}{cc} \mathbf{Q}_{\text {meas}} && \mathbf{0}_{3} \\ \mathbf{0}_{3} && \mathbf{Q}_{\text {bias}} \end{array}\right] \]

• 关于雅克比矩阵的推导，可以通过上一小节离散imu状态传播的公式入手得到。（比较多推导，有时间再补充）

系统中部分误差状态传播对整体系统的影响（当状态传播仅仅与本身前一时刻相关时）

以一个vio系统为例，VIO系统维护了整个系统的协方差矩阵，其中包括了IMU状态与其他状态（比如imu与相机的外参，路标，时间偏移等）的协方差矩阵，前面的传播过程只讨论的IMU本身的协方差矩阵（其实是IMU状态的方差）如何变换，而IMU状态与其他状态变量之间的协方差矩阵如何变换没有涉及。

• 假设整个vio系统的协方差矩阵（这是一个对称矩阵）为

\[ P_{vio} = \left[\begin{array}{cc} \mathbf{P}_{\text {imu}} && \mathbf{P}_{imu}^{others} \\ \mathbf{P}_{others}^{imu} && \mathbf{P}_{\text {others}} \end{array}\right] \]

• 由上面可以知，imu积分传播之后\(\mathbf{P}_{\text {imu}}\)的更新方式为：

\[ P_{k+1}^{imu} = {\Phi}_k P_k^{imu} {\Phi}_k^T + G_k Q_d G_k^T \]

• 其中协方差部分因为\(\mathbf{P}_{others}^{imu}\)与\(\mathbf{P}_{imu}^{others}\)互为转置，所以只需要计算一次即可：

\[ P_{others}^{imu} = P_{others}^{imu} * {\Phi}^T \]

此处还有一点需要注意：此处协方差矩阵是由imu与others共同决定的，此处传播只有imu的误差状态变化了，如果others的误差状态也变化了，则还需要为\(P_{others}^{imu}\)左乘一个others相关的传播雅克比矩阵。系统中一直维护这imu与其他状态的相关性，则在测量更新时更新其他状态时，由于相关性的关系，imu的状态（主要是bias）也会被更新。

• 最后\(P_{others}\)不变。

• 小技巧：对于上述分析，我们会发现我们将要更新的状态imu放到了矩阵的头部，这样在计算协方差矩阵时十分方便，因为不需要考虑协方差是断开的。但是在实际使用中很有可能当前传播的状态变量在系统变量中是在中间。此时进行系统协方差矩阵的传播更新时就会有些麻烦。

  • 解决方法：直接取出\(\left[\begin{array}{cc} \mathbf{P}_{\text {imu}} \\ \mathbf{P}_{others}^{imu} \end{array}\right]\)，imu相关的协方差与方差部分的列，然后一起右乘以\({\Phi}^T\)，然后再取出imu相关的行协方差左乘\(\Phi\)，这样协方差部分已经完成了，方差部分仅仅只需要在叠加一个新引入的噪声\(Q=G_k Q_d G_k^T\)就可以了。
  • 注意：这样做还是需要传播的状态变量imu内部在系统中id连续。

当状态传播不仅仅与本身前一时刻相关时

上面k+1时刻的imu状态仅仅与k时刻的imu状态相关，所以\(\Phi\)维度为\(m*m\)。但是举个例子，msckf中改变路标的anchored帧时，路标新的参数除了与旧的参数相关外，还与相机外参，旧的anchor帧位姿，新的anchor帧位姿相关，此时整个传播过程原理一样，但是形式变了。

• 首先，计算\(\Phi\)矩阵变成了新的路标参数到与其传播相关的状态（旧的路标参数、相机外参、旧的anchor帧位姿、新的anchor帧位姿，假设一共n维度，记为relative）的雅克比，维度为\(m*n\)；噪声部分没有啥变化；
• 此时该路标新的方差变成(从相关的状态的方差转过来的)：

\[ P_{k+1}^{landmark} = \Phi P_k^{relative} \Phi^T + G_k Q_d G_k^T \\ 维度变换为m*m = (m*n)(n*n)(n*m) + m*m \]

• 而该路标与系统中所有其他的状态的协方差也是由相关变量与系统其他状态的协方差转来的（互为转置的仅仅计算一次）：

\[ P_{others}^{landmark} = P_{others}^{relative} * \Phi^T \\ 维度变换为N*m = (N*n)*(n*m) \]

此处为了讨论简单，假设relatives中所有变量在系统中相邻，如果不相邻的话需要对每一个相关变量的协方差依次使用\(\Phi\)进行转换然后叠加起来就好了。此处依然满足上面一节说的，方差部分可以和协方差部分一起完成\(\Phi\)的乘法，最后加上新引入噪声就可以了