11institutetext: Wuhan University, Wuhan Hubei 430079, China 11email:
<EMAIL_ADDRESS>22institutetext: ETH, Zurich Switzerland 22email:
<EMAIL_ADDRESS>33institutetext: York University, Toronto ON, Canada
33email<EMAIL_ADDRESS>
# NOCT: Nonlinear Observability with Constraints and Time Offset

Jianzhu Huai 11 Yukai Lin 22 Yujia Zhang 33

###### Abstract

Nonlinear systems of affine control inputs overarch many sensor fusion
instances. Analyzing whether a state variable in such a nonlinear system can
be estimated (i.e., observability) informs better estimator design. Among the
research on local observability of nonlinear systems, approaches based on
differential geometry have attracted much attention for the solid theoretic
foundation and suitability to automated deduction. Such approaches usually
work with a system model of unconstrained control inputs and assume that the
control inputs and observation outputs are timestamped by the same clock. To
our knowledge, it has not been shown how to conduct the observability analysis
with additional constraints enforced on the system’s observations or control
inputs. To this end, we propose procedures to convert a system model of affine
control inputs with linear constraints into a constraint-free standard model
which is apt to be analyzed by the classic observability analysis procedure.
Then, the whole analysis procedure is illustrated by applying to the well-
studied visual inertial odometry (VIO) system which estimates the camera-IMU
relative pose and time offset. The findings about unobservable variables under
degenerate motion concur with those obtained with linearized VIO systems in
other studies, whereas the findings about observability of time offset extend
those in previous studies. These findings are further validated by simulation.

###### Keywords:

observability with constraints, Lie derivative, time offset, degeneracy
analysis, sensor fusion, visual inertial odometry

##  1 Introduction

Being prevalent in robots and augmented reality, state estimation usually
integrates data from sensors of different modalities mounted on a platform to
estimate the state of the platform, _e.g_., position and orientation. Examples
of state estimation include calibration, odometry, and mapping. For state
estimation, identifying unobservable state variables and handling them
properly in estimation is crucial to designing a good estimator [5]. Given a
multi-sensor system, identifying unobservable state variables without using
real data is usually achieved by observability analysis.

Much research has been conducted on this topic. For a linear system, it is
fairly easy to analyze its observability by using the Kalman’s test [7]. This
gets much harder for nonlinear systems for which conclusions drawn by the
observability analysis usually are confined to a local area in the state-
space, leading to the “local weak observability” [2]. One approach is to
linearize the system and assume that the control inputs are piece-wise
constant [1]. This approach often involves complex integrals and requires
human experience to identify the unobservable directions, thus it is time-
consuming to verify the results. Another problem of this approach is that we
do not know for certain whether all unobservable directions are identified.

An alternative approach is to directly work with the differential equations of
the system and analyze its observability with Lie derivatives as proposed in
[2]. The procedure in [2] has been extended to systems where the inputs
directly affect the output observations [19], and to systems where some inputs
are unknown [15]. These approaches provide analytical solutions and are
suitable for automatic deduction.

However, existing approaches based on Lie derivatives assume that the control
inputs to the system are unconstrained (general) and that there is a zero time
offset between the clock to timestamp control inputs and the clock to
timestamp the observation outputs. To our knowledge, it has not been shown how
to use the method based on Lie derivatives to analyze a nonlinear system when
additional constraints are added. As an example, for a visual inertial
odometry (VIO) system which estimates platform motion by fusing camera images
and inertial measurement unit (IMU) data, it often face scenes where the
landmarks are on a plane or times when the platform is restricted to move on a
plane.

This paper presents several procedures to convert a nonlinear system of affine
inputs with linear constraints to a nonlinear system of affine inputs (termed
a standard nonlinear system), so that the observability properties of the
original system can be obtained by analyzing the converted system with a well-
founded automated procedure. Then, for a monocular VIO problem where motion of
the platform, camera-IMU extrinsic parameters and time offset are to be
estimated, we analyze its observability properties under three types of
constraints. The identified unobservable variables in these cases agree with
the findings reported in other studies. These findings are also validated in
simulation with a filter-based VIO method, showing the effectiveness of
proposed conversion procedures. As for the time offset, we supplement our
earlier work [4] which identifies the condition when the time offset is
unobservable with simulation results.

In summary, the contributions of this paper are:

  1. 1.

To our knowledge, we are the first to deal with observalibity of nonlinear
systems of affine control inputs with linear constraints by using the Lie
derivatives. For this problem, we propose conversion procedures suitable to
automation.

  2. 2.

The effectiveness of the proposed method is demonstrated on a VIO system with
on-the-fly extrinsic and temporal calibration and confirmed by simulation.

##  2 Related Work

This section briefly reviews recent research in observability analysis. The
notion of observability was proposed by Kalman [7] for analyzing linear
systems. Hermann and Krener [2] extended observability analysis to nonlinear
systems by using differential geometry methods. In contrast to linear systems,
the nonlinear system’s observability analysis is typically local, _i.e_., only
valid for a neighborhood in the state-space. There are also global
observability analysis methods for a nonlinear system, _e.g_., [18], but these
problem-specific techniques do not easily adapt to other nonlinear systems.

Two streams of local observability analysis methods are popular for nonlinear
systems, discrete model analysis for piece-wise constant systems, _e.g_., [1],
and continuous model analysis based on the differential equations of a system,
_e.g_., [13]. The discrete model analysis works with the observability matrix
of the linearized system, whereas the continuous model analysis works with the
observability matrix derived from Lie derivatives of the continuous system.

Using the observability matrix built with piece-wise constant transition
functions, Li and Mourikis [10] improved consistency of a filter-based VIO
algorithm by maintaining observability properties of VIO. Recently, [23]
analyzed the unobservable directions of a linearized VIO system with full
self-calibration under motion constraints. Li and Stueckler [9] incorporated
constraints due to velocity controls and planar motion as observations into
the VIO system and analyzed observability of state variables by using the
linearized system model. Generally speaking, the linearized model requires
discretizing the differential equations and brings about complex integrals
which are difficult to deal with in automatic symbolic deduction. By manually
identifying the unobservable directions, this approach is likely to miss some
unobservable directions. For instance, consider the observability properties
obtained in [23] based on a linearized VIO model, the camera-IMU system would
have a different number of unobservable calibration parameters when the axis
labels of the IMU are swapped (_e.g_., x-y-z to z-x-y), indicating that some
unobservable parameters are missed out by manual checking.

Much work has been done on observability analysis with continuous models,
[16], [8], [3], [6], [20], [4], and [9]. Mirzaei _et al_. [16] applied the
continuous analysis to extrinsic calibration of a camera-IMU system based on
an Extended Kalman filter (EKF). Kelly _et al_. [8] proved that the camera-IMU
extrinsic parameters are observable with natural features. Jung _et al_. [6]
analyzed observability of an EKF-based estimator whose state vector includes
the IMU intrinsic parameters. Villaverde [20] extended the continuous model
analysis approach to systems where the control directly feeds to the output
(_i.e_., direct feed-through). The studies in [12] and [15] extended the
continuous analysis to systems with unknown inputs assuming the control input
is piece-wise constant. Huai [4] showed that full self-calibration of the
camera-IMU system could be achieved under general motion by using differential
equations of the VIO system.

These approaches working with continuous models have not explicitly considered
systems with constraints on observations or control inputs.

Previous approaches for temporal calibration identify conditions when the time
offset is unobservable by using system-specific derivation, _e.g_., [11].
Degenerate cases for a lidar-IMU system with on-the-fly extrinsic and temporal
calibration were identified in [24] by using a linearized model. For a camera-
IMU system, Yang _et al_. [23] analyzed cases where the time offset is
unobservable by using a linearized system model. Recently, in [4], degenerate
cases for time offset estimation were identified for a nonlinear system of
affine inputs, encompassing cases reported in [24, 23]. This paper provides
additional simulation results to supplement the findings of [4].

##  3 Nonlinear Observability Analysis Background

This section briefly presents the nonlinear observablity analysis procedure
with Lie derivatives developed in [2] and elaborated in [14], which is the
basis for the observability analysis with constraints.

The two popular observability analysis approaches for nonlinear systems, one
based on the discrete model, and the other based on the continuous model, are
briefly described in order for a better perspective. The given information to
the analysis includes the observations and control inputs in a time interval
$\mathcal{I}=[t_{0},t_{0}+\Delta t]$.

The method based on the discrete model first linearizes the differential
equations of the system around states at different time points in
$\mathcal{I}$, obtaining the discrete transition model and observation model,
and finally by using an observability matrix built from these models,
considers the condition to solve for the first of those states for the time
point sequence. Usually, the observability matrix involves integrals of
control inputs.

In contrast, the method based on differential geometry works with time
derivatives of the process model and the observation model of the nonlinear
system. The values of these time derivatives are known given the set of
observations in $\mathcal{I}$ according to the Taylor expansion of the
observation model at the start time point. These time derivatives are a set of
functions of the state at the start time point and the control inputs. By the
inverse function theorem, if the gradient of the set of equations is full
rank, then the initial state is locally observable. Since the set of equations
is in fact the product of a matrix involving only the control inputs and a
vector involving only Lie derivatives of the observation equations $\mbf h$.
Therefore, the gradient of the set of equations can be equivalently
represented by the gradient of the Lie derivatives of $\mbf h$ (hence the
nonlinear observability matrix in (5)), assuming that the control inputs are
unconstrained (_i.e_., can be chosen by a user so that the maximum rank of the
observability matrix is achieved).

In summary, Table 1 lists differences between observability analysis based on
the discrete model and that based on the continuous model.

Table 1: The differences between observability analysis with the linearized system model and that with the Lie derivatives of the nonlinear system. Differences | Linearized model |  | Nonlinear
---
continuous model
Assumptions |  | A series of state
---
vectors is roughly known;
First order approx.
| One state vector
---
is roughly known;
Inputs are general.
| Observablity matrix
---
$\mathcal{O}$ involves inputs?
| Integrals of
---
inputs are in $\mathcal{O}$.
| Inputs are
---
not in $\mathcal{O}$.
| Amenable to
---
automated deduction
Hardly | Yes
| Identify the
---
complete null space
| Very difficult to
---
manually find all.
| If the automated
---
program is efficient.

Now we briefly present fundamentals of the nonlinear observability analysis.
According to [14], a state at time $t_{0}$, $\mbf x(t_{0})$, is weakly
observable if there is a neighborhood in which all its neighbors can be
distinguished from itself by the knowledge of outputs and unconstrained inputs
in a time interval $\mathcal{I}=[t_{0},t_{0}+\Delta t]$. For a noise-free
system that is affine in the control inputs $\mbf u_{i}$, $i=1,2,\cdots,m$,

$\dot{\mbf{x}}=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\quad\mbf{y}=\mbf{h(x)}$ (1)

the sufficient condition for weak observability of its state $\mbf x$ is that
the observability matrix built from the outputs $\mbf h(\mbf x)$ has full rank
[2], [14]. The observability matrix $\mathcal{O}$ consists of gradients of Lie
derivatives of $\mbf h(\mbf x)$ along vector fields $\mbf f_{i}$ of the
control inputs $u_{i}$. For a vector output function $\mbf h$, its first order
Lie derivative along the vector field $\mbf f_{i}$ of one column is defined by

$\mathcal{L}^{1}_{\mbf f_{i}}\mbf{h}=\nabla\mbf h\cdot\mbf f_{i}$ (2)

The zeroth order Lie derivative is defined as

$\mathcal{L}^{0}\mbf h=\mbf h.$ (3)

Higher order Lie derivatives can be computed recursively by

$\mathcal{L}^{2}_{\mbf f_{i},\mbf f_{j}}\mbf{h}=\mathcal{L}^{1}_{\mbf
f_{j}}\mathcal{L}^{1}_{\mbf f_{i}}\mbf{h}.$ (4)

With these Lie derivatives of the observation function, the entire
observability matrix $\mathcal{O}$ is given by

$\mathcal{O}=\begin{bmatrix}\nabla\mathcal{L}^{0}\mbf h\\\
\nabla\mathcal{L}^{1}_{\mbf f_{i}}\mbf h\\\ \nabla\mathcal{L}^{2}_{\mbf
f_{i},\mbf f_{j}}\mbf h\\\ \vdots\end{bmatrix},$ (5)

where $i,j\in[0,1,\cdots,m]$. $\mathcal{O}$ can be viewed as a codistribution
spanned by row vectors (aka covectors). That is, $\mathcal{O}$ can be written
as

$\mathcal{O}=\mathrm{span}\\{\nabla\mathcal{L}^{0}\mbf
h,\nabla\mathcal{L}^{1}_{\mbf f_{i}}\mbf h,\nabla\mathcal{L}^{2}_{\mbf
f_{i},\mbf f_{j}}\mbf h,\cdots\\}$ (6)

The observability property of the affine-input system can be revealed by
incrementally building up the codistribution and checking its dimension.
Denote the codistribution with gradients of Lie derivatives up to order $k$ by
$\mathcal{O}_{k}$, _e.g_., $\mathcal{O}_{0}=\mathrm{span}\\{\nabla\mbf h\\}$
and $\mathcal{O}_{1}=\mathrm{span}\\{\nabla\mbf h,\nabla\mathcal{L}^{1}_{\mbf
f_{0}}\mbf h,\nabla\mathcal{L}^{1}_{\mbf f_{1}}\mbf
h,\cdots,\nabla\mathcal{L}^{1}_{\mbf f_{m}}\mbf h\\}$. As deduced in [14,
Algorithm 4.2], if
$\mathrm{rank}(\mathcal{O}_{k-1})=\mathrm{rank}(\mathcal{O}_{k})$, the
incremental procedure completes. By then, if
$\mathrm{rank}(\mathcal{O}_{k})=\dim(\mbf x)$, the system is weakly
observable.

To simplify observability analysis, it is beneficial to proactively identify
observable dimensions of the state $\mbf x$ in building up the codistribution.
Considering the codistribution at step $k$, $\mathcal{O}_{k}$, this can be
done by the following property. By Lemma 1 in [4], a component of $\mbf x$,
$\mbf x_{i}$, is weakly observable if removing the column corresponding to
$\mbf x_{i}$ from $\mathcal{O}_{k}$ reduces its rank by 1.

In the end, we note that the above analysis requires that the control inputs
are unconstrained and that the control inputs and the observations are
timestamped by the same clock. The following will try to address the two
limitations.

##  4 Observability Analysis with Constraints

This section presents our procedures to analyze observability when the
nonlinear system is subject to linear constraints. We first define the epitome
systems with constraints of increasing complexity which extend the constraint-
free standard system (1), and then propose the procedures to analyze their
observability. At last, we also list findings about observability of the time
offset.

###  4.1 Extended Systems

For convenience, the standard affine-input system is repeated here. The
dynamics of its state $\mbf x$ is affine in the control inputs
$\mathcal{U}=\\{u_{i}|i=1,2,\cdots,m\\}$. With a bit abuse of notation, the
vector of control inputs $\mbf u$ is used interchangeably with $\mathcal{U}$.

####  4.1.1 Standard System 1

In the standard system, the observation is a function of just the state vector
$\mbf x$.

$\dot{\mbf{x}}=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\quad\mbf{y}(t)=\mbf h(\mbf x(t)).$ (7)

####  4.1.2 System 2 with a zero constraint on the state

There is a 1-D constraint $0=c(\mbf x)$ on the state $\mbf{x}$.

$\begin{split}\dot{\mbf{x}}&=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\\\ \mbf{y}(t)&=\mbf h(\mbf x(t)),\\\ 0&=c(\mbf{x}).\end{split}$ (8)

####  4.1.3 System 3 with a constant constraint on the state

There is a 1-D constraint $d=c(\mbf x)$ on the state where $d$ is an unknown
constant.

$\begin{split}\dot{\mbf{x}}&=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\\\ \mbf{y}(t)&=\mbf h(\mbf x(t)),\\\ d&=c(\mbf{x}).\end{split}$ (9)

####  4.1.4 System 4 with a zero constraint on the state and affine in the
inputs

There is a 1-D constraint $0=c(\mbf x,\mathcal{U}_{c})$ on the state $\mbf{x}$
and a input subset $\mathcal{U}_{c}\in\mathcal{U}$. We also assume that the
constraint is affine in the control inputs.

$\begin{split}\dot{\mbf{x}}&=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\\\ \mbf{y}(t)&=\mbf h(\mbf x(t)),\\\ 0&=c(\mbf
x,\mathcal{U}_{c})=c_{0}(\mbf{x})+\sum_{i=1}^{l}c_{i}(\mbf x)u_{l}\quad
u_{l}\in\mathcal{U}_{c}\end{split}$ (10)

####  4.1.5 System 5 with a constant constraint on the state and affine in the
inputs

There is a 1-D constraint $d=c(\mbf x,\mathcal{U}_{c})$ on the state $\mbf{x}$
and a control input subset $\mathcal{U}_{c}\in\mathcal{U}$, where $d$ is a
unknown constant. We further assume that the constraint is affine in the
control inputs.

$\begin{split}\dot{\mbf{x}}&=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\\\ \mbf{y}(t)&=\mbf h(\mbf x(t)),\\\
d&=c_{0}(\mbf{x})+\sum_{i=1}^{l}c_{i}(\mbf x)u_{l}\quad
u_{l}\in\mathcal{U}_{c}\end{split}$ (11)

###  4.2 Conversion to the Standard Form

For each of the five systems, our goal is to check if the state $\mbf x(t)$ is
weakly observable given control inputs $\mathbf{u}$ and observations in time
interval $[t,t+\Delta t]$. Since we already know how to check the local
observability for System 1 (7), the idea is to convert a system with linear
constraints into the standard form. The guideline for the conversion is to
keep the information of the system before and after the conversion the same.

####  4.2.1 Conversion of System 2

We start with System 2 (8). One may be tempted to simply add the constraint
$0=c(\mbf x)$ to the set of observations. However, the subsequent
observability analysis may consider $c(\mbf x)$ to be time-varying as $\mbf
y(t)$ in $\mbf y(t)=\mbf h(\mbf x(t))$, effectively undoing the constraint.
Therefore, we choose a $x_{p}\in\mbf x$, express $x_{p}$ in terms of the other
variables involved in $c(\mbf x)$ as $x_{p}=c^{\prime}(\mbf x_{r})$, and
finally substitute $c^{\prime}$ for $x_{p}$ in the system equations, _i.e_.,

$\begin{split}\dot{\mbf{x}^{\prime}}&=\mbf{f}_{0}^{\prime}(\mbf
x^{\prime})+\sum_{i=1}^{m}\mbf f_{i}^{\prime}(\mbf x^{\prime})u_{i},\\\
\mbf{y}(t)&=\mbf h^{\prime}(\mbf x^{\prime}(t)),\end{split}$ (12)

where $\mbf x^{\prime}=\mbf x\backslash x_{p}$.

####  4.2.2 Conversion of System 3

System 3 can be converted to the standard form in two steps, first to System
2, and then to System 1. The first step is done by adding $d$ as an unknown
parameter to the state, and the system becomes

$\begin{split}\begin{bmatrix}\dot{\mbf{x}}\\\
\dot{d}\end{bmatrix}&=\begin{bmatrix}\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf
f_{i}(\mbf x)u_{i}\\\ 0\end{bmatrix},\\\ \begin{bmatrix}\mbf{y}(t)\\\
0\end{bmatrix}&=\begin{bmatrix}\mbf h(\mbf x(t))\\\
c(\mbf{x})-d\end{bmatrix}.\end{split}$ (13)

The second step is the same as for System 2 except that the chosen $x_{p}$
should not be $d$.

####  4.2.3 Conversion of System 4

For System 4, to maintain the affine-input property, instead of replacing
$x_{p}$, we pick up a control input $u_{o}\in\mathcal{U}_{c}=\\{u_{o},\mbf
u_{r}\\}$ (subscript ${\cdot}_{o}$ for removal) and remove it from the
differential equation for $\mbf x$ with the steps:

  1. 1.

choose one $u_{o}\in\mathcal{U}_{c}$, and express it by rearranging the
constraint, $u_{o}=c^{\prime}(\mbf x,\mathbf{u}_{r})$ (note that $c^{\prime}$
is affine in $\mathbf{u}_{r}$);

  2. 2.

substitute the expression $c^{\prime}$ for $u_{o}$ in the process equation
$\dot{\mbf x}$.

Next, since $u_{o}$ is known, we will take
$u_{o}=c^{\prime}(x,\mathbf{u}_{r})$ as an observation. However, unlike
observations in the standard form, it has control inputs $\mathbf{u}_{r}$.
Therefore, we will factor these control inputs out of this observation with
the steps:

  1. 3.

treat control inputs $\mathbf{u}_{r}$ as state variables and add to the state
vector $\mbf x$;

  2. 4.

remove $\mathcal{U}_{c}$ from the control inputs, add time derivatives of
$\mathbf{u}_{r}$ to the control inputs,

  3. 5.

finally, add $u_{o}$ and $\mathbf{u}_{r}$ as observations to the system.

In the end, the resulting system has a state vector $\mathbf{x}^{\prime}=[\mbf
x;\mathbf{u}_{r}]$, and control inputs
$\\{\mathcal{U}\backslash\mathcal{U}_{c},\dot{\mathbf{u}}_{r}\\}$. The new
system can be written as

$\begin{split}\frac{d}{dt}\begin{bmatrix}\mbf{x}\\\
\mbf{u}_{r}\end{bmatrix}&=\begin{bmatrix}\mbf{f}_{0}^{\prime}(\mbf
x)+\sum_{i\neq o}^{m}\mbf f_{i}^{\prime}(\mbf x)u_{i},\\\
\dot{\mbf{u}}_{r}\end{bmatrix}\\\ \begin{bmatrix}\mbf{y}(t)\\\ \mbf{u}_{r}\\\
u_{o}\end{bmatrix}&=\begin{bmatrix}\mbf h(\mbf x(t))\\\ \mbf{u}_{r}\\\
c^{\prime}(x,\mbf u_{r})\end{bmatrix}\end{split}$ (14)

where $\mbf f_{0}^{\prime}$ and $\mbf f_{i}^{\prime}$ correspond to $\mbf
f_{0}$ and $\mbf f_{i}$ after $c^{\prime}(\mbf x,\mbf{u}_{r})$ is plugged in,
respectively. It is easy to confirm that the above system is in the standard
form (7).

####  4.2.4 Conversion of System 5

The system 5 can be converted to the standard form in two steps. First, add
$d$ as a unknown parameter to the state, leading to

$\begin{split}\begin{bmatrix}\dot{\mbf{x}}\\\
\dot{d}\end{bmatrix}&=\begin{bmatrix}\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf
f_{i}(\mbf x)u_{i}\\\ 0\end{bmatrix},\\\ \begin{bmatrix}\mbf{y}(t)\\\
0\end{bmatrix}&=\begin{bmatrix}\mbf h(\mbf x(t)),\\\
c_{0}(\mbf{x})+\sum_{i=1}^{l}c_{i}(\mbf x)u_{l}-d\end{bmatrix}\quad
u_{l}\in\mathcal{U}_{c}.\end{split}$ (15)

Second, convert the new system to the standard form as for System 4.

The above conversion procedures can be applied to multi-dimensional
constraints by dealing with each dimension sequentially.

###  4.3 Observability of Time Offset

The observability of the time offset between control inputs and output
observations for a nonlinear system affine in inputs has been conducted in [4]
and the conclusion is repeated below. The nonlinear system with time offset
can be expressed by

$\dot{\mbf{x}}=\mbf{f}_{0}(\mbf x)+\sum_{i=1}^{m}\mbf f_{i}(\mbf
x)u_{i},\quad\mbf{y}(t)=\mbf h(\mbf x(t+t_{d})),$ (16)

where $t_{d}$ is the time offset. Given control inputs $\mathbf{u}$ in time
interval $[t+t_{d},t+\Delta t+t_{d}]$ and observations in $[t,t+\Delta t]$,
the time offset is weakly unobservable when the system goes through constant
control inputs or constant observations.

##  5 Application to Visual Inertial Odometry under Constraints

To validate the proposed procedures, we consider a VIO system that fuses
camera observations of opportunistic landmarks and measurements of an IMU
(consisting of a 3-axis accelerometer and a 3-axis gyroscope) to track motion
of the camera-IMU platform.

Coordinate Frames: The world frame $\\{W\\}$ is chosen at the start of the VIO
with z-axis along the negative gravity, such that $\mbf g^{W}=[0,0,-g]$ where
$g$ is the gravity magnitude. We assume that the IMU is well calibrated but
subject to constant biases. The IMU frame $\\{S\\}$ is realized by the
accelerometer triad. The camera frame $\\{C\\}$ is at the camera optical
center. The body frame $\\{B\\}$ is set to be $\\{S\\}$.

Rotation Parameterization: The Hamilton quaternion as in Eigen library and ROS
[17] is used.

A quaternion $\mbf q_{AB}=[q_{w},q_{x},q_{y},q_{z}]^{\mkern-1.5mu\mathsf{T}}$
can be converted to the rotation matrix $\mbf R_{AB}$ (orientation of frame
$\\{B\\}$ in frame $\\{A\\}$) by

$\mbf R_{AB}(\mbf
q_{AB})=\begin{bmatrix}q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2}&2(q_{x}q_{y}-q_{w}q_{z})&2(q_{x}q_{z}+q_{w}q_{y})\\\
2(q_{x}q_{y}+q_{w}q_{z})&q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2}&2(q_{y}q_{z}-q_{w}q_{x})\\\
2(q_{x}q_{z}-q_{w}q_{y})&2(q_{y}q_{z}+q_{w}q_{x})&q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2}\end{bmatrix}.$
(17)

For easy result interpretation, the extra freedom of a quaternion is trimmed
by expressing it by $\bar{\mbf
q}=[q_{x},q_{y},q_{z}]^{\mkern-1.5mu\mathsf{T}}$ whose correponding unit
quaternion is $\mbf
q=[1,q_{x},q_{y},q_{z}]^{\mkern-1.5mu\mathsf{T}}/\sqrt{[1,q_{x},q_{y},q_{z}]\cdot[1,q_{x},q_{y},q_{z}]^{\mkern-1.5mu\mathsf{T}}}$.
The concern of singularity of $\bar{\mbf q}$ is appeased by considering that
the symbolic analysis procedure assumes general values for variables.
Otherwise, 4-parameter quaternions can be used without affecting the
conclusions.

State Vector: For the VIO system, the state vector consists of velocity of
$\\{B\\}$ relative to $\\{W\\}$ expressed in $\\{B\\}$, $\mbf v^{B}$, the
gravity vector in $\\{B\\}$, $\mbf g^{B}$, IMU biases $\mbf b=[\mbf
b_{g}^{\mkern-1.5mu\mathsf{T}},\mbf
b_{a}^{\mkern-1.5mu\mathsf{T}}]^{\mkern-1.5mu\mathsf{T}}$, the relative pose
of the IMU in the camera frame, $[\mbf p_{CB},\bar{\mbf q}_{CB}]$, and
parameters of a landmark $L$ with coordinates $\mbf
p_{L}^{C}=[p_{x}^{C},p_{y}^{C},p_{z}^{C}]^{\mkern-1.5mu\mathsf{T}}$,
${\boldsymbol{\gamma}}$ and $\rho$,

$\mbf x=[\mbf v^{B},\mbf g^{B},\mbf b,\mbf p_{CB},\bar{\mbf
q}_{CB},{\boldsymbol{\gamma}},\rho],$ (18)

where
${\boldsymbol{\gamma}}\triangleq[p_{x}^{C}/p_{z}^{C},p_{y}^{C}/p_{z}^{C}]$ and
$\rho\triangleq 1/p_{z}^{C}$. The state variables are chosen so that the
unobservable dimensions of a general VIO system (global translation and the
rotation angle about gravity) are not included. The gravity vector $\mbf
g^{B}$ encodes the roll and pitch angles of the system and the gravity
magnitude. Without loss of generality, only one landmark is chosen for the
analysis.

Continuous System Model: For the sake of observability analysis, all the
noises in the process and observation equations are omitted. The perfect IMU
measurements $\mbf a_{t}$ and ${\boldsymbol{\omega}}_{t}$ can be obtained from
the IMU readings $\mbf a_{m}$ and ${\boldsymbol{\omega}}_{m}$ by

$\mbf a_{t}=\mbf a_{m}-\mbf
b_{a}\quad{\boldsymbol{\omega}}_{t}={\boldsymbol{\omega}}_{m}-{\boldsymbol{b}}_{g}$
(19)

By basic derivation, the differential equation of the VIO system is found as

$\begin{bmatrix}\dot{\mbf v}^{B}\\\ \dot{\mbf g}^{B}\\\ \dot{\mbf b}\\\
\dot{\mbf p}_{CB}\\\ \dot{\bar{\mbf q}}_{CB}\\\ \dot{{\boldsymbol{\gamma}}}\\\
\dot{\rho}\end{bmatrix}=\begin{bmatrix}\mbf{v}^{B}\times{\boldsymbol{\omega}}_{t}+\mbf{a}_{t}+\mbf{g}^{B}\\\
\mbf g^{B}\times{\boldsymbol{\omega}}_{t}\\\ \mbf 0_{6}\\\ \mbf 0_{3}\\\ \mbf
0_{3}\\\
\begin{array}[]{c}C_{{\boldsymbol{\gamma}}\rho}[(\mbf{R}_{CB}{\boldsymbol{\omega}}_{t})\times(\rho\mbf{p}_{CB}-\bar{{\boldsymbol{\gamma}}})-\\\
\mbf{R}_{CB}\rho\mbf{v}^{B}]\end{array}\end{bmatrix}$ (20)

where the shorthand symbols are

$\bar{{\boldsymbol{\gamma}}}=[{\boldsymbol{\gamma}}^{\mkern-1.5mu\mathsf{T}}\quad
1]^{\mkern-1.5mu\mathsf{T}},\quad
C_{{\boldsymbol{\gamma}}\rho}=\begin{bmatrix}\mbf{I}_{2}&-{\boldsymbol{\gamma}}\\\
0_{1\times 2}&-\rho\end{bmatrix}.$ (21)

The control inputs to the system are $\mbf u=\\{{\boldsymbol{\omega}}_{m},\mbf
a_{m}\\}$. The observations of the system are the camera reprojections and the
gravity norm. We assume that the camera intrinsic parameters are known, thus
we can undistort and normalize the image observation at image plane $z=1$,
giving $\mbf h_{1}={\boldsymbol{\gamma}}$. In summary, the observations are

$\mbf{h}_{1}={\boldsymbol{\gamma}}\quad
h_{2}={\mbf{g}^{B}}^{\mkern-1.5mu\mathsf{T}}\mbf g^{B}.$ (22)

###  5.1 Observability under Degenerate Motion

In the following, we study the VIO system (20)-(22) under three types of
constraints corresponding to three degenerate motion types, and identify the
corresponding unobservable state variables by using the method in Section 4.2.
The three degenerate motion types are constant local linear acceleration,
single-axis rotation (and otherwise general translation), and pure
translation.

The constant local acceleration involves a 3-D constraint given by

$\mathrm{const}=\mbf a^{B}=\mbf a_{t}+\mbf g^{B}=\mbf a_{m}-\mbf b_{a}+\mbf
g^{B}.$ (23)

When the platform is constrained to go through single-axis rotation (_e.g_.,
general motion on a 2-D planar), the constant global rotation axis constraint
is expressed by
$\mathrm{const}=\omega^{W}_{WB}/\sqrt{(\omega^{W}_{WB})^{\mkern-1.5mu\mathsf{T}}\omega^{W}_{WB}}$.
Without loss of generality, we assume that the rotation axis is aligned with
the z-axis of the IMU frame, thus, the constraint becomes

$[0,0]^{\mkern-1.5mu\mathsf{T}}=[\omega_{tx},\omega_{ty}]^{\mkern-1.5mu\mathsf{T}}=({\boldsymbol{\omega}}_{m}-\mbf
b_{g})_{1:2},$ (24)

where
${\boldsymbol{\omega}}_{t}=[\omega_{tx},\omega_{ty},\omega_{tz}]^{\mkern-1.5mu\mathsf{T}}$.
When the rotation axis is not along the IMU z-axis, we can define a
$\\{B^{\prime}\\}$ frame at the same origin as $\\{B\\}$ and with z-axis along
the rotation axis. Denoting the rotation between the $\\{B^{\prime}\\}$ and
$\\{B\\}$ by $\mbf R_{BB^{\prime}}$, we see that the single-axis rotation
constraint relates to the gyroscope reading by

${\boldsymbol{\omega}}_{m}=\mbf
R_{BB^{\prime}}[0,0,\lambda]^{\mkern-1.5mu\mathsf{T}}+\mbf b_{g},$ (25)

where $\lambda$ is the time-varying angular rate. The corresponding
constraints after rearranging the terms are

$[0,0]^{\mkern-1.5mu\mathsf{T}}=[\mbf
R_{BB^{\prime}}^{\mkern-1.5mu\mathsf{T}}({\boldsymbol{\omega}}_{m}-\mbf
b_{g})]_{1:2}.$ (26)

The pure translation motion means that the angular rate is zero,

$\mbf 0_{3\times 1}={\boldsymbol{\omega}}_{t}={\boldsymbol{\omega}}_{m}-\mbf
b_{g}.$ (27)

Using the procedures presented in Section 4.2, we incorporate these three
types of constraints separately into the unconstrained VIO system (20)-(22)
and convert each of the three specific systems into the standard form. For the
motion of constant local acceleration (23), we use the procedure for System 5.
For the single-axis rotation constraint (24) and the pure translation
constraint (27), the procedure for System 4 is applied.

Then, each converted system is automatically analyzed by an observability
checking procedure similar to Algorithm 4.2 [14] which was implemented in
MATLAB using three key symbolic functions $\mathrm{diff}()$,
$\mathrm{rank}()$, $\mathrm{null}()$. The improvement relative to Algorithm
4.2 [14] includes keeping only row vectors spanning the row space of the
observability matrix $\mathcal{O}_{k}$ after each iteration $k$ to restrain
the problem size and speed up computation of the null space.

The automatic procedure concludes that the system in (20)-(22) is fully
observable, when the platform goes through general motion, as expected. With
the constant local acceleration, the metric scale of the system becomes
additionally unobservable, confirming statements in [21] and [22]. The
automatic procedure identifies the corresponding 1-column null space $\mbf N$,

$\begin{bmatrix}(\mbf v^{B})^{\mkern-1.5mu\mathsf{T}}&(\mbf
g^{B})^{\mkern-1.5mu\mathsf{T}}&\mbf 0_{3}^{\mkern-1.5mu\mathsf{T}}&-(\mbf
a_{t}+\mbf g^{B})^{\mkern-1.5mu\mathsf{T}}&(\mbf
p_{CB})^{\mkern-1.5mu\mathsf{T}}&\mbf 0_{1\times
5}&-\rho\end{bmatrix}^{\mkern-1.5mu\mathsf{T}}.$ (28)

For the single-axis rotation, our procedure found that the z-component of
$\mbf p_{CB}$ is additionally unobservable as concluded in [21] and [22], with
a 1-column null space,

$\begin{bmatrix}\mbf 0_{1\times 12}\quad
2(q_{y}+q_{x}q_{z})\quad-2(q_{x}-q_{y}q_{z})\quad-(q_{x}^{2}+q_{y}^{2}-q_{z}^{2}-1)\quad\mbf
0_{1\times 6}\end{bmatrix}^{\mkern-1.5mu\mathsf{T}},$ (29)

where $\bar{\mbf q}_{CB}=[q_{x},q_{y},q_{z}]$. Since a perturbation
$\epsilon\xi$ in the null space on $\mbf x$, _i.e_., $\mbf x^{\prime}=\mbf
x+\epsilon\xi,\xi\in\mbf N$, does not affect the system, in this case, we see
that the only vector in the null space changes $\mbf p_{CB}$ by the amount
$\epsilon\mbf R_{CB}[0,0,1]^{\mkern-1.5mu\mathsf{T}}$ which corresponds to an
increment on the z-component of $\mbf p_{CB}$.

For the pure translation, our procedure found that $\mbf p_{CB}$, $\mbf
g^{B}$, and $\mbf b_{a}$ are indeterminable (_i.e_., unable to be verified as
observable by Lemma 1 in [4]). There is a 5-column null space given by

$\mbf N^{\mkern-1.5mu\mathsf{T}}=\left[\begin{array}[]{c|ccc|c|ccc|cc}\mbf
0_{3\times 3}&\mbf 0_{3\times 1}&\mbf 0_{3\times 1}&\mbf 0_{3\times 1}&\mbf
0_{3\times 3}&\mbf 0_{3\times 1}&\mbf 0_{3\times 1}&\mbf 0_{3\times 1}&\mbf
I_{3}&\mbf 0_{3\times 6}\\\ \mbf 0_{1\times 3}&-g^{B}_{y}&g^{B}_{x}&0&\mbf
0_{1\times 3}&-g^{B}_{y}&g^{B}_{x}&0&0_{1\times 3}&0_{1\times 6}\\\ \mbf
0_{1\times 3}&-g^{B}_{z}&0&g^{B}_{x}&\mbf 0_{1\times
3}&-g^{B}_{z}&0&g^{B}_{x}&0_{1\times 3}&0_{1\times 6}\end{array}\right].$ (30)

It is easy to confirm that the first 3 rows of $\mbf
N^{\mkern-1.5mu\mathsf{T}}$ correspond to $\mbf p_{CB}$, and the last two rows
correspond to roll and pitch of the platform, meaning that $\mbf p_{CB}$ and
$\mbf q_{WB}$ are unobservable. Because of the correlation between $\mbf
g^{B}$ and $\mbf b_{a}$, both $\mbf g^{B}$ and $\mbf b_{a}$ are
indeterminable. In summary, the findings by our analysis procedure and by
using the linearized model are listed in Table 2.

Table 2: Unobservable state variables of a visual inertial odometry system with extrinsic calibration under three degenerate motion types, obtained by the observability matrix built from the linearized model and by that built from Lie derivatives of the nonlinear system. col($\mbf N$) denotes the number of columns of the null space matrix $\mbf N$. | Unobservable
---
state variables
| Rotation about
---
z-axis of $\\{B\\}$
| Const. local
---
acceleration
| No rotation
---
| Linearized model
---
incl. $\mbf p_{WB}$ and yaw
[21] [22]
|  $\mathbf{p}_{CB}$ along
---
z-axis
scale |  | global rotation,
---
$\mathbf{p}_{CB}$
Nonlinear model (our result) | Indeterminable | $\mathbf{p}_{CB}$ | $\mbf v^{B},\mathbf{b}_{a},\mathbf{p}_{CB},\rho$ | $\mbf g^{B},\mathbf{b}_{a},\mathbf{p}_{CB}$
Null space |  | col($\mbf N$)=1
---
| col($\mbf N$)=1
---
| col($\mbf N$)=5
---
|  | Unobservable
---
|  $\mathbf{p}_{CB}$ along
---
z-axis
| scale
---
| roll and pitch,
---
$\mathbf{p}_{CB}$

Considering the time offset parameter, we also list in Table 3 the sufficient
conditions when it is unobservable as stated in Section 4.3 and those reported
by the related work. The conditions reported by [22] is a subset of those
identified by our analysis, because the VIO system can be rewritten in terms
of alternative sets of control inputs including
$[{\boldsymbol{\omega}}^{B},\mbf a^{W}]$, and $[{\boldsymbol{\omega}}^{B},\mbf
a^{B}]$, where $\mbf a^{W}$ and $\mbf a^{B}$ are the platform acceleration
caused by specific forces expressed in $\\{W\\}$ and $\\{B\\}$, respectively.

Table 3: Sufficient conditions when the camera’s time offset to the IMU is unobservable, obtained by the linearized model and by the continuous model. $\mbf a^{W}$ is the platform acceleration due to specific forces in the $\\{W\\}$ frame. | Time offset unobservable
---
Sufficient condition
Linearized model [22], [23] | const. $\boldsymbol{\omega}^{B}$ and const. $\mathbf{v}^{B}$
const. $\boldsymbol{\omega}^{B}$ and const. $\mathbf{a}^{W}$
| Nonlinear model (our result)
---
const. control inputs

##  6 Experimental Results

This section presents simulation results to validate the preceding
observability properties. To easily reproduce the results, the simulation and
VIO estimation was carried out in OpenVINS. In simulation, a monocular camera-
IMU system moves on a specified trajectory. Random point landmarks are put in
the scene to ensure the camera observes enough features in every view sampled
at 10 Hz. The IMU data are sampled at 400 Hz.

###  6.1 Observability with Constrained Motion

To validate the unobservable parameters listed in Table 2, we simulate a
camera-IMU system moving on a 30∘ slope in three trajectories, straight line
of varying linear accelerations, lemniscate of varying angular rates, and
circle of a constant angular rate, as depicted in Fig. 1. The z-axis of the
IMU frame is along the normal of the slope. The gravity is along the negative
z-axis of the $\\{W\\}$ frame.

Figure 1: Three planar trajectories (black) on a 30∘ slope, lemniscate, line
segment, and circle, and the random landmarks (blue crosses). The gravity is
along the negative z-axis of the $\\{W\\}$ frame.

The estimated state variables are

$\\{\mbf p_{WB},\mbf{q}_{WB},\mbf{v}^{W},\mbf{b},\mbf
p_{BC},\mbf{q}_{BC},t_{d}\\}.$ (31)

Note that OpenVINS estimates a slightly different set of variables, we convert
their estimates and standard deviations to those of (31) for result
presentation. The estimation errors and standard deviations for (31) in a
typical run under these planar trajectories are depicted in Figs. 2 and 3.

Figure 2: Errors of the estimated orientation $\mbf q_{WB}$ (top row),
position $\mbf p_{WB}$ (middle row), and velocity $\mbf v^{W}$ (bottom row),
and their $\pm 3\sigma$ bounds, under three motion types, lemniscate, line
segment and circle.

From Fig. 2, we see that: (1) With lemniscate and circle trajectories, the yaw
component of $\mbf q_{WB}$ does not converge. For the line segment motion, all
three components of $\mbf q_{WB}$ show slightly widening $3\sigma$ bounds,
implying that $\mbf q_{WB}$ is unobservable with only translation. (2) For all
three trajectories, components of $\mbf p_{WB}$ have growing errors and
widening $3\sigma$ bounds, and components of $\mbf v^{W}$ have bounded errors
and $3\sigma$ curves.

Figure 3: Errors of the estimated biases $\mbf b_{g}$ (top row) and $\mbf
b_{a}$ (second row), extrinsic parameters $\mbf q_{BC}$ (third row) and $\mbf
p_{BC}$ (fourth row), and time offset $t_{d}$ (fourth row last), and their
$\pm 3\sigma$ bounds, under three motion types, lemniscate, line segment and
circle.

From Fig. 3, it can be seen that: (1) Gyroscope biases converge well with all
three motion types. (2) For the line segment trajectory, accelerometer biases
do not converge, and for the circle motion, the x- and y- components of the
accelerometer biases do not converge, as $\mbf b_{a}$ is indeterminable in
both cases. (3) For the line segment motion, the z-component of $\mbf q_{BC}$
has standard deviations that only slightly tighten over time, similarly to the
y-component of $\mbf q_{BC}$ with the circle motion. Moreover, the x-component
of $\mbf q_{BC}$ converges to a value of some offset with line segment motion.
These effects are not predicted by the observability analysis and are likely
due to that the filter estimating unobservable parameters tends to be trapped
in local minima. (4) For both line segment and circle motion, all three
components of $\mbf p_{BC}$ have non-decreasing errors and standard
deviations. For the lemniscate motion, $\mbf p_{BC}$ converges except for the
component along the rotation axis (z-axis of the IMU frame), concurring with
the analysis results. (5) In contrast to lemniscate and line segment motion,
the time offset with the circle motion converges to a slightly wrong value and
have relatively large variances, pointing to the unobservable property of time
offset with motion of constant local acceleration.

###  6.2 Unobservable Time Offset with Constant Control Inputs

To validate the observability property of the time offset in a VIO system, we
choose four motion types: (a) circular motion with constant local angular
velocity ${\boldsymbol{\omega}}_{WB}^{B}$ and constant local linear velocity
$\mbf v^{B}$, (b) cylindrical motion with constant local angular velocity
${\boldsymbol{\omega}}_{WB}^{B}$ and constant local linear acceleration $\mbf
a^{B}$, (c) circular motion with varying local angular velocity
${\boldsymbol{\omega}}_{WB}^{B}$ and hence varying local linear acceleration
$\mbf a^{B}$, (d) cylindrical motion with constant local angular velocity
${\boldsymbol{\omega}}_{WB}^{B}$ and varying local linear acceleration $\mbf
a^{B}$, shown in Fig. 4. Based on our analysis, it is expected that the time
offset is unobservable for cases a and b, and is observable for cases c and d.
In contrast, according to the linearized model (Table 3), the time offset is
unobservable for case a, and is observable for cases b, c, and d.

Figure 4: Degenerate motion types and the simulated landmarks: (top left)
circular motion with constant local angular and linear velocity, (top right)
cylindrical motion with constant local angular velocity and constant local
linear acceleration, (bottom left) circular motion with varying local angular
velocity and varying local linear acceleration, (bottom right) cylindrical
motion with constant local angular velocity and varying local (vertical)
linear acceleration. They correspond to cases a, b, c, and d in Section 6.2.

The estimated state variables by the OpenVINS filter are

$\\{\mbf p_{WB},\mbf{q}_{WB},\mbf{v}^{W},\mbf{b},t_{d}\\},$ (32)

where the extrinsic parameters are excluded to reduce their side effects.
Before each run of the VIO filter, a simulated time offset was sampled from a
Gaussian distribution $N(0,0.05\enskip\mathrm{sec})$, and deducted from true
timestamps of the camera frames. We repeated the simulation five times for
every trajectory. The estimated time offsets and $3\sigma$ envelopes for five
runs of each degenerate motion are shown in Fig. 5. From the top right plot of
Fig. 5, we see that the time offset is unobservable with constant local
angular rate and constant local linear acceleration, validating that our
identified conditions when the time offset is unobservable are broader than
those identified with the linearized model. The bottom two plots of Fig. 5
show that varying local angular velocity or varying local linear acceleration
will render time offset observable, implying that the constant control inputs
are almost necessary to make the time offset unobservable.

Figure 5: Estimated time offset and $\pm 3\sigma$ envelopes of five runs on
each motion type with different initial time offsets. The true time offset is
0. The motion types are: (top left) circular motion with constant local
angular and linear velocity, (top right) cylindrical motion with constant
local angular velocity and constant local linear acceleration, (bottom left)
circular motion with varying local angular velocity and varying local linear
acceleration, (bottom right) cylindrical motion with constant local angular
velocity and varying local (vertical) linear acceleration. They correspond to
cases a, b, c, and d in Section 6.2.

In summary, the simulation results confirm the analysis findings and show the
effectiveness of the proposed conversion procedures.

##  7 Conclusions and Future Work

Based on the well-grounded observability analysis with Lie derivatives, this
paper investigates the observability of a nonlinear system affine in inputs
when extra constraints on the control inputs or observations are applied. We
present detailed procedures to convert the nonlinear system with constraints
affine in control inputs to a constraint-free standard form which is ready for
automated deduction. By using the proposed procedures, we analyze the
observability properties of a VIO system under constraints on control inputs.
The analysis results about observability of VIO with constraints and time
offset coincide or extend existing reports, and are validated by simulation.
Moreover, the proposed procedures can easily carry over to other sensor fusion
problems, _e.g_., lidar-IMU fusion [24].

## References

  * [1] Goshen-Meskin, D., Bar-Itzhack, I.: Observability analysis of piece-wise constant systems. I. Theory. IEEE Transactions on Aerospace and Electronic Systems 28(4), 1056–1067 (Oct 1992). https://doi.org/10.1109/7.165367
  * [2] Hermann, R., Krener, A.: Nonlinear controllability and observability. IEEE Transactions on Automatic Control 22(5), 728–740 (Oct 1977). https://doi.org/10.1109/TAC.1977.1101601
  * [3] Hesch, J.A., Kottas, D.G., Bowman, S.L., Roumeliotis, S.I.: Camera-IMU-based localization: Observability analysis and consistency improvement. The International Journal of Robotics Research 33(1), 182–201 (Jan 2014). https://doi.org/10.1177/0278364913509675
  * [4] Huai, J., Lin, Y., Zhuang, Y., Toth, C., Chen, D.: Observability analysis and keyframe-based filtering for visual inertial odometry with full self-calibration. IEEE Transactions on Robotics (Jan 2022)
  * [5] Isidori, A.: Nonlinear Control Systems. Springer Science & Business Media (2013)
  * [6] Jung, J.H., Park, C.G.: Constrained filtering-based fusion of images, events, and inertial measurements for pose estimation. In: 2020 IEEE International Conference on Robotics and Automation (ICRA). pp. 644–650 (May 2020). https://doi.org/10.1109/ICRA40945.2020.9197248
  * [7] Kalman, R.E.: On the general theory of control systems. In: Proceedings First International Conference on Automatic Control, Moscow, USSR. pp. 481–492 (1960)
  * [8] Kelly, J., Sukhatme, G.S.: Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. The International Journal of Robotics Research 30(1), 56–79 (2011)
  * [9] Li, H., Stueckler, J.: Visual-inertial odometry with online calibration of velocity-control based kinematic motion models. IEEE Robotics and Automation Letters 7(3), 6415–6422 (Jul 2022). https://doi.org/10.1109/LRA.2022.3169837, conference Name: IEEE Robotics and Automation Letters
  * [10] Li, M., Mourikis, A.I.: High-precision, consistent EKF-based visual-inertial odometry. The International Journal of Robotics Research 32(6), 690–711 (May 2013). https://doi.org/10.1177/0278364913481251
  * [11] Li, M., Mourikis, A.I.: Online temporal calibration for camera–IMU systems: Theory and algorithms. The International Journal of Robotics Research 33(7), 947–964 (2014)
  * [12] Maes, K., Chatzis, M.N., Lombaert, G.: Observability of nonlinear systems with unmeasured inputs. Mechanical Systems and Signal Processing 130, 378–394 (2019)
  * [13] Martinelli, A.: Visual-inertial structure from motion: Observability and resolvability. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 4235–4242. IEEE, Tokyo, Japan (Nov 2013). https://doi.org/10.1109/IROS.2013.6696963
  * [14] Martinelli, A.: Observability: A new theory based on the group of invariance. Advances in Design and Control, Society for Industrial and Applied Mathematics (Jan 2020). https://doi.org/10.1137/1.9781611976250
  * [15] Martinelli, A.: Nonlinear unknown input observability and unknown input reconstruction: The general analytical solution. arXiv:2201.07610 [cs, math] (Jan 2022)
  * [16] Mirzaei, F.M., Roumeliotis, S.I.: A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation. IEEE Transactions on Robotics 24(5), 1143–1156 (Oct 2008). https://doi.org/10.1109/TRO.2008.2004486
  * [17] Solà, J.: Quaternion kinematics for the error-state Kalman filter. Tech. rep., Institut de Robòtica i Informàtica Industrial, Barcelona (Nov 2017)
  * [18] Tang, Y., Wu, Y., Wu, M., Wu, W., Hu, X., Shen, L.: INS/GPS integration: Global observability analysis. IEEE Transactions on Vehicular Technology 58(3), 1129–1142 (Mar 2009). https://doi.org/10.1109/TVT.2008.926213
  * [19] Villaverde, A.F., Evans, N.D., Chappell, M.J., Banga, J.R.: Input-dependent structural identifiability of nonlinear systems. IEEE Control Systems Letters 3(2), 272–277 (Apr 2019). https://doi.org/10.1109/LCSYS.2018.2868608
  * [20] Villaverde, A.F., Tsiantis, N., Banga, J.R.: Full observability and estimation of unknown inputs, states and parameters of nonlinear biological models. Journal of the Royal Society Interface 16(156), 20190043 (2019)
  * [21] Wu, K.J., Guo, C.X., Georgiou, G., Roumeliotis, S.I.: VINS on wheels. In: 2017 IEEE International Conference on Robotics and Automation (ICRA). pp. 5155–5162 (May 2017). https://doi.org/10.1109/ICRA.2017.7989603
  * [22] Yang, Y., Geneva, P., Eckenhoff, K., Huang, G.: Degenerate motion analysis for aided INS with online spatial and temporal sensor calibration. IEEE Robotics and Automation Letters 4(2), 2070–2077 (Apr 2019). https://doi.org/10.1109/LRA.2019.2893803
  * [23] Yang, Y., Geneva, P., Zuo, X., Guoquan, Huang: Online self-calibration for visual-inertial navigation systems: Models, analysis and degeneracy. arXiv:2201.09170 [cs] (Jan 2022)
  * [24] Zuo, X., Yang, Y., Geneva, P., Lv, J., Liu, Y., Huang, G., Pollefeys, M.: LIC-Fusion 2.0: LiDAR-inertial-camera odometry with sliding-window plane-feature tracking. In: 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 5112–5119 (Oct 2020). https://doi.org/10.1109/IROS45743.2020.9340704, iSSN: 2153-0866