# Marginal Covariance of Parameters in New Observations

Jianzhu Huai1 1Jianzhu Huai was with CEGE, The Ohio State University,
Columbus, OH

###### Abstract

We have observed a common problem of solving for the marginal covariance of
parameters introduced in new observations. This problem arises in several
situations, including augmenting parameters to a Kalman filter, and computing
weight for relative pose constraints. To handle this problem, we derive a
solution in a least squares sense. The solution is applied to the above two
instance situations and verified by independently reported results.

##  I Problem and Assumptions

The problem is to estimate the covariance or information matrix of parameters
$\mathbf{x}_{n}$ which is involved in new observations $\mathbf{z}$ of
existing parameters $\mathbf{x}_{m}$ with prior knowledge of uncertainty. The
subscript m denotes the dimension of the main parameters, and the subscript n
denotes the dimension of the new parameters.

The observations as a vector $\mathbf{z}$ are related to parameters
$\mathbf{x}=[\mathbf{x}_{m}^{\intercal}\enskip\mathbf{x}_{n}^{\intercal}]$ by
observation coefficient matrix,
$\mathbf{H}=[\mathbf{H}_{m}\enskip\mathbf{H}_{n}]$, i.e.,

$\begin{split}\mathbf{z}&=\mathbf{H}\mathbf{x}+\mathbf{w}\\\
&=\begin{bmatrix}\mathbf{H}_{m}&\mathbf{H}_{n}\end{bmatrix}\begin{bmatrix}\mathbf{x}_{m}\\\
\mathbf{x}_{n}\end{bmatrix}+\mathbf{w}\end{split}$ (1)

The additive noise affecting observations, $\mathbf{w}$, is assumed to be
Gaussian white noise, $\mathbf{w}\sim N(\mathbf{0},\mathbf{R})$ where
$\mathbf{R}$ is an diagonal matrix. The coefficient matrices $\mathbf{H}_{m}$
and $\mathbf{H}_{n}$ could be constant or contingent on $\mathbf{x}_{m}$ or
$\mathbf{x}_{n}$.

The prior knowledge about $\mathbf{x}_{n}$ is assumed to be captured by a
Gaussian distribution with mean $\hat{\mathbf{x}}_{m}$ and covariance
$\mathbf{P}$. That is, formally,

$\begin{split}\mathbf{y}&=\begin{bmatrix}\mathbf{I}_{m}&0_{m\times
n}\end{bmatrix}\begin{bmatrix}\mathbf{x}_{m}\\\
\mathbf{x}_{n}\end{bmatrix}+\mathbf{v}\\\
&=\mathbf{F}\mathbf{x}+\mathbf{v}\end{split}$ (2)

where $\mathbf{v}$ is Gaussian white noise $\mathbf{v}\sim
N(\mathbf{0},\mathbf{P})$ and $\mathbf{F}$ is shorthand for the coefficient
matrix. $\mathbf{y}$ is the pseudo observation of $\mathbf{x}_{m}$, and its
realization takes the value of $\hat{\mathbf{x}}_{m}$.

##  II Solution

Covariance for $\mathbf{x}_{n}$ can be derived following the standard least
squares approach. The below derivation is essentially the same as that for
deriving the Kalman filter update [1].

The least squares estimation for
$\mathbf{x}=[\mathbf{x}_{m}^{\intercal}\enskip\mathbf{x}_{n}^{\intercal}]^{\intercal}$
is given by minimizing the cost function $E$,

$E=(\mathbf{y}-\mathbf{F}\mathbf{x})^{\intercal}\mathbf{P}^{-1}(\mathbf{y}-\mathbf{F}\mathbf{x})+(\mathbf{z}-\mathbf{H}\mathbf{x})^{\intercal}\mathbf{R}^{-1}(\mathbf{y}-\mathbf{H}\mathbf{x}).$
(3)

According to the least squares theory [2, (3.13)], the information for the
state $\mathbf{x}$ is given by the Hessian matrix $\boldsymbol{\Lambda}$,

$\begin{split}\boldsymbol{\Lambda}&=\begin{bmatrix}\mathbf{F}^{\intercal}&\mathbf{H}^{\intercal}\end{bmatrix}\begin{bmatrix}\mathbf{P}&\mathbf{0}\\\
0&\mathbf{R}\end{bmatrix}^{-1}\begin{bmatrix}\mathbf{F}\\\
\mathbf{H}\end{bmatrix}\\\
&=\begin{bmatrix}\mathbf{P}^{-1}+\mathbf{H}_{m}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{m}&\mathbf{H}_{m}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{n}\\\
\mathbf{H}_{n}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{m}&\mathbf{H}_{n}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{n}\end{bmatrix}\end{split}$
(4)

which is inverse of the covariance of state estimate $\hat{\mathbf{x}}$,
$\texttt{cov}(\mathbf{x})$. To simplify notations, let’s denote the blocks of
$\boldsymbol{\Lambda}$ as

$\boldsymbol{\Lambda}=\begin{bmatrix}\mathbf{A}&\mathbf{B}\\\
\mathbf{C}&\mathbf{D}\end{bmatrix}.$ (5)

To obtain the marginal covariance for the new parameters $\mathbf{x}_{n}$, and
their correlation to the main parameters $\mathbf{x}_{m}$, we can use the
block matrix inversion formulae [3, (2.2), (2.3)] which obviously require
nonsingular conditions.

A useful result about marginal covariance is obtained when both $\mathbf{D}$
and its Schur complement $\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C}$ are
invertible (i.e., nonsingular). The entire covariance for
$[\mathbf{x}_{m}^{\intercal}\enskip\mathbf{x}_{n}^{\intercal}]^{\intercal}$,
$\texttt{cov}(\mathbf{x})$ is given by

$\left[\begin{array}[]{c|c}(\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}&-(\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\mathbf{B}\mathbf{D}^{-1}\\\
\hline\cr\begin{split}-\mathbf{D}^{-1}\mathbf{C}\cdot\\\
(\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\end{split}&\begin{split}\mathbf{D}^{-1}+\mathbf{D}^{-1}\mathbf{C}\cdot\\\
(\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\mathbf{B}\mathbf{D}^{-1}\end{split}\end{array}\right]$
(6)

whose bottom right block is the marginal covariance for $\mathbf{x}_{n}$.

In case that $\boldsymbol{\Lambda}$ is singular, it is practically less useful
to obtain its inverse. Loosely speaking, this situation occurs because the new
observations $\mathbf{z}$ do not fully constrain the new parameters
$\mathbf{x}_{n}$. In this case, it is often more appropriate and efficient to
use the Cholesky factor of $\boldsymbol{\Lambda}$ (see III-B).

The above results are obtained for linear observation models (1), but they can
be readily extended to nonlinear observation models. In this case, the
observation model is linearized at current state estimate
$\hat{\mathbf{x}}=[\hat{\mathbf{x}}_{m}^{\intercal}\enskip\hat{\mathbf{x}}_{n}^{\intercal}]^{\intercal}$,
and the parameters in (1) and (2) are the perturbations in $\mathbf{x}$,

$\delta\mathbf{x}=\begin{bmatrix}\delta\mathbf{x}_{m}\\\
\delta\mathbf{x}_{n}\end{bmatrix}.$ (7)

These perturbations can be defined by a generalized $+$ operator, the
$\boxplus$-operator [4], in left- or right-multiplication manner.

##  III Applications

Several problems arising from filtering-based visual inertial odometry can be
solved with the above results (4) and (6). The following examines two
problems: initializing covariance of landmark parameters in a EKF-SLAM method,
and weighting relative pose constraints in pose graph optimization.

###  III-A Landmark Covariance Initialization

For a point-based EKF-SLAM method (e.g., [5]) that includes landmark
parameters in the state vector, the covariance for landmark parameters and
their correlation to the existing state need to be properly initialized as
well observed landmarks are added to the state.

This initialization has been studied in [5]. To derive expressions for
relevant covariance blocks, the authors assigned infinite covariance for
landmark parameters in the first place, and then performed the Kalman filter
update, and finally took the limits. In contrast, our approach is less
involved and more general while arriving at exactly the same results.

Assuming that a landmark is well observed (i.e., the required inverses exist),
the equivalency can be shown by multiplying the observation equation (1) with
the $\mathbf{Q}$ matrix of QR decomposition of the observation matrix block
$\mathbf{H}_{n}$ and then applying the result (6) to the transformed
observation equation.

We begin with writing out the linearized observation system for system error
state $\delta\mathbf{x}$ and the error state $\delta\mathbf{f}$ of one
landmark,

$\mathbf{\delta
z}=\begin{bmatrix}\mathbf{H}_{x}&\mathbf{H}_{f}\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}\\\
\delta\mathbf{f}\end{bmatrix}+\mathbf{w}$ (8)

where $\delta\mathbf{z}$ is the observation residual
$\delta\mathbf{z}=\mathbf{z}-h(\mathbf{x})$ of dimension $k$, and the
Jacobians of $h(\cdot)$ relative to $\delta\mathbf{x}$ and $\delta\mathbf{f}$
are $\mathbf{H}_{x}$ of size $k\times m$ and $\mathbf{H}_{f}$ of size $k\times
3$, respectively. The linearization requires an estimate of landmark
parameters which can be obtained by DLT [6] or Gauss-Newton optimization [7].

The multiplication step separates the observations into two subsystems, one
independent of $\mathbf{f}$, and the other dependent on $\mathbf{f}$, thanks
to the QR decomposition of $\mathbf{H}_{f}$,

$\mathbf{H}_{f}=\mathbf{QR}=\begin{bmatrix}\mathbf{Q}_{c}&\mathbf{Q}_{o}\end{bmatrix}\begin{bmatrix}\mathbf{R}_{c}\\\
\mathbf{0}\end{bmatrix}$ (9)

where $\mathbf{Q}$ is an orthogonal matrix consisting of the column space
basis $\mathbf{Q}_{c}$ of size $k\times 3$ and the left nullspace basis
$\mathbf{Q}_{o}$ of size $k\times(k-3)$, and $\mathbf{R}_{c}$ is a $3\times 3$
upper triangular matrix. Here we employs the assumption that the landmark is
well observed. Otherwise, $\mathbf{Q}_{c}$ will have less than 3 columns.

By left multiplying $\mathbf{Q}^{\intercal}$ to the observation system in (8),
the two subsystems become obvious as below,

$\begin{split}\begin{bmatrix}\mathbf{Q}_{c}^{\intercal}\delta\mathbf{z}\\\
\mathbf{Q}_{o}^{\intercal}\delta\mathbf{z}\end{bmatrix}&=\begin{bmatrix}\mathbf{Q}_{c}^{\intercal}\mathbf{H}_{x}&\mathbf{Q}_{c}^{\intercal}\mathbf{H}_{f}\\\
\mathbf{Q}_{o}^{\intercal}\mathbf{H}_{x}&\mathbf{Q}_{o}^{\intercal}\mathbf{H}_{f}\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}\\\
\delta\mathbf{f}\end{bmatrix}+\begin{bmatrix}\mathbf{Q}_{c}^{\intercal}\mathbf{w}\\\
\mathbf{Q}_{o}^{\intercal}\mathbf{w}\end{bmatrix}\\\
\begin{bmatrix}\delta\mathbf{z}_{c}\\\ \mathbf{\delta
z}_{o}\end{bmatrix}&=\begin{bmatrix}\mathbf{H}_{cx}&\mathbf{H}_{cf}\\\
\mathbf{H}_{o}&0\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}\\\
\delta\mathbf{f}\end{bmatrix}+\begin{bmatrix}\mathbf{w}_{c}\\\
\mathbf{w}_{o}\end{bmatrix}\\\ \mathbf{w}_{c}&\sim
N(\mathbf{0},\mathbf{R}_{c})\\\ \mathbf{w}_{o}&\sim
N(\mathbf{0},\mathbf{R}_{o})\end{split}$ (10)

where the covariance for separated noise $\mathbf{w}_{c}$ and $\mathbf{w}_{o}$
are denoted by diagonal matrices $\mathbf{R}_{c}$ of size $3\times 3$ and
$\mathbf{R}_{o}$ of size $(k-3)\times(k-3)$, and the projected Jacobians are
$\mathbf{H}_{cx}$ of size $3\times m$, $\mathbf{H}_{cf}$ of size $3\times 3$,
and $\mathbf{H}_{o}$ of size $(k-3)\times m$.

The information matrix for the system can be written out according to (4) by
plugging in the corresponding terms,

$\begin{split}\mathbf{A}&=\mathbf{P}^{-1}+\mathbf{H}_{m}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{m}\\\
&=\mathbf{P}^{-1}+\begin{bmatrix}\mathbf{H}_{cx}^{\intercal}&\mathbf{H}_{o}^{\intercal}\end{bmatrix}\begin{bmatrix}\mathbf{R}_{c}^{-1}&\mathbf{0}\\\
\mathbf{0}&\mathbf{R}_{o}^{-1}\end{bmatrix}\begin{bmatrix}\mathbf{H}_{cx}\\\
\mathbf{H}_{o}\end{bmatrix}\\\
&=\mathbf{P}^{-1}+\mathbf{H}_{cx}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cx}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o}\\\
\mathbf{B}&=\mathbf{H}_{m}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{n}\\\
&=\begin{bmatrix}\mathbf{H}_{cx}^{\intercal}&\mathbf{H}_{o}^{\intercal}\end{bmatrix}\begin{bmatrix}\mathbf{R}_{c}^{-1}&\mathbf{0}\\\
\mathbf{0}&\mathbf{R}_{o}^{-1}\end{bmatrix}\begin{bmatrix}\mathbf{H}_{cf}\\\
\mathbf{0}\end{bmatrix}\\\
&=\mathbf{H}_{cx}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cf}\end{split}$
(11)

$\begin{split}\mathbf{C}&=\mathbf{H}_{n}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{m}\\\
&=\mathbf{H}_{cf}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cx}\\\
&=\mathbf{B}^{\intercal}\\\
\mathbf{D}&=\mathbf{H}_{n}^{\intercal}\mathbf{R}^{-1}\mathbf{H}_{n}\\\
&=\begin{bmatrix}\mathbf{H}_{cf}^{\intercal}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\mathbf{R}_{c}^{-1}&\mathbf{0}\\\
\mathbf{0}&\mathbf{R}_{o}^{-1}\end{bmatrix}\begin{bmatrix}\mathbf{H}_{cf}\\\
\mathbf{0}\end{bmatrix}\\\
&=\mathbf{H}_{cf}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cf}\end{split}$
(12)

By using the augmented covariance equation (6), the system covariance with the
new landmark can be expanded out. The resultant expressions are identical to
those derived in [5]. For a sanity check, we expand the marginal covariance
for the new landmark parameters by substituting the terms of (11) and (12)
into the bottom right block of (6). First notice that the inverse of
$\mathbf{H}_{cf}$ exists because we assume the landmark is well observed. Then
the central inverse component can be simplified as

$\begin{split}\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C}&=\mathbf{P}^{-1}+\mathbf{H}_{cx}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cx}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o}\\\
&\quad-\mathbf{H}_{cx}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cf}\cdot\mathbf{H}_{cf}^{-1}\mathbf{R}_{c}\mathbf{H}_{cf}^{-\intercal}\cdot\\\
&\quad\mathbf{H}_{cf}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cx}\\\
&=\mathbf{P}^{-1}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o}.\end{split}$
(13)

By the Sherman-Morrison-Woodbury-Schur formula [2, (A.6a)], the inverse of the
above expression can be converted as

$\begin{split}(\mathbf{P}^{-1}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o})^{-1}=\\\
\mathbf{P}-\mathbf{P}\mathbf{H}_{o}^{\intercal}(\mathbf{H}_{o}\mathbf{P}\mathbf{H}_{o}^{\intercal}+\mathbf{R}_{o})^{-1}\mathbf{H}_{o}\mathbf{P}\end{split}$
(14)

Finally, the marginal covariance for the landmark (see (6)) can be derived as

$\begin{split}\texttt{cov}(\delta\mathbf{f})&=\mathbf{D}^{-1}+\mathbf{D}^{-1}\mathbf{C}(\mathbf{A}-\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\mathbf{B}\mathbf{D}^{-1}\\\
&=\mathbf{H}_{cf}^{-1}\mathbf{R}_{c}\mathbf{H}_{cf}^{-\intercal}+\mathbf{H}_{cf}^{-1}\mathbf{R}_{c}\mathbf{H}_{cf}^{-\intercal}\cdot\\\
&\quad\mathbf{H}_{cf}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cx}\cdot(\mathbf{P}^{-1}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o})^{-1}\cdot\\\
&\quad\mathbf{H}_{cx}^{\intercal}\mathbf{R}_{c}^{-1}\mathbf{H}_{cf}\cdot\mathbf{H}_{cf}^{-1}\mathbf{R}_{c}\mathbf{H}_{cf}^{-\intercal}\\\
&=\mathbf{H}_{cf}^{-1}\mathbf{R}_{c}\mathbf{H}_{cf}^{-\intercal}+\mathbf{H}_{cf}^{-1}\mathbf{H}_{cx}\cdot\\\
&\quad(\mathbf{P}^{-1}+\mathbf{H}_{o}^{\intercal}\mathbf{R}_{o}^{-1}\mathbf{H}_{o})^{-1}\mathbf{H}_{cx}^{\intercal}\mathbf{H}_{cf}^{-\intercal}\end{split}$
(15)

In view of (14), it is obvious that the above expression is the same as [5,
(30)].

One benefit of separating the observations into two groups is that expressions
in (6) can be written out analytically as (15). Another benefit is that the
covariance calculation (6) can be divided into two smaller steps, one for
augmenting new parameters to the covariance matrix, and the other for updating
covariance with classic EKF, as done in [8]. The first step uses the projected
observations $\delta\mathbf{z}_{c}$ that depend on both the system state and
landmark parameters, to initialize the covariance for the landmark parameters
and their correlation to the system state. The analytic expressions [8,
(21)-(22)] for these covariance blocks can be derived by (6). The second step
uses the projected observations $\delta\mathbf{z}_{o}$ that do not depend on
landmark parameters, to update the current system state which includes the
just augmented landmark parameters. The net effect of the two steps can be
shown analytically to be identical to the above approach (see (15)) that uses
all observations in a single step.

Because the above derivation has little bearing on the number of landmarks, it
should be able to augment multiple landmarks into the covariance matrix at
once.

###  III-B Relative Pose Uncertainty

Many odometry algorithms use relative pose constraints in graph-based
optimization which often need a uncertainty estimate in order to properly
weight them in optimization [9].

One type of relative pose constraint is calculated from 3D landmark - 2D
feature correspondences. The relative pose estimate can be solved by a PnP
algorithm, e.g., [10]. The information for the relative pose can be obtained
from the Hessian matrix of the linearized observation system if we assume that
the 3D landmarks are free of noise.

Otherwise, if we have prior knowledge about the uncertainty of these 3D
landmarks, then we may use the proposed method to compute the information
matrix for the relative pose constraint. In this case, the existing state is
the 3D landmarks
$\mathbf{x}_{f}=[\mathbf{f}_{1}^{\intercal},\mathbf{f}_{2}^{\intercal},\dots,\mathbf{f}_{l}^{\intercal}]^{\intercal}$,
and the new state is the relative pose, $\mathbf{T}$. The image observations
$\mathbf{z}$ of these landmarks can be linearized and written in terms of the
error state
$\delta\mathbf{x}_{f}=[\delta\mathbf{f}_{1}^{\intercal},\delta\mathbf{f}_{2}^{\intercal},\dots,\delta\mathbf{f}_{l}^{\intercal}]^{\intercal}$
and $\delta\mathbf{T}$,

$\delta\mathbf{z}=\begin{bmatrix}\mathbf{H}_{f}&\mathbf{H}_{T}\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}_{f}\\\
\delta\mathbf{T}\end{bmatrix}+\mathbf{w}$ (16)

where $\mathbf{w}$ is Gaussian white noise, $\mathbf{w}\sim
N(\mathbf{0},\mathbf{R})$. The prior uncertainties for these landmarks may for
instance come from a landmark-based filter for vision-aided odometry, e.g.,
[5], and they typically takes the form (2). Then the information matrix for
the combined system state can be immediately obtained with (4). The
information for $\delta\mathbf{T}$. i.e., inverse of its marginal covariance,
is

$\boldsymbol{\Lambda}_{T}=\mathbf{D}-\mathbf{C}\mathbf{A}^{-1}\mathbf{B}$ (17)

in terms of (5). Because block $\mathbf{A}$ corresponds to landmark parameters
which are well constrained by the prior, it should be always invertible.

To properly weight this relative pose constraint in optimization, the square
root information matrix $\mathbf{R}$ often comes handy for normalizing
(whitening) the measurement error $\delta\mathbf{z}$. It can be calculated by
the $\mathbf{LDL}^{\intercal}$ decomposition of $\boldsymbol{\Lambda}_{T}$ to
handle a possibly singular $\boldsymbol{\Lambda}_{T}$ which may crop up due to
uninformative observations, e.g., [9]. Formally, the decomposition is given as

$\begin{split}\boldsymbol{\Lambda}_{T}&=\mathbf{P^{\intercal}LDL^{\intercal}P}=\mathbf{R}^{\intercal}\mathbf{R}\\\
\mathbf{R}&=\mathbf{\sqrt{D}L^{\intercal}P}\end{split}$ (18)

where $\mathbf{D}$ is a diagonal matrix, $\mathbf{L}$ a lower triangular
matrix with a unit diagonal, and $\mathbf{P}$ a permutation matrix. As such,
$\mathbf{R}$ is not necessarily upper triangular.

##  IV Conclusion

We derive a solution for computing the covariance of extra parameters involved
in new observations of existing parameters with prior knowledge. This solution
can be viewed as a special case of the Kalman filter update, but is more
general as it works with degenerate information matrices. The derived
expressions apply to several problems in vision-aided odometry, including
initializing covariance for landmark parameters, and computing square root
information for relative pose constraints. For covariance initialization of
landmark parameters, the proposed method arrives at expressions exactly
matching with independently reported results [5, 8], thus proving its
validity.

## ACKNOWLEDGMENT

The author thanks Charles Toth for helpful remarks.

## References

  * [1] C. Jekeli, _Inertial Navigation Systems with Geodetic Applications:_. Berlin, Boston: DE GRUYTER, Jan. 2001.
  * [2] B. Schaffrin and K. Snow, _Notes on adjustment computations part I_ , The Ohio State University, Dec. 2019. [Online]. Available: https://earthsciences.osu.edu/sites/default/files/2019-12/OSU_Adjustment_Notes_Part_1.pdf
  * [3] T.-T. Lu and S.-H. Shiou, “Inverses of $2\times 2$ block matrices,” _Computers & Mathematics with Applications_, vol. 43, no. 1-2, pp. 119–129, 2002, publisher: Elsevier. [Online]. Available: http://msvlab.hre.ntou.edu.tw/grades/now/inte/Inverse%20&%20Border/border-LuTT.pdf
  * [4] C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds,” _Information Fusion_ , vol. 14, no. 1, pp. 57–77, July 2011, arXiv: 1107.1119. [Online]. Available: http://arxiv.org/abs/1107.1119
  * [5] M. Li and A. I. Mourikis, “Optimization-based estimator design for vision-aided inertial navigation: supplemental material,” in _Robotics: Science and Systems_ , 2013, pp. 241–248.
  * [6] R. Hartley and A. Zisserman, _Multiple View Geometry in Computer Vision, Second Edition_ , 2nd ed. Cambridge University Press, 2003.
  * [7] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in _Proceedings 2007 IEEE International Conference on Robotics and Automation_ , Apr. 2007, pp. 3565–3572. [Online]. Available: http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf
  * [8] P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in _IROS 2019 Workshop on Visual-Inertial Navigation: Challenges and Applications, Macau, China_ , 2019.
  * [9] E. Westman and M. Kaess, “Degeneracy-aware Imaging Sonar SLAM,” _IEEE Journal of Oceanic Engineering_ , p. 15, 2019.
  * [10] L. Kneip, D. Scaramuzza, and R. Siegwart, “A novel parametrization of the perspective-three-point problem for a direct computation of absolute camera position and orientation,” in _CVPR 2011_ , June 2011, pp. 2969–2976, iSSN: 1063-6919.