NOCT: Nonlinear Observability with Constraints and Time Offset

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.

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].

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.

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$,

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

The zeroth order Lie derivative is defined as

Higher order Lie derivatives can be computed recursively by

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

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

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.

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.

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

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$,

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

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

where the shorthand symbols are

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

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.

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].