An Invariant-EKF VINS Algorithm for Improving Consistency
Teng Zhang Kanzhi Wu Daobilige Su Shoudong Huang Gamini Dissanayake




Image Stitching by Line-guided Local Warping
with Global Similarity Constraint
    Tianzhu Xiang^1, Gui-Song Xia^1, Xiang Bai^2, Liangpei Zhang^1



^1State Key Lab. LIESMARS, Wuhan University, Wuhan, China.

^2Electronic Information School, Huazhong University of Science and Technology, China.

    
=========================================================================================================================================================================================================================








The main contribution of this paper is an invariant extended Kalman filter (EKF) for visual inertial navigation systems (VINS). It is demonstrated that the conventional EKF based VINS is not invariant under the stochastic unobservable transformation, associated with translations and a rotation about the gravitational direction. This can lead to inconsistent state estimates as the estimator does not obey a fundamental property of the physical system. To address this issue, we use a novel uncertainty representation to derive a Right Invariant error extended Kalman filter (RIEKF-VINS) that preserves this invariance property. RIEKF-VINS is then adapted to the multi-state constraint Kalman filter framework to obtain a consistent state estimator.
Both Monte Carlo simulations and real-world experiments are used to validate  the proposed method.








§ INTRODUCTION



Visual-Inertial Navigation Systems (VINS)  have been of significant interest to   the robotics community in the past decade, as
the  fusion of information from a camera and an inertial measurement unit (IMU) provides an effective and affordable  solution for navigation in GPS-denied environment. VINS algorithms can be classified into two categories, namely,  filter based  and  optimization based. Although 
there has been recent progress in the development of optimization based algorithms <cit.><cit.>, the extended Kalman filter (EKF) based solutions are still   extensively used (e.g., <cit.><cit.><cit.><cit.>) mainly as a result of their  efficiency and  simplicity. 

It is well known that conventional  EKF based Simultaneous Localization and Mapping algorithms (EKF-SLAM) <cit.><cit.>  suffer from  inconsistency. Similarly it has been shown that the conventional EKF VINS algorithm (ConEKF-VINS) using  point features in the environment is also inconsistent  resulting in the underestimation of the state uncertainty. This is closely related to the partial observability of these systems because conventional EKF algorithms
do not necessarily guarantee this fundamental property <cit.><cit.> due to the linearized errors, which is the main reason for the overconfident estimates.   
This insight has been a catalyst for a number of  observability-constraint  algorithms  (e.g., <cit.><cit.><cit.>), that explicitly enforces the unobservability of the system along specific directions via  the modifications to the Jacobian matrices. 
Although the observability-constraint  algorithms improve 
the consistency and accuracy of the estimator to some extent <cit.>, extra computations in the update stage are required.
 Bloesch et al. in <cit.>   propose a robot-centric formulation to alleviate the inconsistency. 
 Under the robot-centric formulation, the filter estimates the locations of landmarks in the local frame instead of that in the global frame. As a result,  the system becomes fully observable so that this issue is inherently avoided.
However, this formulation can result in  larger uncertainty and extra computations in the propagation stage, as discussed in <cit.><cit.>.  

Recently,  the manifold and Lie group representations for three-dimensional orientation/pose have been utilized for solving SLAM and VINS. Both  filter based algorithms (e.g., <cit.><cit.><cit.>) and  optimization based algorithms (e.g., <cit.><cit.>) can  benefit from the manifold representation and better accuracy can be achieved. The use of manifold  does not only allow much easier algebraic  computations (e.g., the computation of the Jacobian matrices) and avoid the representation singularity <cit.> but also have inspired a number of researchers to rethink the difference between the state representation and the state uncertainty representation, which is highlighted in  <cit.><cit.>. In fact, this insight is also 
intrinsically understood in  the well-known preintegration visual-inertial algorithm  <cit.> although the algorithm does not use  the manifold representation.
From  the viewpoint of control theory,  Aghannan and Rouchon in <cit.> propose 
a  framework for designing  symmetry-preserving observers on manifolds  by using a subtle geometrically adapted correction term. 
The fusion of the symmetry-preserving theory and EKF has resulted in  the invariant-EKF (I-EKF), which possesses the theoretical
local convergence property <cit.> and preserves the same invariance property of the original system.  I-EKF based observers have been used in the inertial navigation <cit.> and the 2D EKF-SLAM <cit.><cit.>. Our recent work <cit.> also proves the significant improvement in the consistency through  
 a 3D I-EKF SLAM algorithm.  

In this paper,
we argue that 
the absence of the invariance affects the consistency of ConEKF-VINS estimates. There is a correspondence between this and the observability analysis reported in the previous literatures (e.g.,  <cit.><cit.>). The invariance in this 
refers to “the output of the filter is invariant under any stochastic unobservable transformation". For the VINS system, the unobservable transformation is the rotation about the gravitational direction and the translations. 
Adopting the I-EKF framework, we propose the Right Invariant error EKF VINS algorithm (RIEKF-VINS) and prove that it is invariant.
We then integrate RIEKF-VINS into the well-known visual-inertial odometry framework, i.e.,  the multi-state constraint Kalman filter (MSCKF) and remedy the inconsistency of the MSCKF algorithm. 
We  show using extensive Monte Carlo simulations the proposed method outperforms the original MSCKF, especially in terms of the consistency.
A preliminary real-world experiment also demonstrates the improved accuracy of the proposed method.

This paper is organized as follows. Section <ref> recalls the VINS system and gives an introduction of the ConEKF-VINS under the general continuous-discrete EKF. 
 Section <ref> performs the consistency analysis of the general EKF algorithm based  on the invariance theory and proves the absence of the invariance of ConEKF-VINS. 
Section <ref> proposes RIEKF-VINS with the extension to the MSCKF framework. 
Section <ref>  reports both the simulation and experiment results.
 Finally, Section <ref> includes the main conclusions of this work and future work. Appendix provides some necessary formulas used in the proposed algorithms and the proofs of the theorems.


Notations:  Throughout this paper bold lower-case and upper-case letters are reserved for column vectors  and matrices/tuples, respectively. To simplify the presentation, the vector transpose operators are omitted for the case 𝐀=[ 𝐚^⊺, 𝐛^⊺ ,⋯ , 𝐜^⊺ ]^⊺. 
The notation S(· ) denotes the skew symmetric operator that transforms a 3-dimensional vector into a skew symmetric matrix: S(𝐱)𝐲=𝐱×𝐲 for 𝐱, 𝐲∈ℝ^3, where the notation × refers to the cross product.



§ BACKGROUND KNOWLEDGE


In this section, we first provide an overview of the VINS system and then describe the ConEKF-VINS algorithm  based on the framework of the general continuous-discrete EKF.



 §.§ The VINS system

The VINS system is used to estimate the state denoted as the tuple below

    𝐗= ( 𝐑, 𝐯, 𝐩, 𝐛_g, 𝐛_a,  𝐟)

where 𝐑∈𝕊𝕆(3) and 𝐩∈ℝ^3 are the orientation and  position of the IMU sensor, respectively,
𝐯∈ℝ^3 is the IMU velocity expressed in the global frame, 𝐛_g∈ℝ^3 is the gyroscope bias, 𝐛_a∈ℝ^3 is the accelerometer bias and 
 𝐟∈ℝ^3 is the coordinates of the landmark  in the global frame. Note that only one landmark is included    
 the system state (<ref>)   for a more concise notation.  



  §.§.§ The continuous-time motion model

The IMU measurements are usually used for state evolution due to its high frequency. 
The continuous-time motion model of the VINS system is given by the following  ordinary differential equations (ODEs): 

    𝐗̇ = f( 𝐗, 𝐮, 𝐧) 
    
     =   ( 𝐑S(𝐰 - 𝐛_g  -  𝐧_g ), 𝐑(𝐚-𝐛_a - 𝐧_a )+𝐠 , 𝐯, 𝐧_bg , 𝐧_ba  , 0)

where  𝐰∈ℝ^3 is the gyroscope reading,
 𝐚∈ℝ^3 is the  accelerometer reading, 
  𝐠∈𝐑^3 is the global gravity vector (constant),
 and 𝐧= [ 𝐧_g, 𝐧_bg , 𝐧_a , 𝐧_ba ] is  the system noise modeled as a white Gaussian noise with the covariance matrix 𝐐:  𝔼(𝐧 (t)  𝐧(τ)^⊺) =  𝐐δ(t-τ).
 Note that  𝐮= ( 𝐰, 𝐚, 𝐠 ) is the time-varying system input and the IMU noise covariance 𝐐 is a constant matrix as prior knowledge.



  §.§.§ The discrete-time measurement model

The visual measurement as the  system output is discrete due to the low frequency of camera. 
After data association and rectification, 
 the visual measurement of the landmark at time-step k∈ℕ  is available and given by

    𝐳_k = h (𝐗_k, 𝐧_z)    =  𝔥 (  𝐑_k^⊺ (𝐟 -𝐩_k ) ) + 𝐧_z
   
 where 𝐧_z ∼𝒩(0, 𝐕_k) is the measurement noise. Note that
 𝔥 (·) := π∘𝐓_CI, where π denotes the projection function and
     𝐓_CI is the  transformation  from  the IMU frame to the camera frame.  




 §.§ The general continuous-discrete EKF


 Being a natural extension of the standard EKF, the general EKF allows more flexible uncertainty representation by the following:

    𝐗=𝐗̂⊕𝐞 and     𝐞∼𝒩(0, 𝐏 )

 where (𝐗̂, 𝐏) can be  regarded as   the mean estimate and the covariance matrix,   𝐞 is 
a white Gaussian noise vector and the  notation ⊕ is  called retraction in differentiable geometry <cit.>, coupled with the inverse mapping ⊖. 
Note that  the user-defined operators ⊕ and ⊖ need to be designed such that 𝐗=𝐗⊕0 and  𝐞=𝐗⊖𝐗̂. Here we also highlight that the choice of the retraction ⊕ has a significant contribution to the performance of the filter, as discussed in our previous work <cit.>. 

Once determining the retraction ⊕,  the process of the general continuous-discrete EKF
is similar to conventional continuous-discrete EKF, as summarized in  Alg. <ref>.  
For propagation, we first calculate the time-varying Jacobians matrices 𝐅 and 𝐆  from the linearized error-state propagation model:
 
    𝐞̇ = 𝐅𝐞 + 𝐆𝐧 + o(𝐞𝐧).

We then compute the state transition matrix Φ_n:=Φ(t_n+1, t_n) that  is the solution at time t_n+1 of the following ODE:
   
    d/dtΦ (t , t_n ) = 𝐅(t) Φ(t,t_n)

 with the  condition Φ(t_n, t_n) = 𝐈 at time t_n. The matrix 𝐐_d,n can be computed as
 
    𝐐_d,n= ∫_t_n^t_n+1Φ(t_n+1, τ ) 𝐆(τ) 𝐐𝐆^⊺(τ)  Φ^⊺(t_n+1, τ )  dτ.
  





 §.§ ConEKF-VINS


ConEKF-VINS  <cit.> can be regarded as an instance of the general EKF algorithm (Alg. <ref>). 
In ConEKF-VINS,  the uncertainty  representation is  defined as

    𝐗=𝐗̂⊕𝐞
    
    	=    ( 𝐑̂exp( 𝐞_θ ) , 𝐯̂+𝐞_v, 𝐩̂+𝐞_p, 𝐛̂_g+𝐞_bg, 𝐛̂_a+𝐞_ba,  𝐟̂+ 𝐞_f )

where 
𝐞 =  [ 𝐞_θ  , 𝐞_v, 𝐞_p ,𝐞_bg, 𝐞_ba   , 𝐞_f ]∼𝒩(0,𝐏) and exp(·) transforms a 3-dimensional vector into a rotation matrix, given in (<ref>).  The matrices  Φ_n, 𝐐_d,n and 𝐇_n+1 
are omitted here due to space reasons, which can be straightforwardly calculated in the sense of the uncertainty representation (<ref>).
Please  refer to  <cit.> for more details. 




§ CONSISTENCY ANALYSIS



In this section, we first introduce the concepts of unobservable transformation,  invariance and observability. 
We then perform the consistency analysis for the general EKF filter and  prove that ConEKF-VINS  does not have the expected invariance property. Moreover, we also discuss the relationship between invariance and consistency. 






 §.§ Unobservability, unobservable transformation and invariance of the VINS system


The concept observability of nonlinear systems can be traced to the early literature <cit.>.
As discussed in the literatures <cit.><cit.><cit.>,
 the state   (<ref>) of the VINS system is not locally observable. To make it more intuitive, we introduce the unobservability of the VINS system  based on the unobservable transformation rather than   the observability rank criterion  reported in <cit.>. 


The transformation 𝒯 is  called to be an unobservable transformation for the VINS system and 
the output of the VINS system (<ref>)–(<ref>) is invariant under 𝒯 when the following condition is satisfied:
For arbitrary t_i such that 𝐘(t_i)=  𝒯 (𝐗(t_i)), we have 
h(𝐗(t_n), 0 ) = h(𝐘(t_n), 0)  ∀ n ≥ i where the notations 
𝐗(·) and 𝐘(·) denote the two evoluted  trajectories that follow the same ODEs  (<ref>) with the conditions 𝐗(t_i) and 𝐘(t_i) at time t_i, respectively. 
On the other hand, the system is called to be unobservable if there exists an unobservable transformation.



One can see that an unobservable system is always accompanied by an unobservable transformation. And the invariance to   the  unobservable transformation is a more detailed description of the unobservability. 





	
	For the system state (<ref>), a stochastic transformation of translation and rotation (about the gravitational direction) 𝒯_𝐒  is a mapping:
	
    𝒯_𝐒 (𝐗)=( exp(𝐠 ( ϵ_1+θ_1 ))𝐑, 
    	exp(𝐠( ϵ_1+θ_1 ))𝐯,
       exp(𝐠( ϵ_1+θ_1 )) 𝐩+ θ_2+ϵ_2,
       𝐛_g, 𝐛_a,   exp(𝐠( ϵ_1+θ_1 )) 𝐟+ θ_2+ϵ_2     )
	
	where  
	 𝐒= (  θ, ϵ ), θ_1 ∈ℝ, θ_2 ∈ℝ^3, θ= [ θ_1 , θ_2 ]∈ℝ^4, ϵ_1 ∈ℝ, ϵ_2∈ℝ^3 and 
	ϵ= [ ϵ_1 , ϵ_2 ]∈ℝ^4 is a white Gaussian noise with the covariance Σ.
	    𝒯_𝐒 degenerates into the deterministic  transformation 𝒯_𝐃 (𝐃= (  θ, 0 )) under the condition Σ=0. 𝒯_𝐒 degenerates into a stochastic identity transformation under the condition 
	    θ = 0,  .



	
The stochastic transformation 	𝒯_𝐒 is an unobservable transformation to the VINS system (<ref>)–(<ref>). 


It can be straightforwardly verified.

 

 Theorem <ref> corresponds to the conclusion in <cit.><cit.> that the IMU yaw angle and the IMU position  are (locally) unobservable. 




 §.§ The invariance of the general EKF based filter


The general EKF based filter is not a linear system for the estimated state 𝐗̂. However, the invariance of the filter  can be described as the following:

	The output  of a general EKF framework based filter (Alg. <ref>) for the VINS system is  invariant under any stochastic unobservable transformation 𝒯_𝐒 if the  following condition is satisfied:
	  for any  two  estimates (𝐗̂_i, 𝐏_i) and  ( 𝐘̂_i, 𝐏y_i ) at time-step i, where  𝐘̂_i= 𝒯_𝐒 (  𝐗̂_i ) and 𝐏y_i= 𝐌_i 𝐏_i𝐌^⊺_i+ 𝐍_i Σ𝐍^⊺_i in which  
    𝐌_i   :=.∂𝒯_𝐃 (𝐗̂_i ⊕𝐞  ) ⊖𝒯_𝐃 (𝐗̂_i )  /∂𝐞|_𝐞=0

	and
	
    𝐍_i:=.∂𝒯_𝐒 (𝐗̂_i ) ⊖𝒯_𝐃 (𝐗̂_i )  /∂ϵ|_ϵ=0

	we have h(𝐗̂_n,0)=h(𝐘̂_n,0) for all n ≥ i. The notations  
	𝐗̂_n and 𝐘̂_n above represent the mean estimate of this  filter at time-step n
	by using the same input 𝐮 from time t_i to t_n,  from  the  conditions (𝐗̂_i, 𝐏_i) and (𝐘̂_i, 𝐏y_i) at time-step i,  respectively. 


As shown in Def. 1 and Def. 2,  the invariance to any stochastic transformation 𝒯_𝐒 can be divided into two properties: 1) the invariance to any deterministic  transformation 𝒯_𝐃 and 2) the invariance to any stochastic identity transformation.
The following two theorems  analytically provide the methods to judge whether a general EKF based filter has the  two invariances properties above. 



	 
 The output of the general EKF based filter for the VINS system is invariant under any  deterministic unobservable  transformation only if for each  deterministic unobservable  transformation 𝒯_𝐃, there exists an invertible matrix 𝐖_𝐃 (unrelated to 𝐗)  such that
 
    𝒯_𝐃(𝐗⊕𝐞 ) =𝒯_𝐃( 𝐗 ) ⊕𝐖_𝐃𝐞.



	See Appendix <ref>.



		 
	 The output of the general EKF based filter for the VINS system is invariant under any  stochastic identity  transformation only if  
	 
    𝐇_n+i+1Φ_n+iΦ_n+i-1⋯Φ_i𝐍_i = 0∀ n and  i≥ 0.



	See Appendix <ref>.

By using the theorems above, we can easily determine the invariance  properties of ConEKF-VINS. 

 ConEKF-VINS  satisfies (<ref>)  but does not satisfies (<ref>). Hence,  ConEKF-VINS has the invariance to any deterministic unobservable  transformation 𝒯_𝐃  but not the invariance to  stochastic identity  transformations.  In all, the output of  ConEKF-VINS is not invariant under stochastic unobservable transformation 𝒯_𝐒.


For the ConEKF-VINS algorithm,	the invariance  to the deterministic unobservable transformation 𝒯_𝐃 can be verified by using Theorem <ref>. The absence of invariance of  ConEKF-VINS to stochastic identity  transformations can be verified by using Theorem <ref>. More details are omitted here.



The previous literatures <cit.><cit.><cit.><cit.> directly perform the observability analysis of the filter  on the linearized error-state model. 
 However, Theorem <ref> and Theorem <ref>  clarifies the relationship between the  filter and  the linearized error-state model.  





 §.§ Consistency and invariance



The unobservability  in terms of stochastic unobservable transformation 𝒯_𝐒 is a fundamental property of the VINS system. Therefore a consistent filter (as a system for the estimated state 𝐗̂) is expected to mimic this property, i.e.,  the  output of a consistent estimator is invariant under any stochastic unobservable transformation.  The invariance to  to the deterministic transformation 𝒯_𝐃  implies  that the estimates from the filter do not depend on the selection of the (initial)  mean estimate of the unobservable variables, i.e., the IMU yaw angle and the IMU position, essentially.
Similarly, the invariance to stochastic identity transformation implies that the uncertainty w.r.t these unobservable variables does not affect the subsequent  mean estimates.  
We can conclude that the consistency of a filter is tightly coupled with the invariance to stochastic unobservable transformation.  
A  filter that does not have the invariance property will gain the unexpected information and  produce inconsistent (overconfident) estimates.  Note that  ConEKF-VINS is a typical example due to the absence of the invariance property.








§ THE PROPOSED METHOD: 
	 RIEKF-VINS



In this section, we propose RIEKF-VINS by using a new uncertainty representation and prove it has the expected  invariance properties. We then apply  RIEKF-VINS to the MSCKF framework.



 §.§ The Uncertainty representation and Jacobians

RIEKF-VINS also follows the  framework  (Alg. <ref>). 
The uncertainty representation of  RIEKF-VINS is defined as below  

    𝐗=   𝐗̂⊕𝐞
    
    =   ( exp( 𝐞_θ ) 𝐑̂ , exp(𝐞_θ)𝐯̂+J_r(-𝐞_θ)𝐞_v, 
     
       exp(𝐞_θ) 𝐩̂+J_r(-𝐞_θ)𝐞_p,  𝐛̂_g+𝐞_bg, 𝐛̂_a+𝐞_ba, 
       exp(𝐞_θ)   𝐟̂+J_r(-𝐞_θ) 𝐞_f )

where  
𝐞 =  [ 𝐞_θ, 𝐞_v , 𝐞_p , 𝐞_bg, 𝐞_ba , 𝐞_f ]∼𝒩(0,𝐏)  and the right Jacobian operator  J_r(·) is  given in (<ref>). Note that this uncertainty representation intrinsically employs the Lie group so that  the recent result (Theorem 2 of <cit.>) can be used to easily compute the Jacobians 𝐅 and 𝐆 of the propagation

    𝐅 =  [    0_3,3    0_3,3    0_3,3      -𝐑̂    0_3,3    0_3,3;     S(𝐠)    0_3,3    0_3,3 -S(𝐯̂)𝐑̂      -𝐑̂    0_3,3;    0_3,3      𝐈_3    0_3,3 -S(𝐩̂)𝐑̂    0_3,3    0_3,3;    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3;    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3;    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3;          ]
 
and  

    𝐆 =  [      𝐑̂   0_3,3   0_3,3   0_3,3; S(𝐯̂)𝐑̂   0_3,3      𝐑̂   0_3,3; S(𝐩̂)𝐑̂   0_3,3   0_3,3   0_3,3;   0_3,3     𝐈_3   0_3,3   0_3,3;   0_3,3   0_3,3   0_3,3     𝐈_3; S(𝐟̂)𝐑̂   0_3,3   0_3,3   0_3,3;         ].

The measurement Jacobian  is

    𝐇_n+1 = ∂𝔥 ( 𝐟̂_n+1,I) [       0_3,6 -𝐑̂_n+1|n^⊺       0_3,6  𝐑̂_n+1|n^⊺ ]

where 𝐟̂_n+1,I =𝐑̂_n+1|n^⊺(𝐟̂_n+1|n - 𝐩̂_n+1|n )    ∈ℝ^3.



 §.§ Invariance proof

 

The output of RIEKF-VINS  is invariant under any stochastic unobservable transformation 𝒯_𝐒.


For the retraction defined in (<ref>), we have 𝒯_𝐃(𝐗⊕𝐞 ) =𝒯_𝐃( 𝐗 ) ⊕𝐖_𝐃𝐞 ∀ 𝐗 and 𝐞, where 

    𝐖_𝐃 = [       δ𝐑    0_3,3    0_3,3    0_3,3    0_3,3    0_3,3;    0_3,3       δ𝐑    0_3,3    0_3,3    0_3,3    0_3,3; S(θ_2)δ𝐑    0_3,3       δ𝐑    0_3,3    0_3,3    0_3,3;    0_3,3    0_3,3    0_3,3      𝐈_3    0_3,3    0_3,3;    0_3,3    0_3,3    0_3,3    0_3,3      𝐈_3    0_3,3; S(θ_2)δ𝐑    0_3,3    0_3,3    0_3,3    0_3,3       δ𝐑;          ]

and δ𝐑 : = exp(𝐠θ_1). According to Theorem <ref>,  the output of RIEKF-VINS is invariant  under any deterministic transformation  𝒯_𝐃.
On the other hand, for all  i, we have 

    Φ_i = 
    [            𝐈_3              *          0_3,3              *              *          0_3,3;     Δ t_i S(𝐠)              *          0_3,3              *              *          0_3,3; Δ t_i^2/2 S(𝐠)              *            𝐈_3              *              *          0_3,3;          0_3,3              *          0_3,3              *              *          0_3,3;          0_3,3              *          0_3,3              *              *          0_3,3;          0_3,3              *          0_3,3              *              *            𝐈_3;                ]

and 

    𝐍_i =  .∂𝒯_𝐒 (𝐗̂_i ) ⊖𝒯_𝐃 (𝐗̂_i )  /∂ϵ|_ϵ=0 = [     𝐠 0_3,3; 0_3,1 0_3,3; 0_3,1   𝐈_3; 0_3,1 0_3,3; 0_3,1 0_3,3; 0_3,1   𝐈_3 ]

where Δ t_i:= t_i+1-t_i and the elements denoted by the notation * are omitted here because these do not have any contribution to the computation of Φ_i𝐍_i.
Note that Φ_i𝐍_i  = 𝐍_i+1
and 𝐇_i+1𝐍_i+1 = 0
 for all i and then we can easily verify that RIEKF-VINS  satisfies (<ref>). According to Theorem <ref>, the output of RIEKF-VINS is invariant under any stochastic identity transformation.


The observability-constraint filters proposed in  <cit.><cit.><cit.><cit.>  artificially modify the transition matrix Φ_n and the measurement Jacobian 𝐇_n+1 to meet the condition (<ref>) such that they have the invariance to stochastic identity transformation. As a comparison, our proposed RIEKF-VINS employs the uncertainty representation (<ref>) such that 
 the  “natural"  matrices Φ_n and  𝐇_n+1  can elegantly  meet the condition (<ref>). 





 §.§ Application to MSCKF

A drawback of ConEKF-VINS and RIEKF-VINS is the expensive cost of maintaining  the covariance matrix for a number of landmarks. Especially,
RIEKF-VINS suffers from the  complexity quadratic to the number of landmarks in the propagation stage.
On the other hand, the well known MSCKF <cit.> that has the complexity linear to the number of landmarks  inherits the inconsistency of ConEKF-VINS. 
One can see that  the uncertainty w.r.t the global yaw has effects on the mean estimates in the MSCKF algorithm, unexpectedly.
Due to the reasons above,  we integrate RIEKF-VINS into the MSCKF framework such that the modified algorithm has the linear complexity and better consistency.   
For convenience, we call the modified filter as RI-MSCKF.
In this subsection, we do not state all details of RI-MSCKF but  point out the modifications.



  §.§.§ System state and retraction

The system state 𝒳_n at time-step n in RI-MSCKF is 

    𝒳_n= ( 𝐗̅_n, 𝐂_t_1, ⋯, 𝐂_t_j, ⋯,   𝐂_t_k,⋯,𝐂_t_m )

where
𝐗̅_n =  ( 𝐑_n, 𝐯_n, 𝐩_n, 𝐛_g,n, 𝐛_a,n   ) denotes the IMU state at time-step n,
 𝐂_t_i = ( 𝐑^c_t_i, 𝐩^c_t_i  )∈𝕊𝔼(3) denotes the camera pose at the time t_i (t_i < t_n).
According to the IMU state uncertainty in RIEKF-VINS, 
the uncertainty representation of 𝒳_n are defined as below

    𝒳_n    = 𝒳̂_n ⊕𝐞
        = ( 𝐗̂̅̂_n  ⊕_imu𝐞_I, 𝐂̂_t_1⊕_pose𝐞_c^1,⋯,  𝐂̂_t_m⊕_pose𝐞_c^m  )

where 
 𝐞 =  [ 𝐞_I , 𝐞_c ]∈ℝ^15+6m∼𝒩(0,𝐏_n),  𝐞_I∈ℝ^15 and 𝐞_c = [ 𝐞_c^1, ⋯, 𝐞_c^m ]∈ℝ^6m. Note that  ⊕_imu and ⊕_pose are given in Appendix <ref>.  



  §.§.§ Propagation

The mean propagation 𝒳_n+1|n of RI-MSCKF also follows that  of MSCKF while  the covariance 𝐏_n+1|n is calculated by 

    𝐏_n+1|n =Φ̅_n^⊺𝐏_nΦ̅_n+ 𝐐̅_d,n

where Φ̅_n = Diag(Φ^I_n,𝐈_6m ), 𝐐̅_d,n = Diag(𝐐^I_d,n,0_6m,6m ). 
Note that  Φ^I_n and 𝐐^I_d,n are the matrices from the first 15 rows and 15 columns of Φ_n and
𝐐_d,n, respectively, 


where Φ_n and 𝐐_d,n are the matrices of RIEKF-VINS. 




  §.§.§ State augment

Once  a new image is captured at time-step n+1, we augment the system state and the covariance matrix as the following: 

    𝒳̂_n+1|n←  ( 𝒳̂_n+1|n, 𝐂̂_t_n+1  )


    𝐏_n+1|n←[ 𝐈_15+6m;       𝐉 ]𝐏_n+1|n[ 𝐈_15+6m;       𝐉 ]^⊺

where 𝐂̂_t_n+1 =  (  𝐑̂_n+1|nΔ𝐑 , 𝐑̂_n+1|nΔ𝐩+ 𝐩̂_n+1|n       )   ∈𝕊𝔼(3) is
 the mean estimate of camera pose at the time t_n+1, (Δ𝐑, Δ𝐩)∈𝕊𝔼(3)  denotes the transformation from the camera to the IMU. Due to the new uncertainty representation (<ref>), the Jacobian  𝐉 needs to be changed as below
 
    𝐉= [    𝐈_3  0_3,3  0_3,3  0_3,6 0_3,6m;  0_3,3  0_3,3    𝐈_3  0_3,6 0_3,6m ].
      




  §.§.§ Update

Note that the landmark uncertainty  is coupled with the IMU pose in RIEKF-VINS. In RI-MSCKF, 
we  describe the landmark uncertainty coupled with the camera pose 𝐂_t_j that earliest captures the landmark  within the current system state 𝒳_n as below

    (	𝐂̂_t_j, 𝐟̂) ⊕𝐞̅_c^j = (𝐂̂_t_j⊕_pose𝐞_c^j , 𝐞^j_θ𝐟̂+ J_r(- 𝐞^j_θ)𝐞_f  )

where  𝐞̅_c^j=[ 𝐞_c^j ,  𝐞_f ]=  [ 𝐞^j_θ,𝐞^j_p, 𝐞_f  ]∈𝐑^9.
From the uncertainty representations (<ref>) and (<ref>), we can compute the linearized measurement model for the visual measurement at time-step k (t_1 ≤  t_k ≤ t_n). With a slight abuse of notations, the
 linearized measurement model can be represented as below

    π( 𝐑̂^c ⊺_t_k (𝐟̂ - 𝐩̂^c_t_k ) )-𝐳_k   ≈∂π_k 𝐇^*_xk𝐞_n+1|n +∂π_k 𝐇^*_fk𝐞_f+𝐕_k
    𝐳̃_k   ≈∂π_k 𝐇^*_xk𝐞_n+1|n +∂π_k 𝐇^*_fk𝐞_f +𝐕_k
    𝐳̃_k   ≈𝐇_xk𝐞_n+1|n + 𝐇_fk𝐞_f +𝐕_k

where   ∂π_k:= ∂π ( 𝐑̂_t_k^c ⊺ (f̂ -𝐩^c_t_k )  ), 𝐳_k is the measurement captured at the time t_k. Here the matrices  𝐇^*_fk and 𝐇^*_xk  are given by

    𝐇^*_fk  =  𝐑̂^c ⊺_t_k and

 
    𝐇^*_xk    = [ ⋯ ⋯ ⋯ 𝐀 ⋯ 𝐁 ⋯ ⋯ ]

where 𝐀= [ - 𝐑̂^c ⊺_t_k S(𝐟̂),0_3,3 ] and 𝐁= [ 𝐑̂^c ⊺_t_kS(𝐟̂) , - 𝐑̂^c ⊺_t_k ].
Due to the absence of the covariance of landmark, RI-MSCKF also uses the null-space trick on (<ref>) and  the resulting
 residual equation 

    𝐇_fk^⊥𝐳̃_k   ≈𝐇_fk^⊥𝐇_xk𝐞_n+1|n   +𝐇_fk^⊥𝐕_k
    𝐳̃'_k   ≈𝐇'_xk𝐞_n+1|n +  𝐕'_k

 is employed for update.

RI-MSCKF does not need any extra computation to maintain the expected invariance while the observability-constraint algorithms need to explicitly project the  measurement Jacobians onto the observable space. 




§ SIMULATION AND EXPERIMENT





 §.§ Simulation Result


In order to validate the theoretical contributions in this paper, we perform 50 Monte Carlo simulations and compare RI-MSCKF to MSCKF for a Visual-Inertial Odometry (VIO) scenario without loop closure.
 







Consider that a robot equipped with an IMU and a camera moves in a specific trajectory (average speed is 3m/s) with the sufficient 6-DOFs motion, shown as the blue circles in Fig. <ref>. In this environment, 675 landmarks are distributed on the surface of a cylinder with radius 6.5m and height 4m shown as the green stars in Fig. <ref>.
Under the simulated environment, the camera is able to observe sufficiently overlapped landmarks between consecutive frames.
The standard deviation of camera measurement is set as 1.5 pixels.
The IMU noise covariance 𝐐 is set as Diag( 0.008^2𝐈_3,0.0004^2𝐈_3, 0.019^2𝐈_3, 0.05^2𝐈_3 ) (the International System of Units).
 In each round of Monte Carlo simulation, the initial estimate is set as  the ground truth. And the measurements from IMU and camera are generated from the same trajectory with random noises.
 The maximal number of camera poses in the system state  of RI-MSCKF and MSCKF is set as 10. For robust estimation,  we use the landmarks for the update step only when  the landmarks are captured more than 5 times by the cameras within the current system state. 

The results of 50 Monte Carlo simulations are plotted in Fig. <ref>. We use 
the root mean square  error (RMS) and the average normalized estimation error squared (NEES) to evaluate  both accuracy and consistency, respectively. Note that the ideal NEES of orientation is 3 and that of pose is 6. 
As shown in Fig. <ref>, RI-MSCKF clearly outperforms MSCKF especially for the consistency.
This phenomenon  can be explained as  RI-MSCKF has the invariance property to stochastic rotation about the gravitational direction and thus it can reduce the unexpected information gain when compared to MSCKF. In addition, the RMS of orientation and position of both filters increase with the time because the loop closure in this simulation is turned off.  



 §.§ Preliminary Experiment






In order to validate the performance of the proposed RI-MSCKF algorithm under practical environments, we evaluate the algorithm on Euroc dataset <cit.> which is collected on-board a macro aerial vehicle in the indoor environments. Without a delicated designed front-end which handles the feature extraction and tracking perfectly, we selected sequence V2_01_easy in this section to demonstrate the performance of the RI-MSCKF algorithm where the features can be tracked correctly and thus making it perfect to compare our algorithm against the  MSCKF algorithm.


In this preliminary experiment, we designed a front-end based on ORB-SLAM <cit.> while only keeping the feature tracking sub-module. Without knowing the map points, new keyframe is inserted once there is n_frames frames have passed  since the insertion of the last keyframe. One sample image with the tracked landmarks is shown in Fig. <ref>.
 The uncertainty of the IMU sensor is set as instructed in the dataset. The maximal number of the camera poses in the system state is set as 10 and the minimal observed times for a landmark is set as 5. 








Fig. <ref> shows the estimated trajectories using MSCKF and RI-MSCKF. As shown in Fig. <ref> and indicated in Fig.<ref>, RI-MSCKF shows the similar accuracy of position compared with MSCKF but also avoids the drift in the last few frames of the sequence, however,  RI-MSCKF shows significant better results in terms of orientation estimation accuracy compared with the original MSCKF algorithm. 
Even without a robust front-end to handle feature tracking perfectly, this preliminary experiment is able to demonstrate the superiority of RI-MSCKF compared with MSCKF algorithm in terms of the estimation accuracy.














§ CONCLUSION AND FUTURE WORK


In this work,  we proposed the  RIEKF-VINS algorithm and stressed that the consistency of a filter is tightly coupled with the invariance property.
We  proved that RIEKF-VINS has the expected invariance property while ConEKF-VINS does not satisfy  this property.
We also provided the methods to check whether a general EKF based filter has the invariance properties.
After theoretical analysis, we integrated RIEKF-VINS into the MSCKF framework such that the resulting RI-MSCKF algorithm 
can achieve better consistency relative to the original MSCKF.
Monte Carlo simulations  illustrated    the significantly improved performance of RI-MSCKF, especially for the consistency.
The real-world experiments also validated its improved accuracy. Future work includes improving the front end 
to achieve more robust estimation. We will also compare RIEKF-VINS to the observability-constraint algorithms in both simulations and real-world experiments.












 §.§ Some Formulas


  
    exp(𝐲)  =𝐈_3+sin (𝐲 )/𝐲S(𝐲)+ 1-cos(𝐲) /𝐲^2S^2(𝐲)


    J_r(𝐲) =𝐈_3 -1-cos (𝐲 )/𝐲^2S(𝐲)+ 𝐲-sin(𝐲) /𝐲^3S^2(𝐲)

for 𝐲∈ℝ^3. 

The notation ⊕_imu is defined as

    𝐗̅⊕_imu𝐞_I = (exp( 𝐞_θ ) 𝐑 , exp(𝐞_θ)𝐯+J_r(-𝐞_θ)𝐞_v, 
    exp(𝐞_θ) 𝐩+J_r(-𝐞_θ)𝐞_p,  𝐛_g+𝐞_bg,𝐛_a+𝐞_ba )

where 𝐗̅=(𝐑,𝐯,𝐩,𝐛_g,𝐛_a) and 𝐞_I= [ 𝐞_θ,𝐞_v, 𝐞_p, 𝐞_bg, 𝐞_ba ]∈ℝ^15

The notation ⊕_pose is defined as

    𝐂⊕_pose𝐞^i_c = (exp(𝐞^i_θ) 𝐑, exp(𝐞^i_θ) 𝐩+J_r(-𝐞^i_θ)𝐞^i_p  )
  
where 𝐂=(𝐑,𝐩)∈𝕊𝔼(3) and 𝐞^i_c=[ 𝐞^i_θ, 𝐞^i_p ]∈ℝ^6. 
 


 §.§ Proof of Theorem <ref>



Here we only prove the sufficient condition. It is assumed that this filter satisfies:  for each deterministic  unobservable transformation 𝒯_𝐃 there exists 𝐖_𝐃 such that 𝒯_𝐃(𝐗⊕𝐞 ) =𝒯_𝐃( 𝐗 ) ⊕𝐖_𝐃𝐞. 

For any  estimate (𝐗̂_i,𝐏_i) at time-step i, we have another estimate (𝐘̂_i,𝐏y_i)=( 𝒯_𝐃 (  𝐗̂_i),  𝐖_𝐃𝐏_i  𝐖_𝐃^⊺    )  after applying the deterministic transformation 𝒯_𝐃. 
After one step propagation, we have (𝐗̂_i+1|i,𝐏_i+1|i) and (𝐘̂_i+1|i,𝐏y_i+1|i) where 𝐘̂_i+1|i= 𝒯_𝐃 (  𝐗̂_i+1|i )  and 𝐏𝐲_i+1|i=  𝐖_𝐃𝐏_i+1|i𝐖_𝐃^⊺. Note that 𝐇y_i+1 = 𝐇_i+1𝐖_𝐃^-1 and 
then it is easy to obtain 𝐊y=𝐖_𝐃𝐊, resulting in the mean estimate 𝐘̂_i+1  as below

    𝐘̂_i+1   = 𝐘̂_i+1|i⊕𝐊_y 𝐳̃
        =𝒯_𝐃( 𝐗̂_i+1|i  )  ⊕𝐖_𝐃𝐊𝐳̃
       = 𝒯_𝐃 ( 𝐗̂_i+1|i⊕𝐊𝐳̃     ) 
        =  𝒯_𝐃( 𝐗̂_i+1  )


 The covariance matrix after update  becomes 
𝐏y_i+1= (𝐈- 𝐊_y𝐇y_i+1 )𝐏y_i+1|i=    𝐖_𝐃𝐏_i+1𝐖_𝐃^⊺.
In all, 𝐘̂_i+1= 𝒯_𝐃( 𝐗̂_i+1  )  and  𝐏y_i+1= 𝐖_𝐃𝐏_i+1𝐖_𝐃^⊺. By mathematical induction, we can see 
𝐘̂_n= 𝒯_𝐃 (  𝐗̂_n ) for n≥ i and hence 
the output of  this filter  
 is invariant under any deterministic  transformation 𝒯_𝐃.





 §.§ Proof of Theorem <ref>



Here we only prove the sufficient condition.  It is assumed that this filter satisfies:
𝐇_n+i+1Φ_n+iΦ_n+i-1⋯Φ_i𝐍_i = 0∀ n and  i≥ 0. 

For any  estimate (𝐗̂_i,𝐏_i) at time-step i, we have another estimate (𝐘̂_i,𝐏y_i)=(   𝐗̂_i ,  𝐏_i +  𝐍_i Σ𝐍_i^⊺   )  after applying the stochastic identify transformation 𝒯_𝐒 where 𝐒= ( 0, ϵ ) and 
ϵ∼𝒩(0, Σ). After one step propagation, 
we have  (𝐗̂_i+1|i,𝐏_i+1|i) and (𝐘̂_i+1|i,𝐏y_i+1|i)= (𝐗̂_i+1|i,𝐏_i+1|i+ Φ_i  𝐍_i Σ𝐍_i^⊺Φ_i^⊺ ). Note that  𝐇_i+1Φ_i  𝐍_i =0,  we can easily get 
 (𝐘̂_i+1,𝐏y_i+1)= (𝐗̂_i+1,𝐏_i+1+ Φ_i  𝐍_i Σ𝐍_i^⊺Φ_i^⊺     ). By mathematical induction, 
 we have (𝐘̂_n,𝐏y_n)= (𝐗̂_n,𝐏_n+ Φ_n⋯Φ_i  𝐍_i Σ𝐍_i^⊺Φ_i^⊺⋯Φ_n^⊺    ). Therefore,
 the output of this filter is invariant under any stochastic identify transformation.






IEEEtran








