GNSS-Denied Semi Direct Visual Navigation for Autonomous UAVs Aided by PI-Inspired Inertial Priors

Eduardo Gallo [Contact<EMAIL_ADDRESS><EMAIL_ADDRESS><https://orcid.org/0000-0002-7397-0425>] [Affiliation: Centro de Automática y Robótica, Universidad Politécnica de Madrid - Consejo Superior de Investigaciones Científicas. Centre for Automation and Robotics, Polytechnic University of Madrid.] and Antonio Barrientos [Contact<EMAIL_ADDRESS><https://orcid.org/0000-0003-1691-3907>] [Affiliation: Centro de Automática y Robótica, Universidad Politécnica de Madrid - Consejo Superior de Investigaciones Científicas. Centre for Automation and Robotics, Polytechnic University of Madrid.]

§ ABSTRACT

This article proposes a method to diminish the horizontal position drift experienced by the VNS (Visual Navigation System) installed onboard a UAV (Unmanned Air Vehicle) by supplementing its pose estimation non linear optimizations with priors based on the outputs of the GNSS (Global Navigation Satellite System) Denied INS (Inertial Navigation System). The method is inspired by a PI (Proportional Integral) control loop, in which the attitude and altitude inertial outputs act as targets to ensure that the visual estimations do not deviate past certain thresholds from their inertial counterparts. The resulting IA-VNS (Inertially Assisted Visual Navigation System) achieves major reductions in the horizontal position drift inherent to the GNSS-Denied navigation of autonomous UAVs. Stochastic high fidelity Monte Carlo simulations of two representative scenarios involving the loss of GNSS signals are employed to evaluate the results and to analyze their sensitivity to the terrain type overflown by the aircraft. The authors release the  implementation of both the navigation algorithms and the high fidelity simulation as open-source software [1].

Keywords: GNSS-Denied, visual inertial navigation, autonomous navigation, UAV, optimization, prior

§ MATHEMATICAL NOTATION

Any variable with a hat accent < ·̂ > refers to its (inertial) estimated value, and with a circular accent < · > to its (visual) estimated value. In the case of vectors, which are displayed in bold (e.g., x⃗), other employed symbols include the wide hat < · >, which refers to the skew-symmetric form, the bar <·̅>, which represents the vector homogeneous coordinates, and the double vertical bars <  · >, which refer to the norm. In the case of scalars, the vertical bars < | ·| > refer to the absolute value. When employing quaternions and rigid body poses, which are also displayed in bold (e.g., q⃗ and ζ⃗), the asterisk superindex < ·^∗ > refers to the conjugate, their concatenation and multiplication are represented by ∘ and ⊗ respectively, and ⊕ and ⊖ refer to the plus and minus operators.

This article includes various non linear optimizations solved in the spaces of both rigid body rotations and full motions, instead of Euclidean spaces. Hence, it relies on the Lie algebra of the special orthogonal group of ℝ^3, known as 𝕊𝕆(3), and that of the special euclidean group of ℝ^3, represented by 𝕊𝔼(3), in particular what refers to the groups actions, concatenations, perturbations, and Jacobians, as well as with their tangent spaces (the rotation vector r⃗ and angular velocity ω⃗ for rotations, the transform vector τ⃗ and twist ξ⃗ for motions). [2, 3, 4] are recommended as references.

Aerodynamic path angle					                  h							            Geometric altitude

δ						    Error threshold	 						                      						            Pressure altitude

Position of throttle and control surfaces        	I⃗						          Camera image

Control targets							                      J⃗						          Jacobian

Δ						    Estimation error, increment 			                ℳ					        𝕊𝔼(3) Lie group element

Atmospheric pressure offset				                p⃗						          Point, feature

Atmospheric temperature offset		                q⃗						          Attitude, unit quaternion

θ						    Body pitch angle					  		                  r⃗						          Attitude, rotation vector

ζ⃗				    Pose, unit dual quaternion	 			                R⃗						          Attitude, rotation matrix

μ			  			    Mean or expected value	  				                ℛ			            𝕊𝕆(3) Lie group element

ξ				  		    Body bank angle 							                                           Pixel size

ξ⃗					    Motion (𝕊𝔼(3)) velocity	or twist          S							      Sensor dimension

Π⃗					    Camera projection 						                    t							            Time

ϱ_TUK		    Tukey error function	 			  		                T⃗						          Displacement

σ						    Standard deviation 			    			                						          Geodetic coordinates

τ⃗				    Pose, transform vector 					                  v							            Speed

ϕ⃗				    Attitude, Euler angles 					                  v⃗						          Velocity

χ					  	    Bearing									                          w_TUK					          Tukey weight function

ψ						      Heading or body yaw angle					               	x							            Horizontal distance

ω⃗			    Angular (𝕊𝕆(3)) velocity 	          x⃗						          Position

E_PO						    Pose optimization error 					                = 			      Inertial estimated trajectory

E_q						    Attitude adjustment error 				                = 			      Visual estimated trajectory

E_RP						    Reprojection error 								                						          Reference objectives

f						  	    Focal length	                                    = 	    Sensed trajectory

g⃗			          Lie group action (transformation)                 =  		  	    Real trajectory

Five different reference frames are employed in this article: the ECEF frame [The ECEF frame  is centered at the Earth center of mass, with  pointing towards the geodetic North along the Earth rotation axis,  contained in both the Equator and zero longitude planes, and  orthogonal to  and  forming a right handed system.], the NED frame [The NED frame  is centered at the aircraft center of mass, with axes aligned with the geodetic North, East, and Down directions.], the body frame [The body frame  is centered at the aircraft center of mass, with  contained in the plane of symmetry of the aircraft pointing forward along a fixed direction,  contained in the plane of symmetry of the aircraft, normal to  and pointing downward, and  orthogonal to both in such a way that they form a right hand system.], the camera frame [The camera frame  is centered at the optical center (appendix <ref>), with  located in the camera principal axis pointing forward, and ,    parallel to the focal plane.], and the image frame [The two-dimensional image frame  is centered at the sensor corner with axes parallel to the sensor borders (appendix <ref>).]. Superindexes are employed over vectors to specify the reference frame in which they are viewed (e.g.,  refers to ground velocity viewed in , while  is the same vector but viewed in ). Subindexes may be employed to clarify the meaning of the variable or vector, such as in  for air velocity instead of the ground velocity v⃗, in which case the subindex is either an acronym or its  meaning is clearly explained when first introduced. Subindexes may also refer to a given component of a vector, e.g.  refers to the second component of . In addition, where two reference frames appear as subindexes to a vector, it means that the vector goes from the first frame to the second. For example,  refers to the angular velocity from the  frame to the  frame viewed in .

In addition, there exist various indexes that appear as subindexes: j identifies an specific run within each Monte Carlo simulation, n identifies a discrete time instant (t_n) for the inertial estimations, s (t_s) refers to the sensor outputs, i identifies an image or frame (t_i), and k is employed for the keyframes used to generate the map or terrain structure. Other employed subindexes are l for the steps of the various iteration processes that take place, and j for the features and associated 3D points. With respect to superindexes, two stars < ·^⋆⋆ > represent the reprojection only solution, while two circles < ·^∘∘ > identify a target.

§ ACRONYMS

BRIEF         Binary Robust Independent Elementary            MX		        MiX	terrain type

Features	                  						       	NED		      North East Down

DS		            DeSert terrain type	       	                    NSE		      Navigation System Error

DSO		          Direct Sparse Odometry                         	OKVIS     Open Keyframe Visual Inertial SLAM

ECEF	  		   	Earth Centered Earth Fixed							        ORB		      Oriented FAST and rotated BRIEF

EKF		         	Extended Kalman Filter						              PI		        Proportional Integral

FAST           Features from Accelerated Segment Test          PR		        PRaire terrain type

FM		            FarM terrain type		                            RANSAC     RANdom SAmple Consensus

FR		            FoRest terrain type		                	        ROC		      Rate Of Climb

GNSS	          Global Navigation Satellite System	            ROVIO     Robust Visual Inertial Odometry

IA-VNS       Inertially Assisted VNS   	          SLAM	      Simultaneous Localization And Mapping

IA-VNSE     Inertially Assisted Visual Navigation		        SLERP	    Spherical linear interpolation

System Error                 	        	        SVO		      Semi direct Visual Odometry

IMU		     	    Inertial Measurement Unit   						        SWaP	      Size, Weight, and Power

INS             Inertial Navigation System          		        TAS		      True Air Speed

INSE           Inertial Navigation System Error       	       	UAV		      Unmanned Aerial Vehicle

iSAM           Incremental Smoothing And Mapping	 	 		        UR		        URban terrain type

ISO	            International Organization for	    		        USA	        United States of America

Standardization					         				        VINS	      Visual Inertial Navigation System

LSD 	          Large scale Semi Direct           			        VIO			    Visual Inertial Odometry

MAV			        Micro Air Vehicle		          	  			        VNS	  	    Visual Navigation System

MSCKF         Multi State Constraint Kalman Filter            VNSE	      Visual Navigation System Error

MSF	            Multi Sensor Fusion		   				    	          VO	    	    Visual Odometry

MSL		          Mean Sea Level	                                WGS84	    World Geodetic System 1984

§ INTRODUCTION AND OUTLINE

The extreme dependency of an autonomous UAV (Unmanned Air Vehicle) navigation to the presence of GNSS (Global Navigation Satellite System) signals, without which it incurs in a slow but unavoidable position drift that may ultimately lead to the loss of the platform, is discussed in section <ref>. This section also reviews the current approaches to GNSS-Denied navigation, with emphasis on those based on the images taken by onboard cameras.

This article focuses on the need to develop navigation systems capable of diminishing the position drift inherent to the flight in GNSS-Denied conditions of an autonomous fixed wing low SWaP (Size, Weight, and Power) aircraft so it has a higher probability of reaching the vicinity of a recovery point, from where it can be landed by remote control. A previous article by the same authors, [5], showed that it is possible to take advantage of sensors already present onboard fixed wing aircraft[These include accelerometers, gyroscopes, magnetometers, Pitot tube, air vanes, thermometer, and barometer.], the particularities of fixed wing flight, and the atmospheric and wind estimations that can be obtained before the GNSS signals are lost, to develop an EKF (Extended Kalman Filter) based INS (Inertial Navigation System) that results in bounded (no drift) estimations for attitude[A bounded attitude estimation ensures that the aircraft can remain aloft in GNSS-Denied conditions for as long as there is fuel available.], altitude[The altitude estimation error depends on the change in atmospheric pressure offset  [6] from its value at the time the GNSS signals are lost, which is bounded by atmospheric physics.], and ground velocity[The ground velocity estimation error depends on the change in wind velocity from its value at the time the GNSS signals are lost, which is bounded by atmospheric physics.], as well as an unavoidable drift in horizontal position caused by integrating the ground velocity without absolute observations. Figure <ref> graphically depicts that the INS inputs include all sensor measurements with the exception of the camera images I⃗.

[auto, node distance=2cm,>=latex']
[coordinate](midinput) ;
[coordinate, above of=midinput, node distance=0.4cm] (xSENSEDinput);
[coordinate, below of=midinput, node distance=0.4cm] (xESTzeroinput);
[block, right of=midinput, minimum width=3.0cm, node distance=8.0cm, align=center, minimum height=1.5cm] (NAVIGATION)
[coordinate, right of=NAVIGATION, node distance=4.5cm] (output);

[->] (xSENSEDinput) – node[pos=0.47] t_s ∖I⃗t_i = t_s ∖I⃗t_i ($(NAVIGATION.west)+(0cm,0.4cm)$);
[->] (xESTzeroinput) – node[pos=0.1]  ($(NAVIGATION.west)-(0cm,0.4cm)$);
[->] (NAVIGATION.east) – node[pos=0.5] t_n = t_n (output);

flow diagram

This article focuses on visual navigation, this is, that based on the images periodically taken by a down facing camera rigidly attached to the aircraft structure. The objectives, novelty, and applications of the proposed approach are discussed in section <ref>. A VNS (Visual Navigation System) based on an advanced publicly available VO (Visual Odometry) algorithm known as SVO (Semi-Direct Visual Odometry) is introduced in section <ref> and employed as a baseline. Visual navigation does not rely on absolute references and hence slowly accumulates error (drifts) not only in horizontal position, but also in attitude and altitude. Figure <ref> shows that the VNS relies exclusively on the images I⃗t_i as well as the initial visual estimations , as explained in section <ref>.

[auto, node distance=2cm,>=latex']
[coordinate](midinput) ;
[coordinate, above of=midinput, node distance=0.4cm] (IMAGESinput);
[coordinate, below of=midinput, node distance=0.4cm] (xVISSTARzeroinput);

[block, right of=midinput, minimum width=3.0cm, node distance=5.3cm, align=center, minimum height=1.5cm] (NAVIGATION)
[coordinate, right of=NAVIGATION, node distance=4.8cm] (output);

[->] (IMAGESinput) – node[pos=0.45] I⃗t_i ⊂t_i ($(NAVIGATION.west)+(0cm,0.4cm)$);
[->] (xVISSTARzeroinput) – node[pos=0.10]  ($(NAVIGATION.west)-(0cm,0.4cm)$);

[->] (NAVIGATION.east) – node[pos=0.55] t_i = t_i (output);

flow diagram

Section <ref> describes the proposed IA-VNS (Inertially Assisted VNS), which employs the INS attitude and altitude estimations to introduce priors into the pose estimation non linear optimizations present in SVO, resulting not only in significant improvements in the visual attitude and altitude estimations, but specially in a major reduction in horizontal position drift, which is a full order of magnitude lower than the drifts obtained by either the INS or the VNS by themselves. The IA-VNS is graphically depicted in figure <ref>.

[auto, node distance=2cm,>=latex']
[coordinate](midinput) ;
[coordinate, above of=midinput, node distance=0.0cm] (xESTzeroinput0) ;
[coordinate, above of=midinput, node distance=2.2cm] (xVISSTARzeroinput) ;
[coordinate, above of=midinput, node distance=0.4cm] (xSENSEDinput);

[block, right of=midinput, minimum width=3.0cm, node distance=5.6cm, align=center, minimum height=1.5cm] (NAVIGATION)
[block, above of=NAVIGATION, minimum width=4.5cm, node distance=2.5cm, align=center, minimum height=1.5cm] (VISNAVIGATION)

[coordinate, right of=NAVIGATION, node distance=2.5cm] (midpoint);
[black] (midpoint) circle [radius=1pt];
[coordinate, above of=midpoint, node distance=1.2cm] (highpoint);

[->]  (xESTzeroinput0) – node[pos=0.08]  (NAVIGATION.west);
[->] (xVISSTARzeroinput) – node[pos=0.10]  ($(VISNAVIGATION.west)+(0cm,-0.3cm)$);

[->] (NAVIGATION.east) – node[pos=0.6] t_n = t_n ($(NAVIGATION.east)+(4.8cm,0.0cm)$);
[->] (VISNAVIGATION.east) – node[pos=0.6] t_i = t_i ($(VISNAVIGATION.east)+(4.0cm,0.0cm)$);
[->] (midpoint) – (highpoint) -| (VISNAVIGATION.south);

[coordinate, below of=midinput, node distance=1.6cm] (xSENSEDINERinput);
[coordinate, below of=NAVIGATION, node distance=1.6cm] (xSENSEDINERmiddle);
[->] (xSENSEDINERinput) – node[pos=0.45] t_s ∖I⃗t_i = t_s ∖I⃗t_i (xSENSEDINERmiddle) – (NAVIGATION.south);

[coordinate, above of=midinput, node distance=3.5cm] (xSENSEDVISinput);
[coordinate, above of=VISNAVIGATION, node distance=1.0cm] (xSENSEDVISmiddle);
[->] (xSENSEDVISinput) – (xSENSEDVISmiddle) – (VISNAVIGATION.north);
at ($(midinput) + (+1.6cm,+3.1cm)$) I⃗t_i ⊂t_i;

flow diagram

Section <ref> introduces the stochastic high fidelity simulation employed to evaluate the navigation results by means of Monte Carlo executions of two scenarios[These are the same scenarios employed to evaluate the INS in [5].] representative of the challenges of GNSS-Denied navigation, which are described in detail in [7]. The results obtained when applying the proposed algorithms to these two GNSS-Denied scenarios are described in section <ref>. Section section <ref> discusses the sensitivity of the estimations to the type of terrain overflown by the aircraft, as the terrain texture (or lack of) and its elevation relief are key factors on the ability of the visual algorithms to detect and track terrain features. Last, the results are summarized for convenience in section <ref>, while section <ref> provides a short conclusion.

§ GNSS-DENIED NAVIGATION

The number, variety, and applications of UAVs have grown exponentially in the last few years, and the trend is expected to continue in the future [8, 9]. This is particularly true in the case of low SWaP vehicles because their reduced cost makes them suitable for a wide range of applications, both civil and military. [10] presents a comprehensive review of low SWaP UAV navigation systems and the problems they face, including the degradation or absence of GNSS signals.

Aircraft navigation has traditionally relied on the measurements provided by accelerometers, gyroscopes, and magnetometers, incurring in an slow but unbounded position drift that could only be stopped by triangulation with the use of external navigation (radio) aids. More recently, the introduction of satellite navigation (GNSS) has completely removed the position drift and enabled autonomous inertial navigation in low SWaP platforms [11, 12, 13]. On the negative side, low SWaP inertial navigation exhibits an extreme dependency on the availability of GNSS signals. If the signals are not present or can not be employed, inertial systems rely on dead reckoning, which results in position drift, with the aircraft slowly but steadily deviating from its intended route [14]. The availability of GNSS signals cannot be guaranteed; a throughout analysis of GNSS threats and reasons for signal degradation is presented in [15]. In GNSS-Denied conditions, the vehicle is unable to fly its intended route or even return to a safe recovery location, which leads to the uncontrolled loss of the airframe if the GNSS signals are not recovered before the aircraft runs out of fuel (or battery in case of electric vehicles).

The extreme dependency on GNSS availability is not only one of the main impediments for the introduction of small UAVs in civil airspace, where it is not acceptable to have uncontrolled vehicles causing personal or material damage, but it also presents a significant drawback for military applications, as a single hull loss may compromise the onboard technology. At this time there are no comprehensive solutions to the operation of low SWaP autonomous UAVs in GNSS-Denied scenarios, although the use of onboard cameras seems to be one of the most promising routes. Bigger and more expensive UAVs, this is, with less stringent SWaP requirements, can rely to some degree on more accurate Inertial Measurement Units or IMUs (at the expense of SWaP) and additional communications equipment to overcome this problem, but for most autonomous UAVs, the permanent loss of the GNSS signals is equivalent to losing the airframe in an uncontrolled way.

§.§ Possible Approaches to GNSS-Denied Navigation

Inertial navigation employs the periodic readings provided by the IMU to estimate the pose of a moving object by means of dead reckoning or integration. On aircraft, inertial sensors are complemented by magnetometers and a barometer to add robustness to the inertial solution. Fixed wing aircraft are also equipped with a Pitot tube and air vanes required by their control system, although their measurements are usually not employed for navigation. Absolute references such as those provided by navigation radio aids or GNSS receivers are required to remove the position drift inherent to inertial navigation.

Low SWaP autonomous aircraft are too small to incorporate navigation aid receivers, which in any case are not available over vast regions of the Earth, exhibiting an extreme dependency on the availability of GNSS signals. A summary of the challenges of GNSS-Denied navigation and the research efforts intended to improve its performance is provided by [16]. There exist various approaches to mitigate this problem, with detailed reviews provided by [5, 17]. Two promising techniques for completely eliminating the position drift are the use of signals of opportunity[Existing signals originally intended for other purposes, such as those of television and cellular networks, can be employed to triangulate the aircraft position.] [18, 19, 20], and georegistration[The position drift can be eliminated by matching landmarks or terrain features as viewed from the aircraft to preloaded data.] [21, 22, 23, 24], also known as image registration.

§.§ Visual Navigation

Visual Odometry (VO) consists on employing the ground images generated by one or more onboard cameras without the use of prerecorded image databases or any other sensors, incrementally estimating the vehicle pose based on the changes that its motion induces on the images [25, 26, 27]. It  requires sufficient illumination, dominance of static scene, enough texture, and scene overlap between consecutive images or frames. It can rely on a single camera[This article relies on monocular vision exclusively.] (monocular vision), in which case the motion can only be recovered up to a scale factor, or on various cameras (stereo vision), where the differences among the simultaneous images taken with the different cameras are employed to determine the scale. It has been employed for navigation of ground robots, road vehicles, and multirotors flying both indoors and outdoors.

The incremental concatenation of relative poses results in a slow but unbounded pose drift, which can only be eliminated if aided by Simultaneous Localization and Mapping (SLAM) [28, 29], a particular case of VO in which the map of the already viewed terrain is stored and employed for loop closure in case it is revisited by the vehicle during its motion. In this sense, VO only uses the map to improve the local consistency of the solution, while SLAM is more concerned with its global consistency [25]. The result is that SLAM is potentially more accurate, but also slower, computationally more expensive, and less robust.

Modern stand-alone algorithms such as Semi Direct Visual Odometry (SVO) [30, 31], Direct Sparse Odometry (DSO) [32], Large scale Semi Dense SLAM (LSD-SLAM) [33], and large scale feature based SLAM (ORB-SLAM)[ORB stands for Oriented FAST and rotated BRIEF, a type of blob feature.] [34, 35, 36] are robust and exhibit a limited drift.

A typical VO algorithm includes steps to obtain the images, detect and extract its features, either match or track those features[VO algorithms can be divided into feature-based methods (also known as matching methods) and direct or tracking methods [27].], estimate the relative motion between consecutive frames, concatenate them to obtain the full camera pose trajectory, and finally perform some local optimization (bundle adjustment) [25].

§.§ Visual Inertial Navigation

Estimating the aircraft pose based on both IMUs and cameras represents the most promising solution to GNSS-Denied navigation, in what is known as Visual Inertial Odometry (VIO) [37, 38], which can also be combined with image registration to fully eliminate the remaining pose drift. Current VIO implementations are also primarily intended for ground robots, multirotors, and road vehicles, and hence rely exclusively on the vehicle IMU readings and the images taken by the onboard cameras, but do not use other sensors commonly found onboard fixed wing aircraft. VIO has matured significantly in the last few years, with detailed reviews available in [37, 38, 39, 40, 41].

VIO currently appears to represent the state of the art in GNSS-Denied navigation for low SWaP UAVs [10]. There exist several open source VIO packages, such as the Multi State Constraint Kalman Filter (MSCKF) [42], the Open Keyframe Visual Inertial SLAM (OKVIS) [43, 44], the Robust Visual Inertial Odometry (ROVIO) [45], the monocular Visual Inertial Navigation System (VINS-Mono) [46], SVO combined with Multi Sensor Fusion (MSF) [30, 31, 47, 48], and SVO combined with Incremental Smoothing and Mapping (iSAM) [30, 31, 49, 50]. All these open source pipelines are compared in [37], and their results when applied to the EuRoC MAV data sets [51] are discussed in [52]. There also exist various other published VIO pipelines with implementations that are not publicly available [53, 54, 55, 56, 57, 58, 59], and there are also others that remain fully proprietary.

The existing VIO schemes can be broadly grouped into two paradigms: loosely coupled pipelines process the measurements separately, resulting in independent visual and inertial pose estimations, which are then fused to get the final estimate; on the other hand, tightly coupled methods compute the final pose estimation directly from the tracked image features and the IMU outputs [37, 38]. Tightly coupled approaches usually result in higher accuracy, as they use all the information available and take advantage of the IMU integration to predict the feature locations in the next frame. Loosely coupled methods, although less complex and more computationally efficient, lose information by decoupling the visual and inertial constraints, and are incapable of correcting the drift present in the visual estimator.

A different classification involves the number of images involved in each estimation [37, 38, 60], which is directly related with the resulting accuracy and computing demands. Batch algorithms, also known as smoothers, estimate multiple states simultaneously by solving a large nonlinear optimization problem or bundle adjustment, resulting in the highest possible accuracy. Valid techniques to limit the required computing resources include the reliance on a subset of the available frames (known as keyframes)[Common criteria to identify keyframes involve thresholds on the time, position, or pose increments from the previous keyframe.], the separation of tracking and mapping into different threads, and the development of incremental smoothing techniques based on factor graphs [50]. Although employing all available states (full smoothing) is sometimes feasible for very short trajectories, most pipelines rely on sliding window or fixed lag smoothing, in which the optimization relies exclusively on the measurements associated to the last few keyframes, discarding both the old keyframes as well as all other frames that have not been cataloged as keyframes. On the other hand, filtering algorithms restrict the estimation process to the latest state; they require less resources but suffer from permanently dropping all previous information and a much harder identification and removal of outliers, both of which lead to error accumulation or drift.

The success of any VIO approach relies on an accurate calibration of the pose and time offsets between the IMU and the camera [37, 38]. Additional challenges applicable to all pipelines include the different working frequencies of IMUs and cameras, as well the initialization requirements to bootstrap the algorithms.

§ OBJECTIVE, NOVELTY, AND APPLICATION

§.§ Objective

The main objective of this article is to improve the GNSS-Denied navigation capabilities of autonomous aircraft, so in case GNSS signals become unavailable, they can continue their mission or safely fly to a predetermined recovery location. To do so, the proposed approach combines two different navigation algorithms, employing the outputs of an INS specifically designed for the flight without GNSS signals of an autonomous fixed wing low SWaP aircraft [5] to diminish the horizontal position drift generated by a VNS that relies on an advanced visual odometry pipeline such as SVO [30, 31]. Note that the INS makes use of all onboard sensors except the camera, while the VNS relies exclusively on the images provided by the camera.

Each of the two systems by itself incurs in unrestricted and excessive horizontal position drift that renders them inappropriate for long term GNSS-Denied navigation, but for different reasons: while in the INS the drift is the result of integrating the bounded ground velocity estimations without absolute position observations, that of the VNS originates on the slow but continuous accumulation of estimation errors between consecutive frames. The two systems however differ in their estimations of the aircraft attitude and altitude, as they are bounded for the INS but also drift in the case of the VNS. The proposed approach modifies the VNS so in addition to the images it can also accept as inputs the INS bounded attitude and altitude outputs, converting it into an Inertially Assisted VNS or IA-VNS with vastly improved horizontal position estimation capabilities.

§.§ Novelty

The VIO solutions listed in section <ref> are quite generic with respect to the platforms on which they are mounted, with most applications focused on ground vehicles, indoor robots, and multirotors, as well as with respect to the employed sensors, which are usually restricted to the IMU (gyroscopes and accelerometers) and one or more cameras. This article focuses on an specific case (long distance GNSS-Denied turbulent flight of fixed wing low SWaP aircraft), and as such is simultaneously more restrictive but also takes advantage of the sensors already onboard these platforms, such as magnetometers, Pitot tube, and air vanes. In addition, and unlike the existing VIO packages, the proposed solution assumes that GNSS signals are present at the beginning of the flight. As described in detail in [5], these are key to the obtainment of the bounded attitude and altitude INS outputs on which the proposed IA-VNS relies.

The proposed IA-VNS method represents a novel approach to diminish the pose drift of a VO pipeline by supplementing its pose estimation non linear optimizations with priors based on the bounded attitude and altitude outputs of a GNSS-Denied inertial filter. The method is inspired in a PI (Proportional Integral) control loop, in which the inertial attitude and altitude outputs act as targets to ensure that the visual estimations do not deviate in excess from their inertial counterparts, resulting in major reductions to not only the visual attitude and altitude estimation errors, but also to the drift in horizontal position.

§.§ Application

This articles proves that inertial and visual navigation systems can be combined in such a way that the resulting IA-VNS incurs in a GNSS-Denied horizontal position drift that is significantly smaller than what can be obtained by either system individually. In the case that GNSS signals become unavailable in mid flight, GNSS-Denied navigation is required for the platform to complete its mission or return to base without the absolute position and ground velocity observations provided by GNSS receivers. As shown in the following sections, the proposed system can significantly increase the possibilities of the aircraft safely reaching the vicinity of the intended recovery location, from where it can be landed by remote control.

§ SEMI-DIRECT VISUAL ODOMETRY

Semi-Direct Visual Odometry (SVO) [30, 31] is a publicly available advanced combination of feature-based and direct VO techniques primarily intended towards the navigation of land robots, road vehicles, and multirotors, holding various advantages in terms of accuracy and speed over traditional VO algorithms. By combining the best characteristics of both approaches while avoiding their weaknesses, it obtains high accuracy and robustness with a limited computational budget. This section provides a short summary of the SVO pipeline, although the interested reader should refer to [30, 31] for a more detailed description; the pose optimization phase is however described in depth, as it is the focus of the proposed modifications described in section <ref>.

[auto, node distance=2cm,>=latex']
[coordinate](map) ;
[coordinate, above of=map, node distance=0.30cm] (coord_map) ;
[coordinate, above of=map, node distance=1.35cm] (coord_map_fd) ;
[coordinate, above of=map, node distance=0.45cm] (coord_map_mp) ;
[coordinate, below of=map, node distance=0.45cm] (coord_map_df) ;
[coordinate, below of=map, node distance=1.35cm] (coord_map_fa) ;

[coordinate, left of=map,  node distance=4.50cm] (aux) ;
[coordinate, above of=aux, node distance=0.30cm] (coord_aux) ;
[coordinate, above of=aux, node distance=1.35cm] (coord_aux_in) ;
[coordinate, above of=aux, node distance=0.45cm] (coord_aux_hm) ;
[coordinate, below of=aux, node distance=0.45cm] (coord_aux_vs) ;

[coordinate, right of=map, node distance=5.40cm] (mot) ;
[coordinate, above of=mot, node distance=0.30cm] (coord_mot) ;
[coordinate, above of=mot, node distance=1.35cm] (coord_mot_sia) ;
[coordinate, above of=mot, node distance=0.45cm] (coord_mot_fa) ;
[coordinate, below of=mot, node distance=0.45cm] (coord_mot_po) ;
[coordinate, below of=mot, node distance=1.35cm] (coord_mot_so) ;

[rectangle, draw=black!50, fill=black!8,   dashed, rounded corners, left of=coord_aux,  node distance=0.56cm, minimum width=4.7cm, minimum height=4.2cm] (AUXILIARY) ;
[rectangle, draw=black!50, fill=red!30,    dashed, rounded corners, right of=coord_map, node distance=0.0cm, minimum width=4.5cm, minimum height=4.2cm] (MAPPING) ;
[rectangle, draw=black!50, fill=yellow!30, dashed, rounded corners, right of=coord_mot, node distance=0.0cm, minimum width=5.4cm, minimum height=4.2cm] (MOTION) ;

[rectangle, draw=black!50, fill=black!24, left of=coord_aux_in, minimum width=2.5cm, node distance=0.92cm, align=center, minimum height=0.7cm] (IN) 1. ;
[rectangle, draw=black!50, fill=black!24, left of=coord_aux_hm, minimum width=2.5cm, node distance=0.54cm, align=center, minimum height=0.7cm] (HM) 2. ;

[rectangle, draw=black!50, fill=blue!30, left of=coord_map_fd, minimum width=2.5cm, node distance=0.0cm,  align=center, minimum height=0.7cm] (FD) 1. ;
[rectangle, draw=black!50, fill=blue!30, left of=coord_map_mp, minimum width=2.0cm, node distance=0.90cm, align=center, minimum height=0.7cm] (MP) 2. ;
[rectangle, draw=black!50, fill=blue!30, left of=coord_map_df, minimum width=2.5cm, node distance=0.46cm, align=center, minimum height=0.7cm] (DF) 3. ;
[rectangle, draw=black!50, fill=blue!30, left of=coord_map_fa, minimum width=2.5cm, node distance=0.0cm,  align=center, minimum height=0.7cm] (FD) 4. ;

[rectangle, draw=black!50, fill=green!30, left of=coord_mot_sia, minimum width=2.5cm, node distance=0.0cm, align=center, minimum height=0.7cm] (SIA) 1. ;
[rectangle, draw=black!50, fill=green!30, left of=coord_mot_fa,  minimum width=2.5cm, node distance=0.46cm, align=center, minimum height=0.7cm] (FA) 2. ;
[rectangle, draw=black!50, fill=green!30, left of=coord_mot_po,  minimum width=2.5cm, node distance=0.46cm, align=center, minimum height=0.7cm] (PO) 3. ;
[rectangle, draw=black!50, fill=green!30, left of=coord_mot_so,  minimum width=2.5cm, node distance=0.0cm, align=center, minimum height=0.7cm] (SO)  4. ;

at ($(AUXILIARY.west) +(+1.15cm,1.8cm)$) ;
at ($(MAPPING.west) +(+1.63cm,1.8cm)$) ;
at ($(MOTION.west) +(+1.55cm,1.8cm)$) ;

threads and processes

SVO initializes like a feature-based monocular method, requiring the height over the terrain to provide the scale (initialization), and using feature matching and RANSAC [61] based triangulation (initial homography) to obtain a first estimation of the terrain 3D position of the identified features. After initialization, the SVO pipeline for each new image can be divided into two different threads: the mapping thread, which generates terrain 3D points, and the motion thread, which estimates the camera motion.

Once initialized, the expensive feature detection process (mapping thread) that obtains the features does not occur in every frame but only once a sufficiently large motion has occurred since the last feature extraction. When processing each new frame, SVO initially behaves like a direct method, discarding the feature descriptors and skipping the matching process, and employing the luminosity values of small patches centered around every feature to (i) obtain a rough estimation of the camera pose (sparse image alignment, motion thread), followed by (ii) a relaxation of the epipolar restrictions to achieve a better estimation of the different features sub-pixel location in the new frame (feature alignment, motion thread), which introduces a reprojection residual that is exploited in the next steps. At this point, SVO once again behaves like a feature-based method, refining (iii) the camera pose (pose optimization, motion thread) and (iv) the terrain coordinates of the 3D points associated to each feature (structure optimization, motion thread) based on nonlinear minimization of the reprojection error.

In this way, SVO is capable of obtaining the accuracy of direct methods at a very high computational speed, due to only extracting features in selected frames, avoiding (for the most part) robust algorithms when tracking features, and only reconstructing the structure sparsely. The accuracy of SVO improves if the pixel displacement between consecutive frames is reduced (high frame rate), which is generally possible as the computational expenses associated to each frame are low.

None of the motion thread four nonlinear optimization processes listed above makes use of RANSAC, and pose optimization is the only one that employs a robust M-estimator [62, 63] instead of the traditional mean or squared error estimator. This has profound benefits in terms of computational speed but leaves the whole process vulnerable to the presence of outliers in either the features terrain or image positions. To prevent this, once a feature is detected in a given frame (note that the extraction process obtains pixel coordinates, not terrain 3D ones), it is immediately assigned with a depth filter (mapping thread) initialized with a large enough uncertainty around the average depth in the scene; in each subsequent frame, the feature 3D position is estimated by reprojection and the depth filter uncertainty reduced. Once the feature depth filter has converged, the detected feature and its associated 3D point become a map candidate, which it is not yet employed in the motion thread optimizations required to estimate the camera pose. The feature alignment process is however applied in the background to the map candidates, and it is only after several successful reprojections that a candidate is upgraded to a map 3D point and hence allowed to influence the motion result. This two step verification process that requires depth filter convergence and various successful reprojections before a 3D point is employed in the (mostly) non-robust optimizations is key to prevent outliers from contaminating the solution and reducing its accuracy.

§.§ Pose Optimization

As the focus of the proposed improvements within section <ref>, the pose optimization phase is the only one described in detail in this article. Graphically depicted in figure <ref>, pose optimization is executed for every new frame i and makes use of the different features and associated 3D points j present in the map to generate the visual estimation of the pose between the ECEF () and camera () frames ζ⃗_ECi.

[auto, node distance=2cm,>=latex']
[coordinate](midinput) ;
[coordinate, above of=midinput, node distance=0.8cm] (aboveinput) ;
[coordinate, below of=midinput, node distance=0.8cm] (belowinput) ;

[coordinate, above of=midinput, node distance=0.2cm] (refinput) ;

[rectangle, draw=black!50, fill=yellow!30, dashed, rounded corners, right of=refinput, node distance=6.6cm, minimum width=10.2cm, minimum height=3.6cm] (MOTION) ;

[blockgreen, right of=midinput, minimum width=2.0cm, node distance=10.2cm, align=center, minimum height=2.9cm] (PO)

[rectangle, draw=black!50, fill=green!30, dashed, right of=aboveinput, minimum width=4.5cm, node distance=4.0cm, align=center, minimum height=1.0cm] (SIA) ;
[rectangle, draw=black!50, fill=green!30, dashed, right of=belowinput, minimum width=4.5cm, node distance=4.0cm, align=center, minimum height=1.0cm] (FA) ;

[->] (midinput) – node[pos=0.05] p⃗_j^E (PO.west);
[->] ($(PO.east)+(+0.0cm,-0.0cm)$) – node[pos=0.7] ζ⃗_ECi ($(PO.east)+(+1.8cm,-0.0cm)$);
[dashed,->] (SIA.east) – node[pos=0.4] ζ⃗_ECi0 = ζ⃗_ECi^⋆ ($(PO.west)+(+0.0cm,+0.8cm)$);
[->] (FA.east) – node[pos=0.2] p⃗_ij^IMG ($(PO.west)+(+0.0cm,-0.8cm)$);

at ($(MOTION.west) +(+2.1cm,1.50cm)$)  i;

Pose optimization flow diagram

The previous feature alignment phase obtains the projection of the feature points into the current image p⃗_ij^IMG by minimizing the photometric error while ignoring their epipolar constraints, which enables fine tuning the camera pose ζ⃗_ECi^⋆ provided by the sparse image alignment phase by minimizing the reprojection error of the different terrain 3D points p⃗_j^E present in the map, which originate at the structure optimization phase of the previous frame. This process is known as pose refinement or motion-only bundle adjustment [30].

The reprojection error E_RP is defined in (<ref>) as the sum for each 3D point of the norm of the difference between the camera projection Π⃗ of the ECEF coordinates p⃗_j^E transformed into the camera frame[g⃗_ζ⃗_AB represents the 𝕊𝔼(3) transformation of a point from frame B to frame A, as described in [2].] and the image coordinates p⃗_j^IMG.
E_RP = ∑_j ‖Π⃗(g⃗_ζ⃗_ECi^-1(p⃗_j^E)) - p⃗_ij^IMG‖eq:svo_pose_optim_problem_general

This problem can be solved by means of an iterative Gauss-Newton gradient descent process [2, 64]. Given an initial pose estimation ζ⃗_ECi0 taken from the sparse image alignment result (ζ⃗_ECi^⋆), each iteration step l minimizes (<ref>) and advances the estimated solution by means of (<ref>) until the step diminution of the reprojection error falls below a given threshold E_RP,i,l - E_RP,i,l+1 < δ_RP:

\begin{eqnarray}
\nm{E_{RPi,l+1}\lrp{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}} & = & \nm{\sum_j \biggr\lVert \vec \Pi\big(\vec g_{\ds{\circled{\vec \zeta}_{{\sss EC}il}} \oplus \Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}^{-1}(\vec p_{j}^{\sss E})\big) - \vec p_{ij}^{\sss IMG} \biggr\rVert = \sum_j \Bigr\lVert \vec E_{RPi,l+1,j}\lrp{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}  \Bigr\rVert} \label{eq:svo_pose_optim_problem} \\
\nm{\circled{\vec \zeta}_{{\sss EC}i,l+1}} & \nm{\longleftarrow} & \nm{\circled{\vec \zeta}_{{\sss EC}il} \circ Exp\lrp{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} = \circled{\vec \zeta}_{{\sss EC}il} \oplus \Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} \label{eq:svo_pose_optim_iterative}
\end{eqnarray}

Each Δτ⃗_ECil^C represents the update to the camera pose given by the transform vector [2, 4] viewed in the local camera frame F_Cil, which is obtained by following the process described in [2, 64], and results in (<ref>), where J⃗_OF,ilj (<ref>) represents the optical flow obtained in appendix <ref>:

\begin{eqnarray}
\nm{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} & = & \nm{- \bigg[\sum_j {\vec J_{OF,ilj}}^T \ \vec J_{OF,ilj}\bigg]^{-1} \ \sum_j {\vec J_{OF,ilj}}^T \ \Big[\vec \Pi\big(\vec g_{\ds{\circled{\vec \zeta}_{{\sss EC}il}}}^{-1}(\vec p_{j}^{\sss E})\big) - \vec p_{ij}^{\sss IMG}\Big] \ \in \mathbb{R}^6} \label{eq:svo_pose_optim_solution} \\
\nm{\vec J_{OF,ilj}} & = & \nm{\vec J_{OF}\Big(\vec \Pi\big(\vec g_{\circled{\vec \zeta}_{{\sss EC}il}}^{-1}\lrp{\vec p_j^{\sss E}\big)}\Big) \ \  \ \in \mathbb{R}^{2x6}} \label{eq:svo_pose_optim_jacobian}
\end{eqnarray}

In order to protect the resulting pose from the possible presence of outliers in either the 3D points p⃗_j^E or their image projection p⃗_ij^IMG, it is better to replace the above squared error or mean estimator by a more robust M-estimator, such as the bisquare or Tukey estimator [62, 63]. The error to be minimized in each iteration step is then given by (<ref>), where the Tukey error function ϱ_TUKx = ρ_TUK√(x) can be found in [63].

\begin{eqnarray}
\nm{E_{RPi,l+1}\lrp{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}} & = & \nm{\sum_j \varrho_{TUK}\lrp{ \Big[\vec \Pi\big(\vec g_{\ds{\circled{\vec \zeta}_{{\sss EC}il}} \oplus \Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}^{-1}(\vec p_{j}^{\sss E})\big) - \vec p_{ij}^{\sss IMG} \Big]^T \Big[\vec \Pi\big(\vec g_{\ds{\circled{\vec \zeta}_{{\sss EC}il}} \oplus \Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}}^{-1}(\vec p_{j}^{\sss E})\big) - \vec p_{ij}^{\sss IMG} \Big]}} \nonumber \\
& = & \nm{\sum_j \varrho_{TUK}\lrp{{\vec E_{RPi,l+1,j}}^T \, \vec E_{RPi,l+1,j}}} \label{eq:svo_pose_optim_problem_tukey}
\end{eqnarray}

A similar process to that employed above leads to the solution (<ref>), where the Tukey weight function w_TUKx is provided by [63]:

\begin{eqnarray}
\nm{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} & = & \nm{- \bigg[\sum_j w_{TUK}\lrp{{\vec E_{RP,ilj}}^T \vec E_{RP,ilj}} {\vec J_{OF,ilj}}^T \vec J_{OF,ilj}\bigg]^{-1}} \nonumber \\
&  & \nm{\ \ \bigg[\sum_j w_{TUK}\lrp{{\vec E_{RP,ilj}}^T \vec E_{RP,ilj}} {\vec J_{OF,ilj}}^T \vec E_{RP,ilj}\bigg] \ \in \mathbb{R}^6} \label{eq:svo_pose_optim_solution_tukey} \\
\nm{\vec E_{RP,ilj}} & = & \nm{ \vec \Pi\big(\vec g_{\ds{\circled{\vec \zeta}_{{\sss EC}il}}}^{-1}(\vec p_{j}^{\sss E})\big) - \vec p_{ij}^{\sss IMG} \ \in \mathbb{R}^2} \label{eq:svo_pose_optim_comp}
\end{eqnarray}

§ PROPOSED INERTIALLY ASSISTED SEMI-DIRECT VISUAL ODOMETRY

Lacking any absolute references, the SVO based VNS described in section <ref> gradually accumulates errors in each of the six dimensions of the vehicle pose , as shown in section <ref>. If accurate estimations of attitude and altitude can be provided by an INS such as that described in [5], these can be employed to ensure that the visual estimations for body attitude and vertical position ( and ) do not deviate in excess from their inertial counterparts  and , so they too can be considered as bounded up to a certain degree.

[auto, node distance=2cm,>=latex']
[coordinate](midinput) ;
[coordinate, above of=midinput, node distance=1.6cm] (aboveinput_two) ;
[coordinate, above of=midinput, node distance=0.8cm] (aboveinput_one) ;
[coordinate, below of=midinput, node distance=0.8cm] (belowinput_one) ;
[coordinate, below of=midinput, node distance=1.6cm] (belowinput_two) ;

[coordinate, above of=midinput, node distance=0.2cm] (refinput) ;

[rectangle, draw=black!50, fill=yellow!30, dashed, rounded corners, right of=refinput, node distance=6.5cm, minimum width=10.4cm, minimum height=5.0cm] (MOTION) ;

[blockgreen, right of=midinput, minimum width=2.0cm, node distance=10.2cm, align=center, minimum height=4.0cm] (PO)

[rectangle, draw=black!50, fill=green!30, dashed, right of=aboveinput_two, minimum width=4.8cm, node distance=4.0cm, align=center, minimum height=1.0cm] (SIA) ;
[rectangle, draw=black!50, fill=green!30, dashed, right of=belowinput_two, minimum width=4.8cm, node distance=4.0cm, align=center, minimum height=1.0cm] (FA) ;

[->] (aboveinput_one) – node[pos=0.05] p⃗_j^E ($(PO.west)+(+0.0cm,+0.8cm)$);
[->] (midinput) – node[pos=0.06] q̂⃗̂_NBn (PO.west);
[->] (belowinput_one) – node[pos=0.05] ĥ_n ($(PO.west)+(+0.0cm,-0.8cm)$);

[->] ($(PO.east)+(+0.0cm,-0.0cm)$) – node[pos=0.7] ζ⃗_ECi ($(PO.east)+(+1.8cm,-0.0cm)$);
[dashed,->] (SIA.east) – node[pos=0.4] ζ⃗_ECi0 = ζ⃗_ECi^⋆ ($(PO.west)+(+0.0cm,+1.6cm)$);
[->] (FA.east) – node[pos=0.2] p⃗_ij^IMG ($(PO.west)+(+0.0cm,-1.6cm)$);

at ($(MOTION.west) +(+2.1cm,2.20cm)$)  i;

Prior based pose optimization flow diagram

Note that the inertial estimations should not replace the visual ones within SVO, as this would destabilize the visual pipeline preventing its convergence, but just act as anchors so the visual estimations oscillate freely as a result of the multiple SVO optimizations but without drifting from the vicinity of the anchors. This section shows how to modify the cost function within the iterative Gauss-Newton gradient descent pose optimization phase so it can take advantage of the INS outputs.

§.§ Rationale for the Introduction of Priors

The prior based pose optimization process starts by executing exactly the same pose optimization described in section <ref>, which seeks to obtain the camera pose ζ⃗_ECi that minimizes the reprojection error E_RP (<ref>). The iterative optimization results in a series of transform vector updates Δτ⃗_ECil^C (<ref>), where l indicates the iteration step. The camera pose is then advanced per (<ref>) until the step diminution of the reprojection error falls below a certain threshold.

The resulting camera pose, ζ⃗_ECi, is marked with the superindex ⋆⋆ to indicate that it is the reprojection only solution, resulting in ζ⃗_ECi^⋆⋆. Its concatenation with the constant body to camera pose ζ⃗_BC results in the reprojected body pose[Note that a single asterisk superindex < ·^∗ > applied to a quaternion refers to its conjugate or inverse.] ζ⃗_EBi^⋆⋆:
ζ⃗_EBi^⋆⋆ = ζ⃗_ECi^⋆⋆ ⊗ζ⃗_BC^∗  eq:nav_vis_assist_pose_optim_zetaEBstarstar

The reprojected ECEF body attitude q⃗_EBi^⋆⋆ and Cartesian coordinates T⃗_EBi^B⋆⋆ can then be readily obtained, which leads on one hand to the reprojected body attitude q⃗_NBi^⋆⋆, equivalent to the Euler angles ϕ⃗_NBi^⋆⋆ = ψ_i^⋆⋆, θ_i^⋆⋆, ξ_i^⋆⋆^T, and on the other to the geodetic coordinates T⃗_i^E,GDT⋆⋆ (including the altitude h_i^⋆⋆) and ECEF to NED rotation q⃗_ENi^⋆⋆.

Let's assume for the time being that the observed body attitude (q̂⃗̂_NBn) or altitude (ĥ_n) estimated by the INS [5] enable the IA-VNS to conclude that it would be preferred if the optimized body attitude were closer to a certain target attitude identified by the superindex ∘∘, q⃗_NBi^∘∘, equivalent to the target Euler angles ϕ⃗_NBi^∘∘ = ψ_i^∘∘, θ_i^∘∘, ξ_i^∘∘^T. Section <ref> specifies when this assumption can be considered valid, as well as various alternatives to obtain the target attitude from q̂⃗̂_NBn and ĥ_n. The target body attitude is converted into a target camera attitude by means of the constant  and the original reprojected ECEF to NED rotation  q⃗_ENi^⋆⋆, incurring in a negligible error by not considering the attitude change of the NED frame as the iteration progresses.
q⃗_ECi^∘∘ = q⃗_ENi^⋆⋆ ⊗q⃗_NBi^∘∘ ⊗ eq:nav_vis_assist_pose_optim_qECcirccirc

Note that the objective is not for the resulting body attitude q⃗_NBi to equal the target q⃗_NBi^∘∘, but to balance both objectives (minimization of the reprojection error of the different terrain 3D points and minimization of the attitude differences with the targets) without imposing any hard constraints on the pose (position plus attitude) of the aircraft.

§.§ Prior Based Pose Optimization

The attitude adjustment error E_q is defined in (<ref>) as the norm of the Euclidean difference between rotation vectors corresponding to the camera attitude q⃗_ECi and the target camera attitude q⃗_ECi^∘∘ [2, 4]:
E_q(Logq⃗_ECi) = E_qr⃗_ECi = ‖Logq⃗_ECi - Logq⃗_ECi^∘∘ ‖= ‖r⃗_ECi - r⃗_ECi^∘∘ ‖eq:nav_vis_assist_pose_optim_pitch_objective

Its minimization can be solved by means of an iterative Gauss-Newton gradient descent process [2, 64]. Given an initial attitude estimation r⃗_ECi,0 = Logq⃗_ECi,0 taken from ζ⃗_ECi0 = ζ⃗_ECi^⋆, each iteration step l minimizes (<ref>) and advances the estimated solution by means of (<ref>) until the step diminution of the attitude adjustment error falls below a given threshold E_q,i,l - E_q,i,l+1 < δ_q:

\begin{eqnarray}
\nm{E_{q,i,l+1}\lrp{\Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}}} & = & \nm{\Bigr\lVert Log\big(\circled{\vec q}_{{\sss EC}il} \oplus \Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}\big) - Log\lrp{\circled{\vec q}_{{\sss EC}i}^{\circ\circ}}\Bigr\rVert} \nonumber \\
& = & \nm{\Bigr\lVert Log\big(Exp\lrp{\circled{\vec r}_{{\sss EC}il}} \oplus \Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}\big) - \circled{\vec r}_{{\sss EC}i}^{\circ\circ}\Bigr\rVert} \label{eq:nav_vis_assist_pose_optim_pitch_problem} \\
\nm{\circled{\vec q}_{{\sss EC}i,l+1}} & \nm{\longleftarrow} & \nm{\circled{\vec q}_{{\sss EC}il} \circ Exp\lrp{\Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}} = \circled{\vec q}_{{\sss EC}il} \oplus \Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}} \label{eq:nav_vis_assist_pose_optim_pitch_iterative}
\end{eqnarray}

Each Δr⃗_ECil^C represents the update to the camera attitude given by the rotation vector viewed in the local camera frame F_Cl, which is obtained by following the process described in [2, 64][Note that in this process the Jacobian coincides with the identity matrix because the map f⃗r⃗_ECi = r⃗_ECi coincides with the rotation vector itself.], and results in (<ref>), where J⃗_Ril is the 𝕊𝕆(3) right Jacobian J⃗_Rr⃗ provided by [2, 4][[2, 4] also include an expression for the right Jacobian inverse (J⃗_Ril^-1 = J⃗_R^-1r⃗).].

\begin{eqnarray}
\nm{\Delta \circled{\vec r}_{{\sss EC}il}^{\sss C}} & = & \nm{- \Big[\vec J_{Ril}^{-T} \ \vec J_{Ril}^{-1}\Big]^{-1} \ \vec J_{Ril}^{-T} \ \big[\circled{\vec r}_{{\sss EC}il} - \circled{\vec r}_{{\sss EC}i}^{\circ\circ} \big]} \nonumber \\
& = & \nm{- \Big[\vec J_{Ril}^{-T} \ \vec J_{Ril}^{-1}\Big]^{-1} \ \vec J_{Ril}^{-T} \ \big[Log\big(\circled{\vec q}_{{\sss EC}il}\big) - Log\big(\circled{\vec q}_{{\sss EC}i}^{\circ\circ}\big) \big]} \nonumber \\
& = & \nm{- \Big[\vec J_{Ril}^{-T} \ \vec J_{Ril}^{-1}\Big]^{-1} \ \vec J_{Ril}^{-T} \ {\vec E}_{q,il} \ \in \mathbb{R}^3} \label{eq:nav_vis_assist_pose_optim_pitch_solution} \\
\nm{\vec J_{Ril}} & = & \nm{\vec J_R\lrp{\circled{\vec r}_{{\sss EC}il}} = \vec J_R\Big(Log\lrp{\circled{\vec q}_{{\sss EC}il}}\Big) \ \ \ \in \mathbb{R}^{3x3}} \label{eq:nav_vis_assist_pose_optim_pitch_jacobian}
\end{eqnarray}

The prior based pose adjustment algorithm attempts to obtain the  camera pose ζ⃗_ECi that minimizes the reprojection error E_RP discussed in section <ref> combined with the weighted attitude adjustment error E_q. The specific weight f_q is discussed in section <ref>. Inspired in [65], the main goal of the optimization algorithm is to minimize the reprojection error of the different terrain 3D points while simultaneously trying to be close to the attitude and altitude targets derived from the inertial filter.
E_POζ⃗_ECi = E_RPζ⃗_ECi + f_q ·E_q(r⃗_ECi) eq:nav_vis_assist_pose_optim_joint_objective

Although the rotation vector r⃗_ECi = Logq⃗_ECi can be directly obtained from the pose ζ⃗_ECi [2, 4], merging the two algorithms requires a dimension change in the (<ref>) Jacobian, as indicated by (<ref>).
J⃗_RRil^-1 = [O⃗_3x3   J⃗_Ril^-1]   ∈ℝ^3x6eq:nav_vis_assist_optim_pitch_jacobian6

The application of the iterative process described in [65] results in the following solution, which combines the contributions from the two different optimization targets:

\begin{eqnarray}
\nm{\vec H_{PO,il}} & = & \nm{\Big[\sum_j w_{TUK}\lrp{{\vec E_{RP,ilj}}^T \vec E_{RP,ilj}} {\vec J_{OF,ilj}}^T \vec J_{OF,ilj}\Big] + \ f_q^2 \cdot \Big[\vec J_{RRil}^{-T} \ \vec J_{RRil}^{-1}\Big] \ \ \in \mathbb{R}^{6x6}} \label{eq:nav_vis_assist_optim_joint_hes} \\
\nm{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} & = & \nm{- \vec H_{PO,il}^{-1} \bigg[\Big[\sum_j w_{TUK}\lrp{{\vec E_{RP,ilj}}^T \vec E_{RP,ilj}} {\vec J_{OF,ilj}}^T \vec E_{RP,ilj}\Big] + \ f_q \cdot \vec J_{RRil}^{-T} \ {\vec E}_{q,il}\bigg]} \label{eq:nav_vis_assist_pose_optim_joint_solution} \\
\nm{\circled{\vec \zeta}_{{\sss EC}i,l+1}} & \nm{\longleftarrow} & \nm{\circled{\vec \zeta}_{{\sss EC}il} \circ Exp\lrp{\Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} = \circled{\vec \zeta}_{{\sss EC}il} \oplus \Delta \circled{\vec \tau}_{{\sss EC}il}^{\sss C}} \label{eq:nav_vis_assist_pose_optim_joint_iterative}
\end{eqnarray}

§.§ PI Control Inspired Pose Adjustment Activation

Sections <ref> and <ref> describe the attitude adjustment and its fusion with the default reprojection error minimization pose optimization algorithm, but they do not specify the conditions under which the adjustment is activated, how the q⃗_NBi^∘∘ ≡ϕ⃗_NBi^∘∘ target is determined, or the obtainment of its f_q relative weight when applying the (<ref>) joint optimization. These parameters are determined below in three different cases: an adjustment in which only pitch is controlled, an adjustment in which both pitch and bank angles are controlled, and a complete attitude adjustment.

* Pitch Adjustment Activation. The attitude adjustment described in (<ref>) through (<ref>) can be converted into a pitch only adjustment by forcing the yaw and bank angle targets to coincide in each optimization step i with the output of the reprojection only optimization:

\begin{eqnarray}
\nm{\circled{\psi}_i^{\circ\circ}} & = & \nm{\circled{\psi}_i^{\star\star}} \label{eq:nav_vis_assist_priors_activation_pitch_yaw_equal} \\
\nm{\circled{\theta}_i^{\circ\circ}} & = & \nm{\circled{\theta}_i^{\star\star} + \Delta \circled{\theta}_i^{\circ\circ}} \label{eq:nav_vis_assist_priors_activation_pitch_pitch} \\
\nm{\circled{\xi}_i^{\circ\circ}} & = & \nm{\circled{\xi}_i^{\star\star}} \label{eq:nav_vis_assist_priors_activation_pitch_bank_equal}
\end{eqnarray}

When activated as explained below, the new body pose target ζ⃗_EBi^∘∘ only differs in one out of six dimensions (the pitch) from the reprojection only E_RP optimum pose ζ⃗_EBi^⋆⋆, and the difference is very small as its effects are intended to accumulate over many successive images. This does not mean however that the other five components do not vary, as the joint optimization process described in (<ref>) through (<ref>) freely optimizes within 𝕊𝔼3 with six degrees of freedom to minimize the joint cost function E_PO that not only considers the reprojection error, but also the resulting pitch target.

The pitch adjustment aims for the visual estimations for altitude h_i and pitch θ_i (in this order) not to deviate in excess from their inertially estimated counterparts ĥ_n and θ̂_n. It is inspired in a proportional integral (PI) control scheme [66, 67, 68, 69] as the geometric altitude adjustment error[In this context, adjustment error is understood as the difference between the visual and inertial estimations.] Δh = h_i - ĥ_n can be considered as the integral of the pitch adjustment error Δθ= θ_i - θ̂_n in the sense that any difference between adjusted pitch angles (the P control) slowly accumulate over time generating differences in adjusted altitude (the I control). In addition, the adjustment also depends on the rate of climb (ROC) adjustment error[To avoid noise, this is smoothed over the last 100 images or 10  s.] ΔROC = ROC_i - R̂ÔĈ_n, which can be considered a second P control as ROC is the time derivative of the pressure altitude.

Variable     Value     Unit         Variable     Value     Unit

Δh_LOW             25.0        m                     Δθ_1,MAX^∘∘       0.0005      ^∘

Δθ_LOW        0.2         ^∘         Δθ_2,MAX^∘∘       0.0003      ^∘

ΔROC_LOW           0.01        m/s

Δξ_LOW  	        0.2         ^∘         Δξ_1,MAX^∘∘     0.0003      ^∘

tablePitch and bank adjustment settings

Note that the objective is not for the visual estimations to closely track the inertial ones, but only to avoid excessive deviations, so there exist lower thresholds Δh_LOW, Δθ_LOW, and ΔROC_LOW below which the adjustments are not activated. These thresholds are arbitrary but have been set taking into account the INS accuracy and its sources of error, as described in [5]. If the absolute value of a certain adjustment error (difference between the visual and estimated estates) is above its threshold, the IA-VNS can conclude with a high degree of confidence that the adjustment procedure can be applied; if below the threshold, the adjustment should not be employed as there is a significant risk that the true visual error (difference between the visual and actual state) may have the opposite sign, in which case the adjustment would be counterproductive.

As an example, let's consider a case in which the visual altitude h_i is significantly higher than the observed one ĥ_n, resulting in |Δh| > Δh_LOW; in this case the IA-VNS concludes that the aircraft is high and applies a negative pitch adjustment to slowly decrease the body pitch visual estimation  over many images, with these accumulating over time into a lower altitude  that what would be the case if no adjustment were applied. On the other hand, if the absolute value of the adjustment error is below the threshold (|Δh| < Δh_LOW), the adjustment should not be applied as there exists a significant risk that the aircraft is in fact low instead of high (when compared with the true altitude h_t, not the the observed one ĥ_n), and a negative pitch adjustment would only exacerbate the situation. A similar reasoning applies for the adjustment pitch error, in which the IA-VNS reacts or not to correct perceived nose-up or nose-down visual estimations. The applied thresholds are displayed in table <ref>.

The θ_i^∘∘ pitch target to be applied for each image is given by (<ref>), where the obtainment of the pitch adjustment Δθ_i^∘∘ is explained below based on its three components (<ref>):
Δθ_i^∘∘ = Δθ_h^∘∘ + Δθ_θ^∘∘ + Δθ_ROC^∘∘ eq:nav_vis_assist_priors_activation_pitch_pitch_formula

* The pitch adjustment due to altitude, Δθ_h^∘∘, linearly varies between zero when the adjustment error is below the threshold Δh_LOW to Δθ_1,MAX^∘∘ when the error is twice the threshold, as shown in (<ref>). The adjustment is bounded at this value to avoid destabilizing SVO with pose adjustments that differ too much from their reprojection only optimum ζ⃗_EBi^⋆⋆ (<ref>).

Δθ_h^∘∘ =
0     when |Δh| < Δh_LOW

-  signΔh   Δθ_1,MAX^∘∘   |Δh| - Δh_LOW / Δh_LOW     Δh_LOW ≤|Δh| ≤2 ·Δh_LOW

-  signΔh   Δθ_1,MAX^∘∘     when |Δh| > 2 ·Δh_LOW

* The pitch adjustment due to pitch, Δθ_θ^∘∘, works similarly but employing Δθ instead of Δh and Δθ_LOW instead of Δh_LOW, while also relying on the same limit Δθ_1,MAX^∘∘. In addition, Δθ_θ^∘∘ is set to zero if its sign differs from that of Δθ_h^∘∘, and reduced so the combined effect of both targets does not exceed the limit (|Δθ_h^∘∘ + Δθ_θ^∘∘| ≤Δθ_1,MAX^∘∘).

* The pitch adjustment due to rate of climb, Δθ_ROC^∘∘, also follows a similar scheme but employing ΔROC instead of Δh, ΔROC_LOW instead of Δh_LOW, and Δθ_2,MAX^∘∘ instead of Δθ_1,MAX^∘∘. Additionally, it is multiplied by the ratio between Δθ_h^∘∘ and Δθ_1,MAX^∘∘ to limit its effects when the altitude estimated error Δh is small. This adjustment can act in both directions, imposing bigger pitch adjustments if the altitude error is increasing or lower one if it is already diminishing.

If activated, the weight value f_q required for the (<ref>) joint optimization is determined by imposing that the weighted attitude error f_q ·E_q(r⃗_ECi0) coincides with the reprojection error E_RP(ζ⃗_ECi0) when evaluated before the first iteration, this is, it assigns the same weight to the two active components of the joint E_PO(ζ⃗_ECi) cost function (<ref>).

* Pitch and Bank Adjustment Activation. The previous scheme can be modified to also make use of the observed body bank angle ξ̂_n within the framework established by the (<ref>) through (<ref>) attitude adjustment optimization:

\begin{eqnarray}
\nm{\circled{\psi}_i^{\circ\circ}} & = & \nm{\circled{\psi}_i^{\star\star}} \label{eq:nav_vis_assist_priors_activation_pitchbank_yaw_equal} \\
\nm{\circled{\theta}_i^{\circ\circ}} & = & \nm{\circled{\theta}_i^{\star\star} + \Delta \circled{\theta}_i^{\circ\circ}} \label{eq:nav_vis_assist_priors_activation_pitchbank_pitch} \\
\nm{\circled{\xi}_i^{\circ\circ}} & = & \nm{\circled{\xi}_i^{\star\star} + \Delta \circled{\xi}_i^{\circ\circ}} \label{eq:nav_vis_assist_priors_activation_pitch_bankbank_bank}
\end{eqnarray}

Although the new body pose target ζ⃗_EBi^∘∘ only differs in two out of six dimensions (pitch and bank) from the optimum pose ζ⃗_EBi^⋆⋆ obtained by minimizing the reprojection error exclusively, all six degrees of freedom are allowed to vary when minimizing the joint cost function.

The determination of the pitch adjustment Δθ_i^∘∘ does not vary, and that of the bank adjustment Δξ_i^∘∘ relies on a linear adjustment between two values similar to any of the three components of (<ref>), but relying on the bank angle adjustment error Δξ= ξ_i - ξ̂_n, as well as a Δξ_LOW threshold and Δξ_1,MAX^∘∘ maximum adjustment whose values are provided in table <ref>. Note that the value of the Δξ_LOW threshold coincides with that of Δθ_LOW as the INS accuracy for both pitch and roll is similar according to [5].

It is important to remark that the combined pitch and bank adjustment activation is the one employed to generate the IA-VNS results in sections <ref> and <ref>.

* Attitude Adjustment Activation. The use of the observed yaw angle  is not recommended as the visual estimation  provided by the VNS (without any inertial inputs) is in general more accurate than its inertial counterpart , as shown in section <ref> (table <ref>). This can be traced on one side to the bigger influence that a yaw change has on the resulting optical flow when compared with those caused by pitch and bank changes, which makes the body yaw angle easier to track by the VNS, and on the other to the INS relying on the gravity pointing down to control pitch and bank adjustments versus the less robust dependence on the Earth magnetic field and associated magnetometer readings used to estimate the aircraft heading [5].

For this reason, the attitude adjustment process described next has not been implemented, although it is included here as a suggestion for other applications in which the objective may be to adjust the vehicle attitude as a whole. The process relies on the observed attitude q̂⃗̂_NBn provided by the INS and the initial estimation q⃗_NBi^⋆⋆ provided by the reprojection only pose optimization process. Its difference is given by Δr⃗^Bi,⋆⋆ = q̂⃗̂_NBn ⊖q⃗_NBi^⋆⋆, where the superindex  indicates that it is viewed in the pose optimized body frame. This perturbation can be decoupled into  a rotating direction and an angular displacement [2, 4], resulting in Δr⃗^Bi,⋆⋆ = n⃗^Bi,⋆⋆  Δϕ^⋆⋆.

Let's now consider that the IA-VNS decides to set an attitude target that differs by Δϕ^∘∘ from its reprojection only solution q⃗_NBi^⋆⋆, but rotating about the axis that leads towards its inertial estimation q̂⃗̂_NBn. The target attitude q⃗_NBi^∘∘ can then be obtained by 𝕊𝕆(3) Spherical Linear Interpolation (SLERP) [2, 3], where t = Δϕ^∘∘  /  Δϕ^⋆⋆ is the ratio between the target rotation and the attitude error or estimated angular displacement:
q⃗_NBi^∘∘ = q⃗_NBi^⋆⋆ ⊗q⃗_NBi^⋆⋆^∗⊗q̂⃗̂_NBn^teq:nav_vis_assist_priors_activation_pitch_atttitude

§.§ Additional Modifications to SVO

In addition to the PI inspired introduction of priors into the pose optimization phase, the proposed IA-VNS also includes various other modifications to the original SVO pipeline described in section <ref>. These include the addition of the current features to the structure optimization phase (so the pose adjustments introduced by the prior based pose optimization are not reverted), the replacement of the sparse image alignment phase by an inertial estimation of the ζ⃗_ECi0 = ζ⃗_ECi^⋆ input to the pose optimization process, and the use of the GNSS-Based inertial distance estimations to obtain more accurate height and path angle values for the SVO initialization.

§ TESTING: HIGH FIDELITY SIMULATION AND SCENARIOS

To evaluate the performance of the proposed visual navigation algorithms, this article relies on Monte Carlo simulations consisting of one hundred runs each of two different scenarios based on the high fidelity stochastic flight simulator graphically depicted in figure <ref>. Described in detail in [7] and with its open source  implementation available in [1], the simulator models the flight in varying weather and turbulent conditions of a low SWaP fixed wing piston engine autonomous UAV.

[auto, node distance=2cm,>=latex']

[coordinate](xrefinput) ;
[blockgreen, right of=xrefinput, minimum width=2.6cm, node distance=3.0cm, align=center, minimum height=1.25cm] (GUIDANCE) ;
[->] (xrefinput) – node[pos=0.4]  (GUIDANCE.west);

[blockgreen, right of=GUIDANCE, minimum width=2.6cm, node distance=5.0cm, align=center, minimum height=1.25cm] (CONTROL) ;
[->] (GUIDANCE.east) – node[pos=0.5]  (CONTROL.west);

[blockyellow, right of=CONTROL, minimum width=1.8cm, node distance=3.5cm, align=center, minimum height=1.25cm] (AIRCRAFT) ;
[->] (CONTROL.east) –  node[pos=0.5]  (AIRCRAFT.west);

[blockyellow, right of=AIRCRAFT, minimum width=1.8cm, node distance=2.6cm, align=center, minimum height=1.25cm] (EARTH) ;
[->] (EARTH.west) – (AIRCRAFT.east);

[coordinate, right of=AIRCRAFT, node distance=1.3cm] (midpoint);
[blockyellow, below of=midpoint, minimum width=1.8cm, node distance=2.5cm, align=center, minimum height=1.25cm] (FLIGHT) ;
[->] ($(EARTH.south)-(0.50cm,0cm)$) |- ($(FLIGHT.north)+(0.20cm,0.70cm)$) – ($(FLIGHT.north)+(0.20cm,0cm)$);
[->] ($(AIRCRAFT.south)+(0.50cm,0cm)$) |- ($(FLIGHT.north)+(-0.20cm,0.70cm)$) – ($(FLIGHT.north)-(0.20cm,0cm)$);

[blockgreen, below of=GUIDANCE, minimum width=2.6cm, node distance=2.5cm, align=center, minimum height=1.25cm] (NAVIGATION) ;
[coordinate, left of=NAVIGATION, node distance=2.0cm] (pointnav1);
[coordinate, above of=pointnav1, node distance=1.2cm] (pointnav2);
[coordinate, below of=GUIDANCE, node distance=1.3cm] (pointnav3);
[black] (pointnav3) circle [radius=1pt];
[->] ($(NAVIGATION.west)+(0cm,0.3cm)$) -| (pointnav2) – (pointnav3) – (GUIDANCE.south);
[->] (pointnav3) -| node[pos=0.25] =  (CONTROL.south);
[->] ($(NAVIGATION.west)+(-1.5cm,-0.3cm)$) – node[pos=0.4]  ($(NAVIGATION.west)+(0cm,-0.3cm)$);

[blockgreen, below of=CONTROL, minimum width=2.6cm, node distance=2.5cm, align=center, minimum height=1.25cm] (SENSORS) ;
[->] (SENSORS.west) – node[pos=0.5] =  (NAVIGATION.east);

[coordinate, right of=EARTH, node distance=1.5cm] (pointflight1);
[coordinate, above of=EARTH, node distance=1.1cm] (pointflight2);
[black] (pointflight2) circle [radius=1pt];
[->] (FLIGHT.west) – node[pos=0.5] =  (SENSORS.east);
[->] (FLIGHT.east) -| node[pos=0.25] =  (pointflight1) |- (pointflight2) -| (AIRCRAFT.north);
[->] (pointflight2) – (EARTH.north);

Components of the high fidelity simulation

The simulator consists on two distinct processes. The first, represented by the yellow blocks on the right of figure <ref>, models the physics of flight and the interaction between the aircraft and its surroundings that results in the real aircraft trajectory = ; the second, represented by the green blocks on the left, contains the aircraft systems in charge of ensuring that the resulting trajectory adheres as much as possible to the mission objectives. It includes the different sensors whose output comprise the sensed trajectory = , the navigation system in charge of filtering it to obtain the estimated trajectory = , the guidance system that converts the reference objectives  into the control targets , and the control system that adjusts the position of the throttle and aerodynamic control surfaces  so the estimated trajectory  is as close as possible to the reference objectives . Table <ref> provides the working frequencies employed for the different trajectories shown in figures <ref>, <ref>, <ref>, and <ref>.

1cDiscrete Time     Frequency     Period     Variables     Systems

t_t = t ·                500  Hz     0.002  s     =                  Flight physics

t_s = s ·               100  Hz     0.01  s      =            Sensors

t_n = n ·                  100  Hz     0.01  s      =                 Inertial navigation

t_c = c ·                  50  Hz     0.02  s      ,        Guidance & control

t_i = i ·                   10  Hz     0.1  s       =                 Visual navigation & camera

tableWorking frequencies of the different systems and trajectory representations

All components of the flight simulator have been modeled with as few simplifications as possible to increase the realism of the results, as explained in [7, 70]. With the exception of the aircraft performances and its control system, which are deterministic, all other simulator components are treated as stochastic and hence vary from one execution to the next, enhancing the significance of the Monte Carlo simulation results.

§.§ Camera

The flight simulator has the capability, when provided with the camera pose[The camera is positioned facing down and rigidly attached to the aircraft structure.] with respect to the Earth at equally time spaced intervals, of generating images that resemble the view of the Earth surface that the camera would record if located at that particular pose. To do so, it relies on the  library, a modification to  [71] (which in turn relies on  [72]) capable of generating realistic Earth images as long as the camera height over the terrain is significantly higher than the vertical relief present in the image. A more detailed explanation of the image generation process is provided in [70].

It is assumed that the shutter speed is sufficiently high that all images are equally sharp, and that the image generation process is instantaneous. In addition, the camera ISO setting remains constant during the flight, and all generated images are noise free. The simulation also assumes that the visible spectrum radiation reaching all patches of the Earth surface remains constant, and the terrain is considered Lambertian [73], so its appearance at any given time does not vary with the viewing direction. The combined use of these assumptions implies that a given terrain object is represented with the same luminosity in all images, even as its relative pose (position and attitude) with respect to the camera varies. Geometrically, the simulation adopts a perspective projection or pinhole camera model [73], which in addition is perfectly calibrated and hence shows no distortion. The camera has a focal length of 19   mm and a sensor with 768 by 1024 pixels.

§.§ Scenarios

Most VIO packages discussed in section <ref> include in their release articles an evaluation when applied to the  Micro Air Vehicle (MAV) datasets [51], and so do independent articles such as [52]. These datasets contain perfectly synchronized stereo images, IMU measurements, and ground truth readings obtained with laser, for eleven different indoor trajectories flown with a MAV, each with a duration in the order of two minutes and a total distance in the order of 100   m. This fact by itself indicates that the target application of exiting VIO implementations differs significantly from the main focus of this article, which is the long term flight of a fixed wing UAV in GNSS-Denied conditions, as there may exist accumulating errors that are completely non discernible after such short periods of time, but that grow non linearly and have the capability of inducing significant pose errors when the aircraft remains aloft for long periods of time.

The algorithms introduced in this article are hence tested through simulation under two different scenarios designed to analyze the consequences of losing the GNSS signals for long periods of time. Although a short summary is included below, detailed descriptions of the mission, weather, and wind field employed in each scenario can be found in [7]. Most parameters comprising the scenario are defined stochastically, resulting in different values for every execution. Note that all results shown in sections <ref> and <ref> are based on Monte Carlo simulations comprising one hundred runs of each scenario, testing the sensitivity of the proposed navigation algorithms to a wide variety of values in the parameters.

* Scenario #1 has been defined with the objective of adequately representing the challenges faced by an autonomous fixed wing UAV that suddenly cannot rely on GNSS and hence changes course to reach a predefined recovery location situated at approximately one hour of flight time. In the process, in addition to executing an altitude and airspeed adjustment, the autonomous aircraft faces significant weather and wind field changes that make its GNSS-Denied navigation even more challenging.

With respect to the mission, the stochastic parameters include the initial airspeed, pressure altitude, and bearing (, , ), their final values (, , ), and the time at which each of the three maneuvers is initiated[Turns are executed with a bank angle of = ±10   ^∘, altitude changes employ an aerodynamic path angle of = ±2   ^∘, and airspeed modifications are automatically executed by the control system as set-point changes.]. The scenario lasts for = 3800   s, while the GNSS signals are lost at = 100   s.

The wind field is also defined stochastically, as its two parameters (speed and bearing) are constant both at the beginning (, ) and conclusion (, ) of the scenario, with a linear transition in between. The specific times at which the wind change starts and concludes also vary stochastically among the different simulation runs. As described in [7], the turbulence remains strong throughout the whole scenario, but its specific values also vary stochastically from one execution to the next.

A similar linear transition occurs with the temperature and pressure offsets that define the atmospheric properties [6], as they are constant both at the start (, ) and end (, ) of the flight. In contrast with the wind field, the specific times at which the two transitions start and conclude are not only stochastic but also different from each other.

* Scenario #2 represents the challenges involved in continuing with the original mission upon the loss of the GNSS signals, executing a series of continuous turn maneuvers over a relatively short period of time with no atmospheric or wind variations. As in scenario #1, the GNSS signals are lost at = 100   s, but the scenario duration is shorter (= 500   s). The initial airspeed and pressure altitude (, ) are defined stochastically and do not change throughout the whole scenario; the bearing however changes a total of eight times between its initial and final values, with all intermediate bearing values as well as the time for each turn varying stochastically from one execution to the next. Although the same turbulence is employed as in scenario #1, the wind and atmospheric parameters (, , , ) remain constant throughout scenario #2.

§ VISUAL NAVIGATION SYSTEM ERROR IN GNSS-DENIED CONDITIONS

This section presents the results obtained with the proposed IA-VNS when executing Monte Carlo simulations of the two GNSS-Denied scenarios over the MX terrain type[Section <ref> defines the various terrain types than can be overflown by the aircraft, and then analyzes their influence on the simulation results.], each consisting of one hundred executions. They are compared with the results obtained with the stand alone VNS that relies on the original SVO algorithm, and with those of the INS described in [5]. The tables below contain the navigation system error[The NSE is the difference between the real states () and their estimation ( or ) by the navigation system.] (NSE) incurred by the various navigation systems (and accordingly denoted as INSE, VNSE, and IA-VNSE) at the conclusion of the two GNSS-Denied scenarios, represented by the mean, standard deviation, and maximum value of the estimation errors. In addition, the figures depict the variation with time of the NSE mean and standard deviation for the one hundred executions. The following remarks are necessary:

* The results obtained with the INS under the same two GNSS-Denied scenarios are described in detail in [5]. The attitude INSE does not drift and is bounded by the quality of the onboard sensors, ensuring the aircraft can remain aloft for as long as there is fuel available. The vertical position and ground velocity INSEs are also bounded by atmospheric physics and do not drift; their estimation errors depend on the atmospheric pressure offset and wind field changes that occur since the GNSS signals are lost. On the other hand, the horizontal position INSE drifts as a consequence of integrating the ground velocity without absolute observations. Of the six 𝕊𝔼(3) degrees of freedom (three for attitude, two for horizontal position, one for altitude), the INS is hence capable of successfully estimating four of them in GNSS-Denied conditions.

* Visual navigation systems (either VNS or IA-VNS) are only necessary to reduce the estimation error in the two remaining degrees of freedom (the horizontal position). Although both of them estimate the complete six dimensional aircraft pose, their attitude and altitude estimations shall only be understood as a means to provide an accurate horizontal position estimation, which represents their sole objective.

* As described below, the SVO based VNS drifts in all six degrees of freedom. The main focus of this article is on how the addition of INS based priors enables the IA-VNS to reduce the drift in all six dimensions, with the resulting horizontal position IA-VNSE being just a fraction of the INSE. The attitude and altitude IA-VNSEs, although improved when compared to the VNSEs, are qualitatively inferior to the driftless INSEs, but note that their purpose is just to enable better horizontal position IA-VNS estimations, not to replace the attitude and altitude INS outputs.

§.§ Body Attitude Estimation

Table <ref> shows the NSE at the conclusion of both scenarios for the three Euler angles representing the body attitude (yaw ψ, pitch θ, roll ξ)[The yaw angle estimation errors respond to Δ= - ψ and Δ= - ψ, respectively. Those for the body pitch and roll angles are defined accordingly.], as well as the norm of the rotation vector between the real body attitude  and its estimations,  by the INS and  by the VNS or IA-VNS. The errors hence can be formally written as = ⊖ or = ⊖ [2]. In addition, figures <ref> and <ref> depict the variation with time of the body attitude NSE for both scenarios, while figure <ref> shows those of each individual Euler angle for scenario #1 exclusively.

* After a short transition period following the introduction of GNSS-Denied conditions at = 100   s, the body attitude INSE (blue lines) does not experience any drift with time in either scenario, and is bounded by the quality of the onboard sensors and the inertial navigation algorithms [5].

* With respect to the VNSE (red lines), most of the scenario #1 error is incurred during the turn maneuver at the beginning of the scenario (refer to  within [7]), with only a slow accumulation during the rest of the trajectory, composed by a long straight flight with punctual changes in altitude and speed. Additional error growth would certainly accumulate if more turns were to occur, although this is not tested in the simulation. This statement seems to contradict the results obtained with scenario #2, in which the error grows with the initial turns but then stabilizes during the rest of the scenario, even though the aircraft is executing continuous turn maneuvers. This lack of error growth occurs because the scenario #2 trajectories are so twisted (refer to [7]) that terrain zones previously mapped reappear in the camera field of view during the consecutive turns, and are hence employed by the pose optimization phase as absolute references, resulting in a much better attitude estimation that what would occur under more spaced turns. A more detailed analysis (not shown in the figures) shows that the estimation error does not occur during the whole duration of the turns, but only during the roll-in and final roll-out maneuvers, where the optical flow is highest and hence more difficult to track by SVO[For the two evaluated scenarios, the optical flow during the roll-in and roll-out maneuvers is significantly higher than that induced by straight flight, pull-up and push-down maneuvers, and even the turning maneuvers themselves (once the bank angle is no longer changing).].

4cINSE     4cVNSE     4cIA-VNSE

^∘     Δ     Δ     Δ          Δ     Δ     Δ          Δ     Δ     Δ

4lScenario #1 MX

mean     +0.03     -0.03     -0.00     0.158     +0.03     +0.08     +0.00     0.296     +0.03     -0.01     -0.03     0.218

std       0.18      0.05      0.06             0.114       0.13      0.23      0.21             0.158       0.11      0.16      0.14             0.103

max      -0.61     -0.27     -0.23             0.611      +0.63     +0.74     +0.78             0.791      +0.55     -0.37     -0.51             0.606

4lScenario #2 MX

mean     -0.02     +0.01     +0.00     0.128     +0.02     -0.02     +0.00     0.253     +0.02     -0.00     +0.01     0.221

std       0.13      0.05      0.05             0.078       0.08      0.21      0.20             0.161       0.08      0.16      0.19             0.137

max      +0.33     -0.15     +0.15             0.369      +0.22     -0.65     -0.73             0.730      +0.24     +0.62     +0.74             0.788

tableAggregated  final body attitude , , and  (100 runs)

* The IA-VNSE results (green lines) show that the introduction of priors in section <ref> works as intended and there exists a clear benefit for the use of an IA-VNS when compared to the stand alone VNS described in section <ref>. In spite of IA-VNSE values at the beginning of both scenarios that are nearly double those of the VNSE (refer to figures <ref> and <ref>), caused by the initial pitch adjustment required to improve the fit between the homography output and the inertial estimations (section <ref>), the balance between both errors quickly flips as soon as the aircraft starts maneuvering, resulting in body attitude IA-VNSE values significantly lower than those of the VNSE for the remaining part of both scenarios. This improvement is more significant in the case of scenario #1, as the prior based pose optimization is by design a slow adjustment that requires significant time to slowly correct attitude and altitude deviations between the visual and inertial estimations.

every axis legend/.append style=

cycle list=

blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,
xlabel=t s,
ymin=0, ymax=0.5, ytick=0,0.1,0.2,0.3,0.4,0.5,
ylabel=,     ^∘,
axis lines=left,
axis line style=-stealth,
legend entries=

± INSE,
± VNSE,
± IA-VNSE,
legend columns=3,
legend style=font=,
legend cell align=left,

table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (750,0.45) Scenario #1 MX;

Body attitude , , and  for scenario #1  (100 runs)

Qualitatively, the biggest difference between the three estimations resides in the nature of the errors. While the attitude INSE is bounded, drift is present in both the VNS and IA-VNS estimations. The drift resulting from the Monte Carlo simulations may be small, and so is the attitude estimation error , but more challenging conditions with more drastic maneuvers and a less idealized image generation process than that described in section <ref> may generate additional drift.

Focusing now on the quantitative results shown in table <ref>, aggregated errors for each individual Euler angle are always unbiased and zero mean for each of the three estimations (INS, VNS, IA-VNS), as the means tend to zero as the number of runs grows, and are much smaller than both the standard deviations and the maximum values. With respect to the attitude error  and , their aggregated means are not zero (they are norms), but are nevertheless quite repetitive in all three cases, as the mean is always significantly higher than the standard deviation, while the maximum values only represent a small multiple of the means. It is interesting to point out that while in the case of the INSE the contribution of the yaw error is significantly higher than that of the pitch and roll errors, the opposite occurs for both the VNSE and the IA-VNSE. This makes sense as the the gravity direction is employed by the INS as a reference from where the estimated pitch and roll angles can not deviate, but slow changes in yaw generate larger optical flow variations than those caused by pitch and roll variations.

every axis legend/.append style=

cycle list=blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
xmin=0, xmax=500, xtick=0,50,...,500,
xlabel=t s,
ymin=0, ymax=0.5, ytick=0,0.1,0.2,0.3,0.4,0.5,
ylabel=,     ^∘,
axis lines=left,
axis line style=-stealth,
legend entries= ± INSE,
± VNSE,
± IA-VNSE,
legend columns=3,
legend style=font=,
legend cell align=left,

table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (80,0.40) Scenario #2 MX;

Body attitude , , and  for scenario #2  (100 runs)

These results prove that the IA-VNS succeeds when employing the inertially observed pitch and bank angles (, ), whose errors are bounded, to limit the drift of their visual counterparts (, ), as  and [As the individual Euler angle metrics are unbiased or zero mean, the benefits of the proposed approach are reflected in the variation of the remaining metrics, this is, the standard deviation and the maximum value.] are significantly lower for the IA-VNS than for the VNS. Remarkably, this is achieved with no degradation in the body yaw angle, as  remains stable. Note that adjusting the output of certain variables in a minimization algorithm (such as pose optimization) usually results in a degradation in the accuracy of the remaining variables as the solution moves away from the true optimum. In this case, however, the improved fit between the adjusted aircraft pose and the terrain displayed in the images, results in the SVO pipeline also slightly improving its body yaw estimation . Section <ref> shows how the benefits of an improved fit between the displayed terrain and the adjusted pose also improve the horizontal position estimation.

every axis legend/.append style=

cycle list=blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
blue,dashed,no markers,blue,dashed,no markers,
red,dashed,no markers,red,dashed,no markers,
green,dashed,no markers,green,dashed,no markers,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,

ymin=-0.2, ymax=0.3, ytick=-0.3,-0.2,-0.1,0,0.1,0.2,0.3,
ylabel=Δ,   Δ  ^∘,
axis lines=left,
axis line style=-stealth,
legend entries=Δ ±Δ INSE,
Δ ±Δ VNSE,
Δ ±Δ IA-VNSE,
legend columns=3,
legend style=font=,
legend cell align=left,
figs/ch18_res_vis_assist/error_vis_att/error_vis_base_psi_deg_nav.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_psi_deg_vis_relocalization.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_psi_deg_vis.txt[] (0.0,0.0) – (3800.0,0.0);
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (750,0.25) Scenario #1 MX;

every axis legend/.append style=

cycle list=blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
blue,dashed,no markers,blue,dashed,no markers,
red,dashed,no markers,red,dashed,no markers,
green,dashed,no markers,green,dashed,no markers,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,

ymin=-0.2, ymax=0.3, ytick=-0.2,-0.1,0,0.1,0.2,0.3,
ylabel=Δ,   Δ  ^∘,
axis lines=left,
axis line style=-stealth,
legend entries=Δ ±Δ INSE,
Δ ±Δ VNSE,
Δ ±Δ IA-VNSE,
legend columns=3,
legend style=font=,
legend cell align=left,
figs/ch18_res_vis_assist/error_vis_att/error_vis_base_theta_deg_nav.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_theta_deg_vis_relocalization.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_theta_deg_vis.txt[] (0.0,0.0) – (3800.0,0.0);
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (750,0.25) Scenario #1 MX;

every axis legend/.append style=

cycle list=blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
blue,dashed,no markers,blue,dashed,no markers,
red,dashed,no markers,red,dashed,no markers,
green,dashed,no markers,green,dashed,no markers,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,
xlabel=t s,
ymin=-0.2, ymax=0.3, ytick=-0.2,-0.1,0,0.1,0.2,0.3,
ylabel=Δ,   Δ  ^∘,
axis lines=left,
axis line style=-stealth,
legend entries=Δ ±Δ INSE,
Δ ±Δ VNSE,
Δ ±Δ IA-VNSE,
legend columns=3,
legend style=font=,
legend cell align=left,
figs/ch18_res_vis_assist/error_vis_att/error_vis_base_xi_deg_nav.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_xi_deg_vis_relocalization.txtfigs/ch18_res_vis_assist/error_vis_att/error_vis_base_xi_deg_vis.txt[] (0.0,0.0) – (3800.0,0.0);
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (750,0.25) Scenario #1 MX;

Body Euler angles , , and  for scenario #1  (100 runs)

In the case of a real life scenario based on a more realistic image generation process than that described in section <ref>, the VNS would likely incur in additional body attitude drift than in the simulations. If this were to occur, the IA-VNS pose adjustment algorithms described in section <ref> would react more aggressively to counteract the higher pitch and bank deviations, eliminating most of the extra drift, although it is possible that higher pose adjustment parameters than those listed in table <ref> would be required. The IA-VNS is hence more resilient against high drift values than the VNS.

§.§ Vertical Position Estimation

Table <ref> contains the vertical position NSE (Δ= - h, Δ= - h) at the conclusion of both scenarios, which can be considered unbiased or zero mean in all six cases (two scenarios and three estimation methods) as the mean h is always significantly lower than both the standard deviation h or the maximum value h. The NSE evolution with time is depicted in figures <ref> and <ref>, which also include (magenta lines) those Monte Carlo executions that result in the highest IA-VNSEs.

2lScenario MX      1cINSE         1cVNSE         1cIA-VNSE

m     1cΔ         1cΔ         1cΔ

3*#1     mean              -4.18                   +82.91                   +22.86

std       25.78          287.58           49.17

max              -70.49                  +838.32                  +175.76

3*#2     mean              +0.76                   +3.45                   +3.59

std        7.55          20.56          13.01

max              -19.86                  +72.69                  +71.64

tableAggregated  final vertical position , , and  (100 runs)

* The geometric altitude INSE (blue lines) is bounded by the change in atmospheric pressure offset since the time the GNSS signals are lost. Refer to [5] for additional information.

* The VNS estimation of the geometric altitude (red lines) is worse than that by the INS both qualitatively and quantitatively, even with the results being optimistic because of the ideal image generation process employed in the simulation. A continuous drift or error growth with time is present, and results in final errors much higher than those obtained with the GNSS-Denied inertial filter. These errors are logically bigger for scenario #1 because of its much longer duration.

every axis legend/.append style=

cycle list=
blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
magenta,no markers,thick,

blue,dashed,no markers,blue,dashed,no markers,
red,dashed,no markers,red,dashed,no markers,
green,dashed,no markers,green,dashed,no markers,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,
xlabel=t s,
ymin=-200, ymax=300, ytick=-200,-100,...,300,
ylabel=Δ,   Δ  m,
axis lines=left,
axis line style=-stealth,
legend entries=
Δ ±Δ INSE,
Δ ±Δ VNSE,
Δ ±Δ IA-VNSE,
Δ IA-VNSE,

legend columns=4,
legend style=font=,
legend cell align=left,
[] (0.0,0.0) – (3800.0,0.0);
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=5] ;

table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (750,-150) Scenario #1 MX;

Vertical position , , and  for scenario #1  (100 runs)

A small percentage of this drift can be attributed to the slow accumulation of error inherent to the SVO motion thread algorithms introduced in section <ref>, but most of it results from adding the estimated relative pose between two consecutive images to a pose (that of the previous image) with an attitude that already possesses a small pitch error (refer to the attitude estimation analysis in section <ref>). Note that even a fraction of a degree deviation in pitch can result in hundreds of meters in vertical error when applied to the total distance flown in scenario #1, as SVO can be very precise when estimating pose changes between consecutive images, but lacks any absolute reference to avoid slowly accumulating these errors over time. This fact is precisely the reason why the vertical position VNSE grows more slowly in the second half of scenario #2, as shown in figure <ref>. As explained in section <ref> above, continuous turn maneuvers cause previously mapped terrain points to reappear in the camera field of view, stopping the growth in the attitude error (pitch included), which indirectly has the effect of slowing the growth in altitude estimation error.

* The benefits of introducing priors to limit the differences between the visual and inertial altitude estimations are reflected in the IA-VNSE (green lines). The error reduction is drastic in the case of the scenario #1, where its extended duration allows the pose optimization small pitch adjustments to accumulate into significant altitude corrections over time, and less pronounced but nevertheless significant for scenario #2, where the VNSE (an hence also the IA-VNSE) results already benefit from previously mapped terrain points reappearing in the aircraft field of view as a result of the continuous maneuvers. It is necessary to remark the amount of the improvement, as the final standard deviation h diminishes from 287.58 to 49.17   m for scenario #1, and from 20.56 to 13.01   m for scenario #2.

The benefits of the prior based pose optimization algorithm can be clearly observed in the case of the scenario #1 execution with the worst final altitude estimation error, whose error variation with time is depicted in figure <ref> (magenta line). After a rapid growth in the first third of the scenario following a particularly negative estimation during the initial turn, the altitude error reaches a maximum of +233.05   m at 2007.5   s. Attitude adjustment has become active long before, lowering the estimated pitch angle to first diminish the growth of the altitude error and then being able to reduce the error itself, reaching a final value of +175.76   m at . As soon as the differences between the visual pitch, bank, or altitude estimations (,   ,   ) and their inertial counterparts (,   ,   ) exceed certain limits (section <ref>), the attitude adjustment comes into play and slowly adjusts the aircraft pitch to prevent the visual altitude from deviating in excess from the inertial one. This behavior not only improves the IA-VNS altitude estimation accuracy when compared to that of the VNS, but also its resilience, as the system actively opposes elevated altitude errors.

every axis legend/.append style=

cycle list=
blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
magenta,no markers,thick,
blue,dashed,no markers,blue,dashed,no markers,
red,dashed,no markers,red,dashed,no markers,
green,dashed,no markers,green,dashed,no markers,
xmin=0, xmax=500, xtick=0,50,...,500,
xlabel=t s,
ymin=-20, ymax=40, ytick=-20,-10,...,40,
ylabel=Δ,   Δ  m,
axis lines=left,
axis line style=-stealth,
legend entries=
Δ ±Δ INSE,
Δ ±Δ VNSE,
Δ ±Δ IA-VNSE,
Δ IA-VNSE,
legend columns=4,
legend style=font=,
legend cell align=left,
[] (0.0,0.0) – (500.0,0.0);
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=5] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (425,35) Scenario #2 MX;

Vertical position , , and  for scenario #2  (100 runs)

Significantly better altitude estimation errors (closer to the inertial ones) could be obtained if more aggressive settings were employed for Δθ_1,MAX^∘∘ and Δθ_2,MAX^∘∘ within table <ref>, as the selected values are far from the level at which the pose optimization convergence is compromised. This would result in more aggressive adjustments and important accuracy improvements for those cases in which altitude error growth is highest. The settings employed in this article are modest, as the final objective is not to obtain the smallest possible attitude or vertical position IA-VNSE (as they are always bigger than their INSE counterparts), but to limit them to acceptable levels so SVO can build a more accurate terrain map, improving the fit between the multiple terrain 3D points displayed in the images and the estimated aircraft pose. To do so it is mandatory to balance the pitch and bank angle adjustments with the need to stick to solutions close to those that minimize the reprojection error, as explained in section <ref>. Higher Δθ_1,MAX^∘∘ and Δθ_2,MAX^∘∘ accelerate the adjustments but may decrease the quality of the map. It is expected that a better rendition of the real 3D position of the features detected in the keyframes as they are tracked along successive images will lower the incremental horizontal displacement errors, and hence result in a lower horizontal position IA-VNSE, which is the real objective for the introduction of the priors.

The IA-VNS altitude estimation improvements over those of the VNS are not only quantitative. Figure <ref> shows no increment in  (green lines) in the second half of scenario #1 (once on average the deviation has activated the attitude adjustment feature). The altitude estimation by the IA-VNS can hence also be described as bounded and driftless, which represents a qualitative and not only quantitative improvement over that of the VNS. The bounds are obviously bigger for the IA-VNS than for the INS. In the case of scenario #2, figure <ref> shows a slow but steady  growth with time, but this is only because the error amount on average is not yet significant enough to activate the attitude adjustment feature within pose optimization.

§.§ Horizontal Position Estimation

The horizontal position estimation capabilities of the INS, VNS, and IA-VNS share the fact that all of them exhibit an unrestrained drift or growth with time, as shown in figures <ref> and <ref>. The errors obtained at the end of both scenarios are shown in table <ref>, following the same scheme as in previous sections. While the approximately linear INS drift appears when integrating the bounded ground velocity errors [5], the visual drifts (both VNS and IA-VNS) originate in the slow accumulation of errors caused by the concatenation of the relative poses between consecutive images without absolute references, but also show a direct relationship with the scale error committed when estimating the aircraft height over the terrain during the initial homography.

3lScenario MX       2cINSE            2cVNSE         2cIA-VNSE

1cDistance     2cΔx̂_HOR         2cΔx_HOR         2cΔx_HOR

1cm     1cm     1c%         1cm     1c%         1cm     1c%

3*#1     mean     107873      7276     7.10          4179      3.82          488     0.46

std       19756      4880      5.69          3308      2.73          350     0.31

max      172842     25288     32.38         21924     14.22         1957     1.48

3*#2     mean     14198     216     1.52         251     1.77          33     0.23

std       1176     119     0.86         210     1.48          26     0.18

max      18253     586     4.38         954     7.08         130     0.98

tableAggregated  final horizontal position , , and  (100 runs)

In the case of the VNS (red lines), its scenario #1 horizontal position estimations appear to be significantly more accurate than those of the INS (blue lines). Note however that the ideal image generation process discussed in section <ref> implies that the simulation results should be treated as a best case only, and that the results obtained in real world conditions would likely imply a higher horizontal position drift. The drift experienced by the  VNS in figure <ref> (scenario #2) also shows the same diminution in its slope in the second half of the scenario discussed in previous sections, which is attributed to previously mapped terrain points reappearing in the camera field of view as a consequence of the continuous turns present in scenario #2. Additionally, notice how the VNSE starts growing at the beginning of the scenario, while the INSE only starts doing so after the GNSS signals are lost at = 100   s [5].

every axis legend/.append style=

cycle list=
blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
magenta,no markers,thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,
xlabel=t sec,
ymin=0, ymax=8000, ytick=0,2000,4000,6000,8000,
ylabel=,     m,
axis lines=left,
axis line style=-stealth,
legend entries=
± INSE,
± VNSE,
± IA-VNSE,
legend columns=1,
legend style=font=,
legend cell align=left,
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=5] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (650,3200) Scenario #1 MX;

Horizontal position , , and  for scenario #1  (100 runs)

The IA-VNS (green lines) results in major horizontal position estimation improvements over the VNS. The final horizontal position error mean  diminishes from 3.82 to 0.46   % for scenario #1, and from 1.77 to 0.23   % for scenario #2. The repeatability of the results also improves, as the final standard deviation  falls from 2.73 to 0.31   % and from 1.48 to 0.18   % for both scenarios. Note that although these results may be slightly optimistic due to the optimized image generation process, they are much more accurate than those obtained with the INS, for which the error mean and standard deviation amount to 7.10 and 5.69   % for scenario #1, and 1.52 and 0.86   % in case of scenario #2.

every axis legend/.append style=

cycle list=
blue,no markers,very thick,
red,no markers,very thick,
green,no markers,very thick,
magenta,no markers,thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
xmin=0, xmax=500, xtick=0,50,...,500,
xlabel=t sec,
ymin=0, ymax=400, ytick=0,100,...,400,
ylabel=,     m,
axis lines=left,
axis line style=-stealth,
legend entries=
± INSE,
± VNSE,
± IA-VNSE,
legend columns=2,
legend style=font=,
legend cell align=left,
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=5] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
node [draw, shape=rectangle, fill=white] at (70,360) Scenario #2 MX;

Horizontal position , , and  for scenario #2  (100 runs)

It is interesting to remark how the prior based pose optimization described in section <ref>, an algorithm that adjusts the aircraft pitch and bank angles based on deviations between the visually estimated pitch angle, bank angle, and geometric altitude, and their inertially estimated counterparts, is capable of not only improving the visual estimations of those three variables, but doing so with a minor improvement in the body yaw estimation and an extreme reduction in the horizontal position error. When the cost function within an optimization algorithm is modified to adjust certain target components, the expected result is that this can be achieved only at the expense of the accuracy in the remaining target components, not in addition to it. The reason why in this case all target components improve lies in that the adjustment creates a better fit between the ground terrain and associated 3D points depicted in the images on one side, and the estimated aircraft pose indicating the position and attitude from where the images are taken on the other.

§ INFLUENCE OF TERRAIN TYPE

The type of terrain overflown by the aircraft has a significant influence on the performance of the visual navigation algorithms described in sections <ref> and <ref>, which can not operate unless the feature detector is capable of periodically locating features in the various keyframes, and which also requires the depth filter to correctly estimate the 3D terrain coordinates of each feature. The terrain texture (or lack of) and its elevation relief are hence the two most important characteristics in this regard. To evaluate its influence, each of the scenario #1 one hundred Monte Carlo runs is executed flying above four different zones or types of terrain[All section <ref> results have been obtained with the mix (MX) zone.], intended to represent a wide array of conditions; images representative of each zone as viewed by the onboard camera are included below. The use of terrains that differ in both their texture and vertical relief is intended to provide a more complete validation of the proposed algorithms. Note that the only variation among the different simulations is the terrain type, as all other parameters defining each scenario (mission, aircraft, sensors, weather, wind, turbulence, geophysics, initial estimations) are exactly the same for all simulation runs.

* The desert (DS) zone is located in the Sonoran desert of southern Arizona (USA) and northern Mexico. It is characterized by a combination of bajadas (broad slopes of debris) and isolated very steep mountain ranges. There is virtually no human infrastructure or flat terrain, as the bajadas have sustained slopes of up to 7^∘. The altitude of the bajadas ranges from 300 to 800   m above MSL, and the mountains reach up to 800   m above the surrounding terrain. Texture is abundant because of the cacti and the vegetation along the dry creeks.

* The farm (FM) zone is located in the fertile farmland of southeastern Illinois and southwestern Indiana (USA). A significant percentage of the terrain is made of regular plots of farmland, but there also exists some woodland, farm houses, rivers, lots of little towns, and roads. It is mostly flat with an altitude above MSL between 100 and 200   m, and altitude changes are mostly restricted to the few forested areas. Texture is nonexistent in the farmlands, where extracting features is often impossible.

Typical “desert” () and “farm” () terrain views

* The forest (FR) zone is located in the deciduous forestlands of Vermont and New Hampshire (USA). The terrain is made up of forests and woodland, with some clearcuts, small towns, and roads. There are virtually no flat areas, as the land is made up by hills and small to medium size mountains that are never very steep. The valleys range from 100 to 300   m above MSL, while the tops of the mountains reach 500 to 900   m. Features are plentiful in the woodlands.

* The mix (MX) zone is located in northern Mississippi and extreme southwestern Tennessee (USA). Approximately half of the land consists of woodland in the hills, and the other half is made up by farmland in the valleys, with a few small towns and roads. Altitude changes are always presents and the terrain is never flat, but they are smaller than in the DS and FR zones, with the altitude oscillating between 100 and 200   m above MSL.

Typical “forest” () and “mix” () terrain views

The short duration and continuous maneuvering of scenario #2 enables the use of two additional terrain types. These two zones are not employed in scenario #1 because the authors could not locate wide enough areas with a prevalence of this type of terrain (note that scenario #1 trajectories can conclude up to 125   km in any direction from its initial coordinates, but only 12   km for scenario #2).

* The prairie (PR) zone is located in the Everglades floodlands of southern Florida (USA). It consists of flat grasslands, swamps, and tree islands located a few meters above MSL, with the only human infrastructure being a few dirt roads and landing strips, but no settlements. Features may be difficult to obtain in some areas due to the lack of texture.

* The urban (UR) zone is located in the Los Angeles metropolitan area (California, USA). It is composed by a combination of single family houses and commercial buildings separated by freeways and streets. There is some vegetation but no natural landscapes, and the terrain is flat and close to MSL.

Typical “prairie” () and “urban” () terrain views

The MX terrain zone is considered the most generic and hence employed to evaluate the visual algorithms in section <ref>. Although scenario #2 also makes use of the four terrain types listed for scenario #1 (DS, FM, FR, and MX), it is worth noting that the variability of the terrain is significantly higher for scenario #1 because of the bigger land extension covered. The altitude relief, abundance or scarcity of features, land use diversity, and presence of rivers and mountains is hence more varied when executing a given run of scenario #1 over a certain type of terrain, than when executing the same run for scenario #2. From the point of view of the influence of the terrain on the visual navigation algorithms, scenario #1 should theoretically be more challenging than #2.

2lScenario #1 Zone     2cMX     2cFR     2cFM     2cDS

2l     1cm     1c%     1cm     1c%     1cm     1c%     1cm     1c%

3*IA-VNSE     mean      488     0.46      566     0.53      489     0.45      514     0.48

std       350             0.31       406             0.38       322             0.28       352             0.31

max      1957             1.48      2058             1.71      1783             1.34      1667             1.37

tableInfluence of terrain type on final horizontal position  for scenario #1 (100 runs)

Table <ref> and figure <ref> show the horizontal position IA-NVSE for scenario #1 and all terrain types. Table <ref> and figure <ref> do the same for scenario #2.

every axis legend/.append style=

cycle list=blue,no markers,thick,
red,no markers,thick,
green,no markers,thick,
orange,no markers,thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
orange,dashed,no markers,ultra thin,orange,dashed,no markers,ultra thin,
xmin=0, xmax=3800, xtick=0,500,...,3500,3800,
xlabel=t s,
ymin=0, ymax=1000, ytick=0,250,...,1000,
ylabel=  m,
axis lines=left,
axis line style=-stealth,
legend entries=MX baseline,
legend columns=4,
legend style=font=,
legend cell align=left,
figs/ch19_ana_vis_assist/zone_xhor/error_base_pos_hor_m_pc_vis.txttable [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=4] ;
table [header=false, x index=0,y index=7] ;
table [header=false, x index=0,y index=10] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=5] ;
table [header=false, x index=0,y index=6] ;
table [header=false, x index=0,y index=8] ;
table [header=false, x index=0,y index=9] ;
table [header=false, x index=0,y index=11] ;
table [header=false, x index=0,y index=12] ;
node [draw, shape=rectangle, fill=white] at (750,375) Scenario #1;
node [draw, shape=rectangle, fill=white] at (750,625)  ± IA-VNSE;

Influence of terrain type on horizontal position  for scenario #1 (100 runs)

The influence of the terrain type on the horizontal position IA-VNSE is very small, with slim differences among the various evaluated terrains. The only terrain type that clearly deviates from the others is FR, with slight but consistently worse horizontal position estimations for both scenarios. This behavior stands out as the abundant texture and continuous smooth vertical relief of the FR terrain is a priori beneficial for the visual estimations.

2lScenario #2 Zone     2cMX     2cFR     2cFM     2cDS     2cUR     2cPR

2l     1cm     1c%     1cm     1c%     1cm     1c%     1cm     1c%     1cm     1c%     1cm     1c%

3*IA-VNSE     mean      33     0.23      40     0.28      33     0.23      31     0.22      32     0.23      31     0.22

std       26             0.18       35             0.24       24             0.17       24             0.17       25             0.18       25             0.17

max      130             0.98      188             1.29      117             0.85      114             0.86      128             0.96      119             0.90

tableInfluence of terrain type on final horizontal position  for scenario #2 (100 runs)

Although beneficial for the SVO pipeline, the more pronounced vertical relief of the FR terrain type breaches the flat terrain assumption of the initial homography, hampering its accuracy, and hence results in less precise initial estimations, including that of the scale. The IA-VNS has no means to compensate the initial scale errors, which remain approximately equal (percentage wise) for the full duration of both scenarios.

every axis legend/.append style=

cycle list=blue,no markers,thick,
red,no markers,thick,
green,no markers,thick,
orange,no markers,thick,
violet,no markers,thick,
magenta,no markers,thick,
blue,dashed,no markers,ultra thin,blue,dashed,no markers,ultra thin,
red,dashed,no markers,ultra thin,red,dashed,no markers,ultra thin,
green,dashed,no markers,ultra thin,green,dashed,no markers,ultra thin,
orange,dashed,no markers,ultra thin,orange,dashed,no markers,ultra thin,
violet,dashed,no markers,ultra thin,violet,dashed,no markers,ultra thin,
magenta,dashed,no markers,ultra thin,magenta,dashed,no markers,ultra thin,
xmin=0, xmax=500, xtick=0,50,...,500,
xlabel=t s,
ymin=0, ymax=100, ytick=0,20,...,100,
ylabel=  m,
axis lines=left,
axis line style=-stealth,
legend entries=MX baseline,
legend columns=6,
legend style=font=,
legend cell align=left,
figs/ch19_ana_vis_assist/zone_xhor/error_alter_pos_hor_m_pc_vis.txttable [header=false, x index=0,y index=1] ;
table [header=false, x index=0,y index=4] ;
table [header=false, x index=0,y index=7] ;
table [header=false, x index=0,y index=10] ;
table [header=false, x index=0,y index=13] ;
table [header=false, x index=0,y index=16] ;
table [header=false, x index=0,y index=2] ;
table [header=false, x index=0,y index=3] ;
table [header=false, x index=0,y index=5] ;
table [header=false, x index=0,y index=6] ;
table [header=false, x index=0,y index=8] ;
table [header=false, x index=0,y index=9] ;
table [header=false, x index=0,y index=11] ;
table [header=false, x index=0,y index=12] ;
table [header=false, x index=0,y index=14] ;
table [header=false, x index=0,y index=15] ;
table [header=false, x index=0,y index=17] ;
table [header=false, x index=0,y index=18] ;
node [draw, shape=rectangle, fill=white] at (425,90) Scenario #2;
node [draw, shape=rectangle, fill=white] at (100,70)  ± IA-VNSE;

Influence of terrain type on horizontal position  for scenario #2 (100 runs)

A similar but opposite reasoning is applicable to the FM type and in a lesser degree to the UR and PR types. Although a flat terrain in which all terrain features are located at a similar altitude is detrimental to the overall accuracy of SVO, and results in slightly worse body attitude and vertical position estimations, it is beneficial for the homography initialization and the scale determination, resulting in consistently more accurate horizontal position estimations.

§ SUMMARY OF RESULTS

This article proposes an SVO based IA-VNS installed onboard a fixed wing autonomous UAV that takes advantage of the GNSS-Denied estimations provided by an INS to assist the visual pose optimization algorithms. The method is inspired in a PI control loop, in which the inertial attitude and altitude outputs act as targets to ensure that the visual estimations do not deviate in excess from their inertial counterparts, resulting in major improvements when estimating the aircraft horizontal position without the use of GNSS signals. The results obtained when applying the proposed algorithms to high fidelity Monte Carlo simulations of two scenarios representative of the challenges of GNSS-Denied navigation indicate the following:

* The body attitude estimation shows significant quantitative improvements over a stand-alone VNS in both pitch and bank angle estimations, with no negative influence on the yaw angle estimations. A small amount of drift with time is present, and can not be fully eliminated. Body pitch and bank angle estimations do not deviate in excess from their INS counterparts, while the body yaw angle visual estimation is significantly more accurate than that obtained by the INS.

* The vertical position estimation shows major improvements over that of a stand-alone VNS, not only quantitatively but also qualitatively, as drift is fully eliminated. The visual estimation does not deviate in excess from the inertial one, which is bounded by atmospheric physics.

* The horizontal position estimation, whose improvement is the main objective of the proposed algorithm, shows major gains when compared to either the stand-alone VNS or the INS, although drift is still present.

In addition, although the terrain texture (or lack of) and its elevation relief are key factors for the visual odometry algorithms, their influence on the aircraft pose estimation results are slim, and the accuracy of the IA-VNS does not vary significantly among the various evaluated terrain types.

§ CONCLUSIONS

The proposed inertially assisted VNS (IA-VNS), which in addition to the images taken by an onboard camera also relies on the outputs of an INS specifically designed for the challenges faced by autonomous fixed wing aircraft that encounter GNSS-Denied conditions, possesses significant advantages in both accuracy and resilience when compared with a stand-alone VNS, the most important of which is a major reduction in its horizontal position drift independently of the terrain type overflown by the aircraft. The proposed IA-VNS can significantly increase the possibilities of the aircraft safely reaching the vicinity of the intended recovery location upon the loss of GNSS signals, from where it can be landed by remote control.

§ OPTICAL FLOW

Consider a pinhole camera [74] (one that adopts an ideal perspective projection) such as that depicted in figure <ref>. The image frame  is a two-dimensional Cartesian reference frame = {,  ,   } whose axes are parallel to those of the  camera frame (∥,   ∥), and whose origin  is located on the focal plane displaced a distance  from the principal point so the  coordinates  and  of any point in the image domain Ω are always positive. The perspective projection map = Π⃗ that converts points viewed in  into  is hence the following:

\begin{eqnarray}
\nm{\pIMGi}  & = & \nm{\dfrac{f}{\sPX} \ \dfrac{\pCi}{\pCiii} + \cIMGi} \label{eq:Vis_camera_pIMGi} \\
\nm{\pIMGii} & = & \nm{\dfrac{f}{\sPX} \ \dfrac{\pCii}{\pCiii} + \cIMGii} \label{eq:Vis_camera_pIMGii}
\end{eqnarray}

(OC)    at (+0.0,+0.0);
(iCi)   at  ($(OC) + (+0.0,+0.7)$);
(iCii)  at  ($(OC) + (-0.5,-0.5)$);