arxiv: v1 [math.oc] 12 May 2018

Transcription

1 Observability Analysis of Aided NS with Heterogeneous Features of Points, Lines and Planes Yulin Yang - yuyang@udeledu uoquan Huang - ghuang@udeledu Department of Mechanical Engineering University of Delaware, Delaware, USA arxiv:855876v mathoc May 8 Abstract n this paper, we perform a thorough observability analysis for linearized inertial navigation systems (NS) aided by exteroceptive range and/or bearing sensors (such as cameras, LiDAR and sonars) with different geometric features (points, lines and planes) While the observability of vision-aided NS (VNS) with point features has been extensively studied in the literature, we analytically show that the general aided NS with point features preserves the same observability property that is, 4 unobservable directions, corresponding to the global yaw and the global position of the sensor platform We further prove that there are at least 5 (and 7) unobservable directions for the linearized aided NS with a single line (and plane) feature; and, for the first time, analytically derive the unobservable subspace for the case of multiple lines/planes Building upon this, we examine the system observability of the linearized aided NS with different combinations of points, lines and planes, and show that, in general, the system preserves at least 4 unobservable directions, while if global measurements are available, as expected, some unobservable directions diminish n particular, when using plane features, we propose to use a minimal, closest point (CP) representation; and we also study in-depth the effects of 5 degenerate motions identified on observability To numerically validate our analysis, we develop and evaluate both EKFbased visual-inertial SLAM and visual-inertial odometry (VO) using heterogeneous geometric features in Monte Carlo simulations ntroduction nertial navigation systems (NS) have been widely used for providing 6 degrees-of-freedom (DOF) pose estimation when navigating in 3D However, due to the noises and biases that corrupt the MU readings, simple integration of the local angular velocity and linear acceleration measurements can cause large drifts in a short period of time, in particular, when using cheap MEMS MUs To mitigate this issue, additional sensors (eg, optical camera, 3, LiDAR4, 5, and imaging sonar6) are often used, ie, aided NS Among possible exteroceptive sensors, optical cameras which are low-cost and energy-efficient while providing rich environmental information are ideal aiding sources for NS and thus, vision-aided NS (ie, VNS) have recently prevailed, in particular, when navigating in PS-denied environments (eg, indoors) 7, 8, 9,,,, 3 While many different VNS algorithms were developed in last decade, the extended Kalman filter (EKF)-based methods are still among the most popular ones, such as multi-state constraint Kalman filter (MSCKF), observability-constrained (OC)-EKF 7, 4, optimal-state constraint (OSC)-EKF 5, and right invariant error (R)-EKF 6 System observability plays an important role in consistent state estimation 7 and thus, significant research efforts have been devoted to the observability analysis of VNS For example, it has been proved in 8 that biases, velocity, and roll and pitch angles in VNS are observable; in 7, 9 the null space of observability matrix (unobservable subspace) of linearized VNS was analytically derived; and in 8, the Lie-derivative based nonlinear observability analysis was presented However, since most of the current VNS algorithms (eg, 7, 8, 9,,, ) are developed based on point features, the observability analysis is performed primarily using point measurements Very few have yet studied the observability properties of the aided NS with heterogeneous geometric features of points, lines and planes that are extracted from range and/or bearing sensor measurements n this paper, building upon our recent conference publications,, we perform a thorough observability analysis for the linearized aided NS using points, lines, planes features and their different combinations n particular, we propose to use the closest point (CP) from the plane to the origin to represent plane features, because this parameterization is not only minimal but also can directly be used for the plane error

2 state update in vector space Moreover, we perform in-depth study to identify 5 degenerate motions, which is of practical significance, as these motions may negatively impact the system observability properties by causing more unobservable directions and thus exacerbate the VNS estimators (eg, see 3, 4) The insights obtained from the observability analysis are leveraged when developing our EKF-based VNS algorithms (including visual-inertial odometry (VO) and visual-inertial SLAM (V-SLAM)) using heterogeneous geometric features, which are evaluated in simulations to validate our analysis n particular, the main contributions of this wor include: n the case of point features, we generalize the VNS observability analysis to encompass any type of aiding sensors (such as 3D LiDAR, D imaging sonar and stereo cameras) and analytically show that the same observability properties remain (ie, 4 unobservable directions) n the case of line (or plane) features, we perform observability analysis for the linearized aided NS with line (or plane) features and show that there exist at least 5 (or 7) unobservable directions for a single line (or plane) feature Moreover, we analytically derive the unobservable subspaces for multiple lines (or planes) in the state vector, without any assumption about features n particular, we advocate to use the closest point (CP) representation, which is simple and compact, for both plane feature parameterization and error state propagation n the case of different combinations of point, line and plane features, we show that in general there are at least 4 unobservable directions that are analytically derived, for the linearized aided NS with heterogeneous features By employing the spherical coordinates for the point feature, we identify 5 degenerate motions that cause the aided NS to have more unobservable directions On the other hand, we study in-depth the effects of global measurements on the system observability, and show that they, as expected, will greatly improve the observability To validate our observability analysis of linearized aided NS, we develop the EKF-based V-SLAM and MSCKF-based VO using heterogeneous geometric features (ie, points, lines, planes, and their different combinations) and perform extensive Monte-Carlo simulations by comparing the standard and the benchmar (ideal) filters Related Wor Aided NS is a classical research topic with significant body of literature 5 and has recently been reemerging in part due to the advancement of sensing and computing technologies n this section, we briefly review the related literature closest to this wor by focusing on the vision-aided NS Aided NS with Points, Lines, and Planes As mentioned earlier, vision-aided NS (VNS) arguably is among the most popular localization methods in particular for resource-constrained sensor platforms such as mobile devices and micro aerial vehicles (MAVs) navigating in PS-denied environments (eg, see 6, 7,, 8) While most current VNS algorithms focus on using point features (eg, 7, 8, 9, ), line and plane features may not be blindly discarded in structured environments 9, 3, 3, 3, 33, 34, 35, 36, 4, in part because: (i) they are ubiquitous and compact in many urban or indoor environments (eg, doors, walls, and stairs), (ii) they can be detected and traced over a relatively long time period, and (iii) they are more robust in texture-less environments compared to point features n the case of utilizing line features, Kottas et al 9 represented the line with a quaternion and a distance scalar and studied the observability properties for linearized VNS with this line parameterization Yu et al 3 proposed a minimal four-parameter representation of line features for VO using rolling-shutter cameras, while Zheng et al 3 used two end points to represent a line and designed point/line VO based on MSCKF Recently, He et al 3 employed the Plücer representation for line parameterization and orthogonal representation 37 for line error states, and developed a tightly-couple eyframe-based inertialaided mono SLAM system

3 n the case of exploiting plane features, uo et al 34 analyzed the observability of VNS using both point and plane features, while assuming the plane orientation is a priori nown The authors have shown that VNS with only plane bearing measurements have unobservable directions but 4 if both point and plane measurements are present Hesch et al 35 developed a D LiDAR-aided NS algorithm that jointly estimates the perpendicular structural planes associated with buildings, along with the MU states However, one particular challenge of using plane features is the plane parameterization 36 A conventional method is to use the plane normal direction and a distance scalar, which, however, is over-parameterized, resulting in singular information matrix in least-squares optimization if not treated carefully Alternatively, one may use a spherical parametrization (two angles and one distance scalar) 38, which is minimal but might suffer from singularities similar to gimbal loc for Euler angles Kaess 36 used a unit quaternion for plane representation by leveraging the quaternion error states for propagation, while Wu et al 4 employed both a quaternion and a distance scalar for planes but assuming D quaternion error states VNS Observability Analysis As system observability is important for consistent estimation 7, in our prior wor 39, 4, 4, 4, 4, 43, 44,, we have been the first to design observability-constrained consistent estimators for robot localization and mapping problems Since then, significant research efforts have been devoted to the observability analysis of VNS n particular, in 45, 46 the system s indistinguishable trajectories were examined from the observability perspective By employing the concept of continuous symmetries as in 47, Martinelli 8 analytically derived the closed-form solution of VNS and identified that MU biases, 3D velocity, global roll and pitch angles are observable He has also examined the effects of degenerate motion 3, minimum available sensors 48, cooperative VO 49 and unnown inputs 5, 5 on the system observability Based on the Lie derivatives and observability matrix ran test 5, Hesch et al 8 analytically showed that the monocular VNS has 4 unobservable directions, ie, the global yaw and the global position of the exteroceptive sensor uo et al 34 extended this method to the RBD-camera aided NS that preserves the same unobservable directions if both point and plane measurements are available With the similar idea, in 53, 54,, the observability of MU-camera (monocular, RBD) calibration was analytically studied, which shows that the extrinsic transformation between the MU and camera is observable given generic motions Additionally, in 55, 56, the system with a downward-looing camera measuring point features from horizontal planes was shown to have the observable global z position of the sensor More importantly, as in practice VNS estimators are built upon the linearized system, it necessitates to perform observability analysis for the linearized VNS whose observability properties can be exploited when designing an estimator For instance, Li et al 57, 9 performed observability analysis for the linearized VNS (without considering biases) and adopted the idea of first-estimates Jacobian 4 to improve filter consistency Analogously, in 58, 7, 9, the authors conducted observability analysis for the linearized VNS with full states (including MU biases) and analytically showed the system unobservable directions by finding the right null space of the observability matrix 7 Based on this analysis, the observabilityconstrained (OC)-VNS algorithm was developed n this wor, we thus primarily focus on the observability analysis of the linearized aided NS with heterogeneous features and developing estimation algorithms to validate it 3 Problem Statement n this section, we describe the system and measurement models for the aided NS with different geometric features, providing the basis for our ensuing observability analysis The state vector of the aided NS contains the current MU state x MU and the feature state x f : x x MU x f q b g V b a P x f () n the above expressions, q is a unit quaternion represents the rotation from the global frame {} to the current MU frame {}, whose corresponding rotation matrix is R( q) b g and b a represent the gyroscope

4 and accelerometer biases, respectively, while V and P denote the current MU velocity and position in the global frame x f denotes the generic features, which can be points, lines, planes or their different combinations 3 MU Propagation Model The MU inematic model is given by 59: q(t) Ω ( ω(t) ) q(t) Ṗ (t) V (t), V (t) a(t) ḃ g (t) n wg (t), ḃ a (t) n wa (t) ẋ f (t) mf () where ω and a are the angular velocity and linear acceleration, respectively n wg and n wa are the zeromean aussian noises driving the gyroscope and accelerometer biases m f is the dimension of x f, and ω ω ω 3 ω Ω(ω) ω, ω ω 3 ω The continuous-time linearized error-state equation is given ω ω by: Fc (t) x(t) 5 mf c (t) x(t) + n(t) : F(t) x(t) + (t)n(t) (3) mf 5 mf mf where F c (t) and c (t) are the continuous-time error-state transition matrix and noise Jacobian matrix, respectively n(t) n g n wg n a nwa are modeled as zero-mean aussian noise with autocorrelation E n(t)n (τ) Q c δ(t τ) Note that n g (t) and n a (t) are the aussian noises contaminating the angular velocity and linear acceleration measurements The discrete-time state transition matrix Φ (+,) from time t to t +, can be derived from Φ (+,) F(t )Φ (+,) with the identity as the initial condition, which is given by 7 (see Appendix A): Φ Φ mf mf 3 Φ (+,) Φ 3 Φ 3 3 Φ 34 3 mf mf (4) 3 Φ 5 Φ 5 Φ 53 Φ 54 3 mf 3 3 mf 3 mf 3 mf 3 mf 3 mf mf where Φ ij is the (i, j) bloc of this matrix, and in particular, Φ 54 will be useful for the ensuring analysis for pure translation and its expression can be analytically given by (see 7, 4) with pure translation: Φ 54 t ts t t τ Rdτd ts R t ts t dτd ts t Rδt (5) where δt t t is the time elapse from the beginning With the state transition matrix (4), we can also analytically or numerically compute the discrete-time noise covariance: 3 Point Measurements Q t+ t Φ (,τ) c (τ)q c c (τ)φ (,τ)dτ (6) Note that point measurements from different exteroceptive sensors (such as monocular/stereo camera, acoustic sonar, and LiDAR) in the aided NS generally can be modeled as range and/or bearing observations which are functions of the relative position of the point feature expressed in the sensor frame Throughout this paper ˆx is used to denote the estimate of a random variable x, while x x ˆx is the error in this estimate m n and n denote m n and n n matrices of zeros, respectively, and n is the identity matrix

5 P f x f y f z f (see ): z (r) z p z (b) with P f R ( P f P ) P f P f + n ( (r) h b P f, n (b)) (7) (8) where h b ( ) is a generic bearing measurement function whose concrete form depends on the particular sensor used (more comprehensive cases can be found in Appendix B) n (7), n (r) and n (b) are zero-mean aussian noises (inferred from sensor raw data) for the range and bearing measurements We then linearize these measurements about the current state estimate: z p H x + H n n z (r) P Pf f x z (b) P Pf f x x + n(r) x + z(b) n (b) n (b) Hr H : f x + n (r) H b H f x + H n n (b) : Hr H b }{{} H proj n (r) H f x + H n n (b) The interested readers are referred to Appendix B for detailed derivations of these Jacobians when using different sensors 33 Line Measurements We propose to use the Plücer representation for the line feature in the state vector but the minimal orthonormal representation for the error state, which was introduced in 37 Specifically, the Plücer representation can be initialized by the two end points P and P of a line segment L, as: P P L nl () P P where n L and v L are the normal and directional vectors (which are not normalized to be unit) for the line L, which clearly is over parameterized for 4 DOF lines A minimal parameterization of the error state is desirable for covariance propagation and update To this end, we have: L n L v L nl n L v L v L v L (9) L n L v L n n L v L v L () Based on this, we define: R L (θ L ) exp( θ L ) w w W L (φ L ) η w w nl n L v L v L n L + v L n L v L n L v L nl v L v L n L () (3) where w n L, w v L and η Since R w L SO(3) and W L SO(), we define the error +w states for these parameters as δθ L and δφ L corresponding to R L and W L, respectively With that, the state vector with the line feature can be written as see (): where L n L x q b g V v L and L δθ L δφ L b a P L Note that in this wor we assume the frame of the aiding exteroceptive sensor coincides with the MU frame in order to eep our analysis concise (4)

6 Table : Different Plane Representations # Models Parameters π p x + π p y + π 3 p z + π 4 π, π, π 3, π 4 n π P f d n π, n π, d 3 cos φ cos θ cos φ sin θ sin φ P f d θ, φ, d 4 p z ap x + bp y + c a, b, c with e 3 n π 5 ap x + bp y + cp z + a, b, c with d 6 q n π +d q, q d 7 Π dn Π x π, y π, z π n particular, the visual line measurement is given by the distance from two ending points x s and x e of line segment to the line in the image (also see our prior wor 6): z l x s l l +l x e l l +l (5) l K n L l l l 3 (6) f K f (7) f c f c f f n L L R R P L (8) v L 3 R where K is the projection matrix for line (not point) features (see Appendix C), with f, f, c and c as the camera intrinsic parameters The relationship (8) is derived based on the geometry P i P + R P i (i, ) see () Moreover, the measurement Jacobian can be computed as follows (Appendix D): where we have used the following identities: 34 Plane Measurements 34 Overview of plane representations H z l l l x : H lh f (9) H l u le l v n le l n () l n u le l v n le l n e x s l, e x e l, l n (l + l ) x s u, v,, xe u, v, Different representations have been developed for plane estimation 6 and we summarize in Table the most commonly used plane parameterizations along with the new representations introduced in this paper Note that p f p x, p y, p z represents the point in the plane Π Model 6 is the most general representation using the homogeneous coordinates (π i, i 4) Model (Hesse form) uses the unit normal direction n π n x, n y, n z and the shortest distance from the origin to the plane d Both models are not minimal, so the information matrix will become singular if directly using them for least-squares optimization Model 3 is similar to the spherical coordinate, which parameterizes a plane with angles (horizontal angle θ and

7 elevation angle φ) and distance d Model 3 is appealing since it is a minimal parameterization while suffering from singularities when φ ± π which is similar to the gimbal loc issue for Euler angles Model 4 63 and Model 5 64 are used under certain conditions Model 6 65, 36 uses a unit 4-dimensional vector for plane representation n 36 it was treated as a unit quaternion and thus quaternion error states to represent the plane error propagation n 4 both quaternion and distance d were used for plane parameterization while the error states contain D quaternion errors and the distance error The error states for both quaternion related plane representations lac physical interpretation Since different representations have their own advantages, some wor has combined these models For example, both Models and 4 are used in 63, and in 64 Model 5 was used for plane fitting and Model 7 for formulating the cost function of plane matching Our recent wor employed Models and 3 for plane and its error state, respectively 34 Closest point (CP) parameterization A plane Π is often represented by the normal direction n π and the shortest distance d from the origin However, we propose to use Model 7, the closest point (CP) of plane to the origin, for our plane parameterization This is due to the facts: (i) it is a minimal representation; and (ii) its error states are in vector space can be interpreted geometrically Note that there is one degenerate case associated with this representation, ie, when d, which, however, can be easily avoided in practice As in practice, we can extract plane features, eg, from 3D point clouds, direct measurements of plane features are given by: z π dn π + n (π) : Π + n (π) () where Π represents the plane in the sensor s local frame and n (π) represents the plane measurement noise To compute the measurement Jacobian of this plane measurement (), note first that the plane parameters (Model ) in the global frame can be transformed to the local frame as: n π d The corresponding closest points (Model 7) have: R 3 P n π d Π d n π ( P n π + d ) R n π n π P R n π + d R n π (3) We now can compute the local-plane measurement Jacobian wrt the plane feature using the chain rule: Π Π Π ñ π ñ π Π + Π d where the pertinent intermedian Jacobians are computed as: d Π Π ñ ˆR (( ˆd ˆn ) π ˆP 3 ˆn ) π ˆP π () (4) (5) L Π d ˆR ˆn π (6) ñ π (ˆn ˆn Π π π 3 ˆn ) πˆn π (7) ˆd d Π xπ y π z π ˆn x π + yπ + zπ π (8) Substitution of the above expressions into (4) yields: ( ( Π ˆR ˆd ˆn ) π ˆP 3 ˆn π ˆP + ˆn π ˆP ˆn ) πˆn π Π ˆd (9)

8 We now compute the measurement Jacobian wrt to the MU states by applying perturbation of δθ and P on (3): Π ˆn π ( ˆP ˆRˆn π + ˆd ˆRˆn π + ˆd ˆn ) π ˆP ˆR ˆn π δθ (3) ˆn π ˆP ˆRˆn π + ˆd ˆRˆn π L ˆR ˆn πˆn π P (3) which immediately provides the desired Jacobians: Π ( δθ ˆd ˆn ) π ˆP ˆR ˆn π (3) Π P ˆR ˆn πˆn π (33) Stacing (9), (3) and (33) yields the complete the measurement Jacobian of the plane measurement wrt the state (): H Π δθ Π 3 9 P Π Π (34) 35 Observability Analysis Observability analysis for the linearized aided NS can be performed in a similar way as in 4, 7 n particular, the observability matrix M(x) is given by: H Φ (,) H Φ (,) M(x) (35) H Φ (,) where H is the measurement Jacobian at time step The unobservable directions span the right null space of this matrix 4 Observability Analysis of Aided NS with Homogeneous Features n this section, we perform observability analysis for the linearized systems of aided NS using homogeneous geometric features including only points, lines and planes; and the observability analysis for aided NS with heterogeneous geometric features will be presented in next section 4 Aided NS with Point Features We first consider the aided NS with point features and conduct the observability analysis in a similar way as in 4, 7 n particular, as the unobservable directions of this aided NS span the right null space of M(x) (35), we compute the measurement Jacobians H (p) based on (7) as follows see (9): H (p) Hr, ˆPf H ˆR ˆR b, }{{} H f, H proj, ˆR Hp where we have used (8) and (9) as well as the following matrix: ( ) H p ˆPf ˆP ˆR (36) (37)

9 Specifically, for each bloc row of M(x) see (35), we have: where H (p) Φ (,) H proj, ˆR Γ Γ Γ 3 Γ Γ Γ ( ˆPf ˆP ˆV δt + ) g(δt ) ( ˆPf ˆP ) ˆR Φ Φ 5 ˆR (38) Γ 3 3 δt, Γ 4 Φ 54 (39) where g g and g,, g, Note that for the analysis purpose, we assume that in computing different Jacobians the linearization points for the same state variables remain the same By inspection, it is not difficult to see that the null space of M(x) in this case is given by: where N g is defined by: N g N g 3 N ˆP g 3 : N r N p ˆPf g 3 ( ˆR g) 3 (4) ( ˆV g) 3 (4) t is interesting to notice that in (4), N p corresponds to the sensor s global translation, while N r relates to the global rotation around the gravity direction We thus see that the system has at least 4 unobservable directions (N p and N r ) Moreover, in analogy to 4, 8,, we have further performed the nonlinear observability analysis based on Lie derivatives 5 for the continuous-time nonlinear aided NS, which is summarized as follows: Lemma 4 The continuous-time nonlinear aided NS with point features (detected from generic range and/or bearing measurements), has 4 unobservable directions Proof See Appendix M 4 Aided NS with Line Features When navigating in structured environments, line features might be ubiquitous and should be exploited in the aided NS to improve performance n the following, we perform observability analysis for the aided NS with line features to provide insights for building consistent estimators 4 Single Line With the line measurements (5), the measurement Jacobian is computed by (see (4) and Appendix D): H (l) H l, K ˆR Hl 3 9 ˆv L H l H l3 }{{} H f, (4) where we have employed the following identities: ( H l ˆn L ˆP ˆv L ) ˆR (43) H l ˆn L ˆP ˆv L (44) ( w H l3 ˆn L + w ˆP ˆv ) L (45) w w

10 With this, the bloc row of the observability matrix M(x) (35) at time step can be written as: H (l) Φ (,) H l, K ˆR Γl Γ l ˆv L δt Γ l3 ˆv L Γ l4 Γ l5 where (46) Γ l ( ˆn L ˆP ˆv L + ˆv L ˆP + ˆv L ˆV δt ˆv L ĝ δt ˆv L ˆP ) ˆR ( Γ l ˆn L ˆP ˆv L ) ˆR Φ + ˆv L Φ 5 Γ l3 ˆv L Φ 54, Γ l4 H l, Γ l5 H l3 Therefore, we have the following result: Lemma 4 The aided NS with a single line feature has at least 5 unobservable directions denoted by N l : N g 3 N v N l ˆP g L ˆR 3 w g ˆv w e e 3 : N l N l:5 (47) η we 3 where ˆn e and ˆv e are the normalized unit vectors of ˆn L and ˆv L, respectively, and N v and L ˆR are defined by: Proof See Appendix E N v 3 ˆv 3 e 3 (48) ˆR L ˆn ˆv e e ˆn e ˆv e (49) t is not difficult to see that N l relates to the sensor rotation around the gravitational direction, N l:4 associates with the sensor s global translation, and N l5 corresponds to the sensor motion along the line direction Note also that the above analysis is based on the projective line measurement model (5) Additionally, in Appendix F, we have also considered the direct line measurement model, for example, by extracting lines from point clouds, and show that the same unobservable subspace N l (47) holds 4 Multiple Lines We extend the analysis to the case where l > general, unparallel lines are included in the state vector To this end, by noting that the rotation between line i and line j (i, j {, l}) is given by: Li L j ˆR Li ˆR Lj ˆR, we can prove that in this case there are 4 unobservable directions Lemma 43 The aided NS with l > general, unparallel line features has at least 4 unobservable directions: N g 3 ˆP g Li ˆR w g, ˆv w, e e L L i ˆR N L η w,e 3 L L i ˆR : N L N L:4 (5) w g l, ˆv w l, el e L l L i ˆR ηl w l, e 3 L l L i ˆR where η i, w,i, w,i and ˆv ei are the parameters related to line i (i { l}) see (3) Proof See Appendix We want to point out again that the above Lemma 43 holds under the assumption that not all the lines are parallel; if all parallel lines, the system will have one more unobservable direction, which coincides with the line direction ˆv e

11 43 Aided NS with Plane Features Now we perform observability analysis of the aided NS with plane features that are important geometric features commonly seen in structured environments n particular, our analysis is based on the CP parameterization of plane features 43 Single Plane We first consider a single plane feature Π included in the state vector: H (π) x q b g V b a p Π Based on the plane measurements (), we can compute the Jacobians as see (9), (3) and (33): ˆR Hπ ˆn πˆn π H π where: ( H π ˆd ˆn ) π ˆP ˆn π ˆR ( ˆd ˆn ) π ˆP 3 ˆn π ˆP + ˆn π ˆn ˆP πˆn π H π ˆd The bloc row of the observability matrix is computed by: where H (π) Φ (,) ˆR Γπ Γ π ˆn πˆn π δt Γ π3 ˆn πˆn π Γ π4 Γ π ( d ˆn ) π ˆP ˆn π ˆn πˆn π ˆP + ˆV δt gδt ˆP ˆR (56) ( Γ π ˆd ˆn ) π ˆP ˆn π ˆR Φ ˆn πˆn π Φ 5 (57) Γ π3 ˆn πˆn π Φ 54 Γ π4 H π With that, we have the following result: Lemma 44 The aided NS with a single plane feature has at least 7 unobservable directions: N g 3 N 3 N π ˆP g Π ˆR 3 ˆΠ g ˆn : N π N π:4 N π5:7 π e 3 3 n above expression, given ˆn and ˆn that are the unit vector orthonormal to each other and perpendicular to ˆn π, we have defined N 3 and the plane orientation ΠR as follows: 3 ˆR ˆn 3 π N ˆn ˆn 3 (6) (5) (5) (53) (54) (55) (58) (59) (6) Proof See Appendix H ˆR Π ˆn ˆn ˆn π (6) Note that as compared to 34 where it was shown that the VNS with bearing measurements to planes has unobservable directions, we analytically show here that, given the direct plane measurements (), the aided NS with with a single plane feature has at least 7 unobservable directions: (i) N π that relates to the rotation around the gravity, (ii) N π:4 that associate with the position of the sensor platform, (iii) N π5:6 that correspond to the motions parallel to the plane, and (iv) N π7 that corresponds to the rotation around the plane normal direction This analysis is directly verified by numerical simulation results (see Fig 3)

12 43 Multiple Planes Assuming that there are s > plane features in the state vector, we first note that the rotation between plane i and plane j (i, j {, s}) is given by: Πi Π j ˆR Πi ˆR Πj ˆR We then can prove the following result: Lemma 45 The aided NS system with s > plane features in the state vector has the following unobservable directions: N g N i j ˆP g Πi ˆR 3 N Π ˆΠ g ˆn π e 3 Π Π i ˆR 3 : N Π N Π:4 N Π5 (63) ˆΠs g ˆn πs e 3 Πs Π i ˆR 3 where n πi is the normal direction vector for plane i (i, j { s}) and N i j is defined by: N i j 6 ( ˆn π i ˆn ) π j 3 (64) Depending on the number of planes and their properties, we have the following observatioins: f s and the planes are not parallel, the system will have at least 5 unobservable directions given by N Π:5 f s 3 and these planes intersections are not parallel, the system will have at least 4 unobservable directions given by N Π:4 Proof See Appendix Up to this point, we have shown from the system observability perspective that the minimal CP representation is an appealing parameterization in part because it preserves the observability properties for aided NS with plane features Note that if all the planes are parallel, the linearized system will have three more unobservable directions corresponding to the motion perpendicular to the plane normal direction and rotation around the normal direction f all the planes intersections are parallel, then the motion along the plane intersection direction is unobservable For completeness and comparison, in Appendix J we also present the observability analysis for the aided NS with plane feature using the Hesse form (ie, Model in Table ) 5 Observability Analysis for Aided NS with Heterogeneous Features n this section, we study the observability properties for the aided NS with different combinations of geometrical features including points, lines and planes To eep presentation concise, in the following we first consider that case of one feature of each type included in the state vector, and then extend to the general case of multiple heterogeneous features 5 Point and Line Measurements Consider a point feature P f and a line feature L in the state vector, yielding x f P f L see () f the sensor measures both the point and line features, we have the following measurement model see (7) and (5): zp z pl (65) z l

13 For observability analysis, we compute the measurement Jacobians wrt the point and line feature as see (36) and (4): H (pl) Hproj, 3 ˆR 3 Hp 3 H l, K ˆR H l 3 9 ˆv (66) L 3 H l H l3 The -th bloc row of the observability matrix M(x) becomes: H (pl) Hproj, Φ (,) 3 ˆR 3 Γ Γ Γ 3 Γ H l, K 3 ˆR Γ l Γ l ˆv L δt Γ l3 ˆv L 3 Γ l4 Γ l5 (67) t is not difficult to find the unobservable directions as: N g 3 ˆP g 3 N pl ˆPf g 3 w g ˆv w e e L ˆR : N pl N pl:4 η we 3 L ˆR Clearly, for the aided NS with combination features of point and line, there are also at least 4 unobservable directions: one is N pl which relates the rotation around the gravity direction, and the other three are N pl:4 which correspond to the global position of the sensor platform Moreover, we can readily extend to the case of multiple points and lines Assuming m points and l lines in the state vector, we can show that there are also at least 4 unobservable directions: N g 3 ˆP g 3 ˆPf g 3 ˆPfm g 3 N P L w g, ˆv w, e e L ˆR ηw,e 3 L ˆR : N P L N P L:4 (69) w g l, ˆv w l, el e L l ˆR ηl w l, e 3 L l ˆR 5 Point and Plane Measurements Consider the case where we have a point and a plane in the state vector (), and thus the feature state x f P f Π Therefore, in this case, the measurement model consists of the point measurement and plane measurement see (7) and (): zp z pπ (7) The measurement Jacobian is computed as see (36) and (34): H (pπ) Hproj, 3 ˆR 3 Hp ˆR H π 3 9 ˆn πˆn π 3 H π The -th bloc row of the observability matrix M(x) is: H (pπ) Hproj, Φ (,) 3 ˆR 3 Γ Γ Γ 3 Γ ˆR Γ π Γ π ˆn πˆn π δt Γ π3 ˆn πˆn π 3 Γ π4 (7) z π (68) (7)

14 where Γ i, Γ πi, i 4, are the same as in the previous sections The unobservable directions can hence be found as: N g 3 N pπ ˆP g 3 ˆPf g 3 : N pπ N pπ:4 (73) ˆΠ g ˆn Π e 3 Π ˆR Clearly, in this case, the unobservable directions of the aided NS consist of the rotation around the gravity direction N pπ, and the global position of the sensor platform N pπ:4 Similarly, we can extend this analysis to multiple features iven m points and s planes in the state vector, the null space of the observability matrix can be obtained as follows: N g 3 ˆP g 3 ˆPf g 3 N P Π ˆPfm g 3 Π g Π s g 53 Line and Plane Measurements ˆn π e 3 Π ˆn πs e 3 Πs : N P Π N P Π:4 ˆR ˆR Now consider the case where a line and a plane is in the state vector, ie, x f L Π The measurement model is given by see (5) and (): zl z lπ z π (75) (74) The measurement Jacobian becomes see (4) and (34): H (lπ) Hl, K 3 ˆR 3 Hl 3 9 ˆv L H l H l ˆR H π 3 9 ˆn πˆn π 3 3 H π (76) The -th bloc row of the observability matrix M(x) is: H (lπ) Φ (,) Hl, K Based on that, we have the following result: Γl ˆR 3 Γ l ˆv L δt Γ l3 ˆv L Γ l4 Γ l5 3 3 ˆR Γ π Γ π ˆn πˆn π δt Γ π3 ˆn πˆn π 3 3 Γ π4 (77) Lemma 5 The aided NS with a single line and plane feature generally has the following unobservable directions: N g 3 N v ˆP g 3 3 N lπ w g ˆv w e e 3 η we 3 L ˆR : N lπ N lπ:4 N lπ5 (78) ˆΠ g ˆn π e 3 Π ˆR 3 n particular, depending on the feature properties, we have:

15 f the line is parallel to the plane, the linearized system will have at least 5 unobservable directions given by N lπ:5 f the line is not parallel to the plane, the linearized system will have at least 4 unobservable directions given by N lπ:4 Proof See Appendix K Similarly, we can extend the analysis to the case of multiple lines and planes iven l lines and s planes, the unobservable directions fo the aided NS with general, unparallel lines and planes are given by: N g 3 ˆP g 3 w g, ˆv w, e e L ˆR η w,e 3 L ˆR N LΠ w g l, ˆv w l, el e L l ˆR : N LΠ N LΠ:4 (79) η l w l, e 3 L l ˆR Π g ˆn π e 3 Π ˆR Π s g ˆn πs e 3 Πs ˆR 54 Point, Line and Plane Measurements Lastly, let us consider the case where all three types of features (a single point, line, and plane) are in in the state vector, ie, x f P f L Π The measurement model becomes see (7), (5) and (): z plπ The measurement Jacobian can be computed as see (36), (4) and (34): H (plπ) H proj, z p z l z π (8) ˆR 3 3 H p H l, K ˆR 3 H l 3 9 ˆv L 3 H l H l ˆR H π 3 9 ˆn (8) πˆn π H π The -th bloc row of the observability matrix M(x) is: H proj, ˆR 3 3 H (plπ) Φ (,) 3 H l, K ˆR ˆR Γ Γ Γ 3 Γ (8) Γ l Γ l ˆv L δt Γ l3 ˆv L 3 Γ l4 Γ l5 3 Γ π Γ π ˆn πˆn π δt Γ π3 ˆn πˆn π Γ π4 The observability properties of this aided NS are given by:

16 Lemma 5 The aided NS with one point, one line and one plane feature in the state vector, has at least 4 unobservable directions: N g 3 ˆP g 3 ˆPf g 3 N plπ w g ˆv w e e L ˆR : N plπ N plπ:4 (83) η we 3 L ˆR ˆΠ g ˆn π e 3 Π ˆR Proof See Appendix L We also extend this analysis to multiple points, lines and planes iven m points, l lines and s planes for the estimation, the unobservable directions can be found as follows: N g 3 ˆP g 3 ˆPf g 3 ˆPfm g 3 w g, ˆv w, e e L ˆR ηw,e 3 L ˆR N P LΠ : N P LΠ N P LΠ:4 (84) w g l, ˆv w l, el e L l ˆR ηl w l, e 3 L l ˆR Π g ˆn π e 3 Π ˆR Π s g ˆn πs e 3 Πs ˆR 6 Observability Analysis of Aided NS with lobal Measurements Aided NS may also have access to (partially) global measurements provided by, for example, PS receivers, sun/star sensors, barometers and compasses ntuitively, such measurements would alter the system observability properties, even if only partial (not full 6 DOF pose) information is available n this section, we systematically examine the impacts of such measurements on the system observability 6 lobal Position Measurements We consider the case where besides the point, line and plane measurements, global position measurements are also available from, for example, a PS receiver or a barometer n the following, we use such a global measurement individually along x, y and z-axis 6 lobal x Measurement f sensor s translation along x direction is nown, then the additional global x-axis measurement is given by z (x) e P The measurement Jacobian and the bloc row of observability matrix can be computed as see (8): H Φ (,) H (plπ) Φ (,) H (x) Φ (,) (85)

17 where H (x) is the global x measurement Jacobian, yielding: H (x) Φ (,) e We can show that the unobservable subspace becomes: A x A N x x w ˆv w e e L ˆRA x η we 3 L ˆRA ˆn x π e 3 Π ˆRA x (86) (87) where A x As compared to N in (83) without global x measurements, both the global translation in x direction and the rotation around the gravity direction become observable 6 lobal y Measurement f sensor s translation along y direction is nown, then the additional global y-axis measurement is given by z (y) e P The measurement Jacobian and the bloc row of observability matrix can be computed as see (8): H Φ (,) H (plπ) Φ (,) H (y) Φ (,) (88) where H (y) is the global y measurement Jacobian, yielding: H (y) Φ (,) e We can show that the unobservable subspace becomes: N y w A y A y w ˆv e e L ˆRA y η w e 3 L ˆRA y ˆn π e 3 Π ˆRA y (89) (9) where A y As compared to N in (83) without global y measurements, both the global translation in y direction and the rotation around the gravity direction become observable 63 lobal z Measurement Proceeding similarly, if the global translation in z direction is directly measured, eg, by a barometer, we have an additional global z-axis measurement z (z) e 3 P n this case, the bloc row of the observability matrix becomes: H Φ (,) H (plπ) Φ (,) H (z) Φ (,) (9)

18 Since e 3 is parallel to g, we have e 3 P g Therefore, the system s unobservable directions become: N g ˆP g A z ˆPf g A z N z w g ˆv w e e L ˆRA (9) z η we 3 L ˆRA z ˆΠ g ˆn π e 3 Π ˆRA z where A z Clearly, only translation in z becomes observable, while, different from the previous case of the global x or y measurements, the rotation around the gravity direction remains unobservable 6 lobal Orientation Measurements We here consider the case where the aided NS has access to global orientation measurements, for example, provided by a sun sensor, or a magnetic compass, or by detecting a plane with nown orientation 34, 55: z (n) N n R N n n this case, the Jacobian and the bloc row of the observability matrix can be computed as: H Φ (,) H (plπ) Φ (,) H (n) Φ (,) (93) where H (n) is the orientation measurement Jacobian, yielding: H (n) Φ (,) ˆR N n ˆR Γ5 3 9 (94) where Γ 5 N n ˆR Φ f N n is not parallel to g, ie, N g, the rotation around the gravity direction becomes observable, and the unobservable directions are: 3 3 N n 3 w ˆv w e e L ˆR η we 3 L ˆR ˆn π e 3 Π ˆR n summary, as expected, the global measurements will mae the aided NS more observable n particular, if a global full position measurements by PS or a prior map are available, the system will become fully observable, while global orientation measurements can mae the rotation around gravitational direction observable, as long as this orientation is not parallel to the direction of gravity 7 Analysis of Degenerate Motion While Wu et al 4 have recently reported that pure translation and constant acceleration are degenerate for monocular VNS with point features, in this section, we here perform a comprehensive study of degeneration motion for the aided NS with heterogeneous features of points, lines and planes, which is important to identify in order to eep estimators healthy n particular, to ease our analysis, we use the range and bearing parameterization (ie, spherical coordinates) of a point feature, instead of its conventional 3D position: r f cos θ cos φ x f : θ r f b f r f sin θ cos φ φ sin φ x f y f z f (95) : P f (96)

19 where r f is the range, θ and φ are the horizontal and elevation angle of the point n this case (point features), the bloc row of the observability matrix can be computed as see (38): where H (p) Φ (,) H proj, ˆR Γ Γ Γ 3 Γ 4 3 ˆbf ˆr f cos ˆφˆb ˆr f ˆb (97) ˆb sin ˆθ cos ˆθ ˆb cos ˆθ sin ˆφ sin ˆθ sin ˆφ cos ˆφ By inspection, the unobservable directions can be found as: N g 3 ˆP g 3 ˆb N rb f : N (ˆb g ) rb,r N rb,p ˆr f cos ˆφ (ˆb ) ˆr f (98) (99) () where N rb,p and N rb,r are the unobservable directions associated with the global translation and the global rotation around the gravity direction, which, as expected, agrees with the preceding analysis (4) 7 Pure Translation Based the above analysis of point measurements, we show that given point, line and plane measurements (8), if the sensor undergoes pure translation, the system gains the following additional unobservable directions (by noting that the state vector () includes the MU state, one point in spherical coordinates (96), one line and one plane): ˆR 3 ˆV N R ˆR g ˆP Θ 3 3 Π where Θ f cos ˆθ tan ˆφ sin ˆθ tan ˆφ Similar to 4, this unobservable direction can be easily verified ˆr f sin ˆθ cos ˆθ see (8): H (p) H (plπ) Φ (,) N R Φ (,) N R H Φ (,) N R () H (π) Φ (,) N R Specifically, using (5) and (39), we have this useful identity: Γ ˆR 4 δt 3 Φ ˆR 54 δt 3 With this, we can easily verify each bloc row of () as follows: ( H (p) Φ (,) N R H proj, ˆR Γ ˆR 4 ) δt 3 g H (l) Φ (,) N R H l, ˆR ˆv L H (π) Φ (,) N R ˆR ˆn πˆn π () ( Φ ˆR 54 ) δt 3 g ˆv L ˆP ˆP ˆv L + ˆP ˆv L ( Φ 54 ˆR + δt 3 ) g

20 where we have also employed the identities: (a b) ba ab and a b ba a b We see from Θ that its first row corresponding to the range of the point feature see (96) are all zeros and thus this unobservable direction () relates to the bearing of the feature Note also that the global rotation of the sensor becomes unobservable, rather than only the global yaw is unobservable for general motions see () and (83) t is important to notice that no assumption is made about the type of sensors used, and thus, the aided NS with generic sensors (not including global sensors) with pure translation will all gain additional unobservable directions of N R 7 Constant Acceleration As it is not straightforward to have direct plane measurements () for NS aided by a monocular camera, to ease our analysis of VNS, from now on we focus on the point and line measurements (65) n particular, if the camera moves with constant local acceleration, ie, a is constant, then the system will have one more unobservable direction given by: N a 6 ˆV â ˆP ˆr f e 3 w w (3) ˆb Since a monocular amera provides only bearing measurements, H proj, H b,,, where ˆb i, ˆb, (i, ) are orthogonal to ˆb f (see Appendix B) n this case, we have: H (p) H Φ (,) N a Φ (,) N a H (l) (4) Φ (,) N a which can be verified by using the identity shown in 4: Γ 4 a ˆP ˆP ˆV δt : ) H (p) Φ (,) N a H b, ˆR ( ˆV δt Γ 4â ˆP + r f ˆbf ( ) H b, ˆR ˆPf ˆP H (9) b, ˆPf (5) ( H (l) Φ (,) N a H l, K ˆR ˆv L ˆP + w ˆn w L + ˆP ˆv ) L w w H l, K (),(6) ˆRˆn L (6) Note that this unobservable subspace (3) relates only to the scale as its nonzeros all appear on the scalesensitive states f using a sensor that can provide the scale (such as stereo and RBD cameras), this unobservable direction will vanish 73 Pure Rotation f the sensor has only rotational motion, then P 3 For monocular-camera based point and line measurements (65), the system will gain the following extra unobservable directions corresponding to the feature scale: 5 e N s 3 (7) H (p) H Φ (,) N s Φ (,) N s H (l) (8) Φ (,) N s which can be seen as follows see (5) and (6): H (p) Hb, Φ (,) N s ˆRˆbf H (l) Φ (,) N s w w H l, K ˆRˆn L (9) ()

21 Note that the first row of N s relates to the scale of the point feature (range r f ), and the second row to the scale of the line (the shortest distance from the origin to the line), which implies that we have more unobservable directions related to the feature scales 74 Moving Towards Point Feature With the point and line measurements (65), if the camera moving towards the point feature, the system will gain one more unobservable direction related to the point scale (range): N 5 e 3 () This degenerate motion indicates that the sensor is moving along the direction of the point feature s bearing direction, that is: P α b f, where α denotes the scale of the sensor s motion Then, we can arrive at: P f r f b f R ( r f α ) b f () Similar to the case of pure rotation, we can verify the additional unobservable direction N as follows see (5): H (p) H Φ (,) N Φ (,) N ˆrf H (l) ˆr f α H b, ˆbf (3) Φ (,) N 75 Moving in Parallel to Line Feature Similarly, if the camera is moving in parallel to the line feature, the system will also gain one more unobservable direction related to this line feature s scale (distance): N (4) This degenerate motion indicates that the sensor is moving parallel to the line direction, that is, P v L Then, we have the following verification see (6): H (p) H Φ (,) N Φ (,) N H (l) Φ (,) N w w H l, K (5) ˆRˆn L 8 Monte Carlo Simulations To validate our observability analysis of aided NS using heterogeneous geometric features, we perform extensive Monte Carlo simulations of vision-aided NS: 3 (i) visual-inertial SLAM (V-SLAM), and (ii) visualinertial odometry (VO), which are among the most popular localization technologies in part due to their ubiquitous availability and complementary sensing modality To this end, we have adapted both the EKFbased V-SLAM and MSCKF-based VO algorithms to fuse measurements of points, lines, planes and their different combinations To the best of our nowledge, algorithmically, we, for the first time, introduce and evaluate the EKF-based V-SLAM/VO approaches with heterogeneous features (which are common in structured environments) n particular, we have compared two different EKFs in both V-SLAM and VO: (i) the ideal EKF that uses true states as the linearization points in computing filter Jacobians and has been shown to have correct observability properties and expected to be consistent, thus being used as the benchmar in simulations as in the literature (eg, 39, 4,, 7, 8); and (ii) the standard EKF that uses current state estimates as the linearization points in computing filter Jacobians and has been shown to be overconfident (inconsistent), 7, 8 The metrics used to evaluate estimation performance are the root mean squared error (RMSE) and the average normalized (state) estimation error squared (NEES) 66 The RMSE provides a measure of accuracy, while the NEES is a standard criterion for evaluating estimator consistency, which (implicitly) indicates the correctness of the EKF system observability 3 Note that similar results as presented in this section would be expected if other aiding sensors are used, for example, acoustic-aided NS was developed in our recent wor 6

22 The simulated trajectories and different geometric features are shown in the left of Figs and, where we simulate a camera/mu sensor suite is moving on the sinusoidal trajectories to collect measurements to different features For the results in Fig, we developed the EKF-based V-SLAM algorithm, which simultaneously preforms visual-inertial localization and mapping by eeping different features in the state vector n contrast, for the results in Fig, we adapated the MSCKF-based VO, which estimates only the sensor poses while marginalizing out different (not only points) features with null space operation t is clear from these results of both V-SLAM and VO in Figs and that the standard EKF/MSCKF performs worse than the benchmar ideal filter, which agrees with the literature (with point features only), 7, 8 This again reflects the importance of system observability for consistent state estimation Moreover, in order to directly validate the unobservable subspace of the aided NS found in our analysis, using the same simulation setup as above but with a single feature, we have constructed the observability matrix of the ideal EKF-based V-SLAM with a single point (or line or plane) and numerically computed the dimension of its null space, which is shown in Fig 3 Clearly, the dimension of the unobservable subspace for the (ideal) V-SLAM with a single point (line or plane) is 4 (5 or 7), which agrees with our analysis 9 Conclusions and Future Wor n this paper, we have performed observability analysis for aided NS with different geometric features including points, lines and planes, which are detected from generic range and/or bearing measurements encompassing vision-aided NS (VNS) as a special case As in practice, most aided-ns estimators are built based on the linearized systems, whose observability properties directly impact the estimation performance, this wor has primarily focused on observability analysis of the linearized aided NS with points, lines, planes and their combinations n particular, in the case of point features, we analytically show that the aided NS (both linearized and nonlinear) using generic range and/or bearing measurements has 4 unobservable directions n the case of lines (planes), we prove that there exist at least 5 (7) unobservable directions with a single line (plane) feature, and for the first time, analytically derived the unobservable directions for multiple lines and planes We have generalized this observability analysis for linearized aided NS with different combinations of point, line and plane features and summarized important results in Table Based on this analysis, we have also systematically investigated the effects of global measurements on the observability of aided NS, and found, as expected, that global information improves the system observability Moreover, we have identified comprehensively 5 types of degenerate motions that negatively impact the system observability and should be avoided if possible (otherwise, extra sensors may be needed) Additionally, it should be noted that, during our analysis, we have employed different point representation (Euclidean and spherical coordinates), different plane representation (CP and Hesse form) and different line measurement models (projective and direct), to analytically show that the systems have the same observability properties regardless these different representations To validate our analysis, we have developed EKF-based V-SLAM and MSCKF-based VO using heterogeneous geometric features of points, lines and planes, and evaluated their performance extensively in Monte-Carlo simulations n the future, we will leverage the insights gained from this observability analysis to design consistent estimators for aided NS with different geometric features by enforcing proper observability constraints as in our prior wor 7 We will also investigate the (stochastic) observability of aided NS under adversarial attacs 67 or unnown inputs 5 in order to design secure estimators for robot navigation Appendix A: State Transition Matrix Follow the reference 7, the state transition matrix Φ (,) is defined as: Φ Φ Φ (,) Φ 3 Φ 3 3 Φ Φ 5 Φ 5 Φ 53 Φ (6)

23 Figure : Monte-Carlo results of EKF-based V-SLAM using different geometric features