Reference card · Lesson 5
Preintegration covariance
One covariance, propagated a sample at a time, and why it decides who wins.
Independent fresh noise is added to the propagated covariance; this does not guarantee growth in every direction. The biases are conditioned on in this 9×9 model. See the right Jacobian for the meaning of Jr.
Key equations
Warm-up, from memory
$$ \mathbf{p}(t+\Delta t) = \mathbf{p}(t) + \mathbf{v}(t)\,\Delta t + \tfrac{1}{2}\mathbf{g}\,\Delta t^{2} + \tfrac{1}{2}\mathbf{R}(t)(\tilde{\mathbf{a}} - \mathbf{b}^{a})\,\Delta t^{2} $$
One sample at a time
$$ \begin{aligned}
\boldsymbol{\eta}^{\Delta}_{i,k+1} &= \mathbf{A}_k\,\boldsymbol{\eta}^{\Delta}_{ik} + \mathbf{B}_k\,\boldsymbol{\eta}^{d}_k\\
\Sigma_{i,k+1} &= \mathbf{A}_k\,\Sigma_{ik}\,\mathbf{A}_k^{\top} + \mathbf{B}_k\,\Sigma_{\eta}\,\mathbf{B}_k^{\top}, \qquad \Sigma_{ii} = \mathbf{0}
\end{aligned} \tag{63} $$
$$ \mathbf{A}_k = \begin{bmatrix}
\Delta\tilde{\mathbf{R}}_{k,k+1}^{\top} & \mathbf{0} & \mathbf{0}\\
-\Delta\tilde{\mathbf{R}}_{ik}\,\mathbf{a}_k^{\wedge}\,\Delta t & \mathbf{I} & \mathbf{0}\\
-\tfrac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\,\mathbf{a}_k^{\wedge}\,\Delta t^{2} & \mathbf{I}\,\Delta t & \mathbf{I}
\end{bmatrix}
\qquad
\mathbf{B}_k = \begin{bmatrix}
\mathbf{J}_r^{k}\,\Delta t & \mathbf{0}\\
\mathbf{0} & \Delta\tilde{\mathbf{R}}_{ik}\,\Delta t\\
\mathbf{0} & \tfrac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\,\Delta t^{2}
\end{bmatrix} $$
Practice
$$ \begin{aligned}
\Sigma_{i,k+1} &= \mathbf{A}_k\,\Sigma_{ik}\,\mathbf{A}_k^{\top} + \mathbf{B}_k\,\Sigma_{\eta}\,\mathbf{B}_k^{\top}\\
\Sigma_{ii} &= \mathbf{0}\\
\Sigma_{\eta} &= \frac{1}{\Delta t}\begin{bmatrix}\sigma_g^{2}\mathbf{I} & \mathbf{0}\\ \mathbf{0} & \sigma_a^{2}\mathbf{I}\end{bmatrix}
\end{aligned} $$
Say it at a whiteboard
"Preintegration's noise is a linear function of the IMU's white noise, so I propagate its 9×9
covariance over \([\delta\boldsymbol{\phi}, \delta\mathbf{v}, \delta\mathbf{p}]\) one sample at a time:
\(\Sigma \leftarrow \mathbf{A}\Sigma\mathbf{A}^{\top} + \mathbf{B}\Sigma_{\eta}\mathbf{B}^{\top}\), starting from
zero, with the datasheet densities turned into discrete covariances \(\sigma^{2}/\Delta t\). A and B are eq.
(31) linearised, plus one coupling: an orientation error tilts the specific force. The inverse of that
covariance weights the IMU residual. Dead reckoning generally accumulates uncertainty, but propagation does not guarantee monotonic growth in every direction."