Scale Factor Estimation for Quadrotor Monocular-Vision Positioning Algorithms

Abstract

Unmanned aerial vehicle (UAV) autonomous navigation requires access to translational and rotational positions and velocities. Since there is no single sensor to measure all UAV states, it is necessary to fuse information from multiple sensors. This paper proposes a deterministic estimator to reconstruct the scale factor of the position determined by a simultaneous localization and mapping (SLAM) algorithm onboard a quadrotor UAV. The position scale factor is unknown when the SLAM algorithm relies on the information from a monocular camera. Only onboard sensor measurements can feed the estimator; thus, a deterministic observer is designed to rebuild the quadrotor translational velocity. The estimator and the observer are designed following the immersion and invariance method and use inertial and visual measurements. Lyapunov’s arguments prove the asymptotic convergence of observer and estimator errors to zero. The proposed estimator’s and observer’s performance is validated through numerical simulations using a physics-based simulator.

1. Introduction

Nowadays, quadrotors are used in various applications, thanks to their low cost, mechanical robustness, and high maneuverability. Such applications include homeland security, forest-fire control, surveillance, sea and land exploration, human search and rescue, archaeological exploration, and volcanic activity monitoring, among many others [1,2]. Most of the abovementioned applications become impractical or even dangerous for human operators; thus, autonomous navigation, control, and guidance are required.

A quadrotor can perform autonomous navigation in unknown environments when its autopilot has access to all states. However, providing access to all quadrotor states without relying on a remote computer or sensors demands a vehicle with the capacity to process and extract information from only onboard sensors. The bottleneck to measuring all quadrotor states is that there are no out-of-the-box functional and reliable sensors to measure all states directly. For example, the quadrotor’s attitude is obtained by fusing the measurements from an inertial measurement unit (IMU). On the other hand, the global position system (GPS) is the primary sensor used for quadrotor positioning, but it presents limitations. It fails in environments where satellite communication is degraded, called GPS-denied environments, such as water bodies and indoors [3]. Furthermore, low-cost GPS does not provide enough resolution for trajectories on a centimeter scale, and the price of a GPS with higher resolution, such as differential GPS, increases drastically.

An algorithm to fuse GPS measurements with optical flow information using a Kalman filter (KF) was proposed in [4]. Shortly after, the work reported in [5] presented an improved sensor fusion algorithm based on an extended Kalman filter (EKF) that includes the measurements from an inertial navigation system (INS). Both sensor fusion algorithms improved the position estimation for low-cost GPS but not for GPS-denied environments.

Computer vision has emerged as a powerful solution for quadrotor position estimation. Visual sensors have many advantages over other sensors: they are cheap, provide color and geometric information for scene understanding, and consume less power. Many computer-vision algorithms are available for position estimation. For example, visual odometry (VO) estimates the ego-motion of a vehicle with an onboard camera. VO incrementally estimates the vehicle’s pose by examining the changes that motion induces on the input images [6]. Using a red, green, blue, depth (RGBD) camera, the method reported in [7] improved the VO algorithm by including a novel covariance estimation technique. The resulting VO-based algorithm allowed autonomous quadrotor navigation with satisfactory results. The depth camera measurement allows for determining the position scale factor.

The SLAM algorithm estimates the vehicle’s pose and, at the same time, constructs a map of the surroundings. The most successful versions of SLAM, running in real time, are ORB-SLAM [8] and LSD-SLAM [9]. These SLAM variants rely on techniques used to calculate the camera position and construct the map, such as feature extraction, or direct methods that operate on the image intensities [10]. A comparison between SLAM algorithms for mobile robot navigation in indoor environments is reported in [11], where it is concluded that ORB-SLAM can be used to determine the robot position with an additional module to recover the scale factor. The report [12] presents quadrotor autonomous navigation using a SLAM algorithm without determining the position scale factor.

Semidirect visual odometry (SVO) is a hybrid algorithm combining feature-based and direct methods. It estimates the relative motion between two frames by minimizing photometric errors. The projection error between the location of the feature points and their predicted positions is minimized to obtain the optimal camera pose [13]. The autonomous navigation of a UAV using SVO and a recovery mechanism to reinitialize the visual map when a failure occurs are proposed in [14]. The navigation strategy includes a pose estimation scheme for temporary vehicle control and a method to correct the scene scale factor using altitude measurements.

With some differences, computer-vision algorithms can be implemented using monocular or stereoscopic cameras. Stereo cameras can capture three-dimensional images, meaning the scene’s depth is known. This ability leads to better accuracy and resolution than in the monocular camera case for position estimation purposes. The algorithms ORB-SLAM, LSD-SLAM, and SVO, among others, were first developed for monocular camera implementation and, years after, improved for multi-camera configurations as reported in [15,16,17], respectively.

Nevertheless, monocular cameras are preferred for implementation in small vehicles for several reasons: a single camera is easy to mount due to its smaller size, is lighter and cheaper, and consumes less power. Additionally, a single camera configuration is free from the burden of multi-camera calibration and requires less processing power from the CPU onboard than multi-camera configurations [18]. Only one drawback is present for monocular cameras: they cannot recover the image’s three-dimensional structure and the camera position with complete metric information; in other words, the information on the scene’s depth is unavailable. This phenomenon is known as similarity ambiguity [19]. At least one piece of metric information is required to recover the absolute scale factor. This cue may come from prior scene knowledge, such as camera height, object size, vehicle speed, stereo camera baseline, or other sensors such as LiDAR or GPS.

Some methods have been proposed to deal with the similarity ambiguity problem. In [20,21], the extra piece of geometric information to determine the position measurement scale factor comes from an ultrasonic sensor and a one-dimensional laser range finder (LRF), respectively. These approaches require additional sensors onboard. Besides, the absolute scale is only calculated on the axis where the sensor is mounted, so it is assumed that the scale is the same on the other two axes, which is not always valid.

An EKF algorithm considering multirotor dynamics is proposed in [22] to estimate the scale factor online. A scale factor observability analysis supports the estimator design. However, using EKF on this approach makes the estimator nondeterministic, so stability is not formally proven.

In the field of deterministic estimators, ref. [23] presents a scale estimator based on control stability. It shows that the absolute scale and control gain have a unique linear relationship. The absolute scale can be estimated by detecting self-induced oscillations and analyzing the system stability. The problem with this approach is that an adaptive control technique must be used for an online estimation, leaving out other types of controllers.

This article presents a scale factor estimator in the cartesian plane fused with a velocity observer, deterministic and based on the quadrotor dynamic model, using only onboard sensors. The set of sensors provides the quadrotor’s attitude and angular velocity from an attitude and heading reference system (AHRS), the quadrotor’s acceleration from the set of sensors of the inertial measurement unit, and the scaled position from a SLAM algorithm based on a monocular camera. The scale factor estimator and the velocity observer are designed following the immersion and invariance methodology introduced in [24]. The singular contributions of this work are: the estimator and observer are designed considering the full quadrotor nonlinear model, the Coriolis forces are not neglected, the position scale factor is reconstructed in all three dimensions, and it is formally proven using Lyapunov arguments that the estimator and observer errors locally asymptotically converge to zero. Numerical simulations using Gazebo are presented to support the theoretical developments. The outcomes of this paper are based on the preliminary works reported in [25,26].

The remaining parts of the paper are arranged as follows. The sensor models used are presented in Section 2, along with the quadrotor dynamics. Section 3 outlines the fundamental contribution of this paper and describes the mathematical advancements used to create the scale factor estimator. Through numerical simulations, Section 4 illustrates the performance of the estimator. Finally, Section 5 wraps up this paper with a few closing thoughts and suggestions for future work.

2. Materials and Methods

Table 1. Quadrotor dynamic model notation.

Symbol	Variable	Units
$X={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\top }$	Translational position in inertial coordinates	m
$R\in SO\left(3\right)$	Rotation matrix from body to inertial coordinates	dimensionless
$V^b={\left[\begin{array}{ccc}u& v& w\end{array}\right]}^{\top }$	Translational velocity in body frame coordinates	m/s
$\Omega ={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^{\top }$	Angular velocity in body coordinates	rad/s
$M^b={\left[\begin{array}{ccc}{M}_x^b& M_y^b& M_z^b\end{array}\right]}^{\top }$	Moments generated by the differential thrust, and reaction moment between the four rotors	Nm
m	Quadrotor mass	kg
g	Gravity acceleration constant	m/s${}^2$
$T_T$	Total thrust generated by the four rotors	N
$\mu$	Parameter related to aerodynamic drag force [27]	kg/s
$J\in {ℝ}^{3\times 3}$	Quadrotor inertia matrix	kg m${}^2$

summarizes the notation used to introduce the quadrotor dynamic model.

In Table 1, the International System of Units is considered, and

$$SO\left(3\right)=\left\{R\in {ℝ}^{3\times 3}|R^{\top }R=I,\det \left(R\right)=1\right\}$$

with I the identity matrix.

2.1. Quadrotor Dynamics

An inertial coordinate frame and a non-inertial coordinate frame (body frame) attached to the quadrotor center of gravity are needed to describe the quadrotor dynamic, see Figure 1. The following equations, expressed in mixed inertial and body coordinates, describe the translational and rotational quadrotor dynamics [26]:

$$\begin{array}{ccc}\hfill \dot{X}& =& R{V}^b\hfill \\ \hfill m{\dot{V}}^b& =& mg{R}^{\top }{e}_3-T_T{e}_3-\mu H{V}^b-mS(\Omega )V^b\hfill \\ \hfill \dot{R}& =& RS(\Omega )\hfill \\ \hfill J\dot{\Omega }& =& -S(\Omega )J\Omega +M^b\hfill \end{array}$$

(1)

with

$$e_3=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right],S(\Omega )=\left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]$$

2.2. Available Sensors

It is assumed that the quadrotor carries onboard a set of sensors that provide the following measurements.

2.2.1. Scaled Position

The vehicle carries a monocular camera facing the horizontal plane and the necessary computer power to implement a monocular-vision algorithm to determine its scaled inertial position. Therefore, the following measurement is available

$$\begin{array}{c}{y}_1=X_s=\left[\begin{array}{c}{k}_{x}x\\ k_{y}y\\ k_{z}z\end{array}\right]=\left[\begin{array}{ccc}{k}_x& 0& 0\\ 0& k_y& 0\\ 0& 0& k_z\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=diag\left(K\right)X\end{array}$$

(2)

where $X_s$ is the scaled position delivered by the monocular SLAM vision algorithm and $K={\left[\begin{array}{ccc}{k}_x& k_y& k_z\end{array}\right]}^{\top }$ is the dimensionless unknown scale factor on the axes $X^i{Y}^i{Z}^i$, respectively.

Remark 1.

The operator $diag\left(A\right)$ represents a diagonal matrix whose elements are the elements of vector $A\in {ℝ}^3$. This operator satisfies the following indentities

$$diag(A+B)=diag\left(A\right)+diag\left(B\right)$$

(3)

$$diag\left(A\right)B=diag\left(B\right)A$$

(4)

with $A,B\in {ℝ}^3$ vectors.

2.2.2. Specific Acceleration

Commonly, quadrotors are equipped with an inertial measuring unit (IMU) that measures the Earth’s magnetic field intensity, angular velocity, and specific acceleration in body coordinates. According to the Accelerometer Tutorial reported in [27], the specific acceleration measured by an accelerometer mounted on a quadrotor is given by

$$a^b=\frac{1}{m}{F}_T^b-g{R}^{\top }{e}_3$$

(5)

where $a^b$ is the specific acceleration measured in the body axis and $F_T^b$ is the total external force acting on the quadrotor expressed in the body axis. From the quadrotor dynamics model in Equation (1), it follows that

$$F_T^b=mg{R}^{\top }{e}_3-T_T{e}_3-\mu H{V}^b$$

(6)

as a result,

$$a^b=-\frac{T_T}{m}{e}_3-\frac{\mu }{m}H{V}^b$$

(7)

Hence, the specific acceleration is an available output, element wise it reads as

$$y_2=\left[\begin{array}{c}\hfill a_x^b\\ \hfill a_y^b\\ \hfill a_z^b\end{array}\right]=-\frac{1}{m}\left[\begin{array}{c}\hfill \mu u\\ \hfill \mu v\\ \hfill T_T\end{array}\right]$$

(8)

with $a_x^b$, $a_y^b$ and $a_z^b$ the specific acceleration along the body axis.

2.2.3. Attitude and Heading Reference Systems

The device that computes the quadrotor’s attitude and rotational velocity from the IMU measurements is called the attitude and heading reference system (AHRS). Assuming that the quadrotor carries an AHRS, the following signals are available.

$$\begin{array}{c}\hfill y_3=R=\left[\begin{array}{c}{r}_1^{\top }\\ r_2^{\top }\\ r_3^{\top }\end{array}\right]\end{array}$$

(9)

$$\begin{array}{c}\hfill y_4=\Omega =\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}$$

(10)

where $r_i\in {𝕊}^2$, $i=1,\dots ,3$ are the columns of the rotation matrix transposed,

$${𝕊}^2=\{A\in {ℝ}^3|A^{\top }A=1\}$$

is the unit 2-sphere.

2.2.4. Vertical Speed

Through the use of a laser sensor or an ultrasonic sensor, the vertical quadrotor position can be measured so that the vertical speed can be determined. As a result, it is presumed that the subsequent measurement is available

$$y_5=w$$

(11)

Finally, note that the quadrotor translational dynamic, the first equation in (1), expressed in terms of the measured states reads as

$${\dot{V}}^b=g{y}_3^{\top }{e}_3+y_2-S\left(y_4\right)V^b$$

(12)

2.3. Immersion and Invariance Observers

The following developments are based on Chapter 5 of [24]. Consider the following non-linear, deterministic, time invariant system

$$\begin{array}{ccc}\hfill \dot{\eta }& =& f_1(\eta ,y)\hfill \\ \hfill \dot{y}& =& f_2(\eta ,y)\hfill \end{array}$$

(13)

where $\eta \in 𝓡\subset {ℝ}^n$ and $y\in 𝓨\subset {ℝ}^m$ are the unmeasured and measured states, respectively. It is assumed that the vector fields $f_1(\eta ,y)$ and $f_2(\eta ,y)$ are forward complete.

Definition 1.

The dynamic system

$$\begin{array}{ccc}\hfill \dot{\hat{\eta }}& =& \phi (\hat{\eta },y)\hfill \end{array}$$

(14)

with$\hat{\eta }\in {ℝ}^n$, is an observer for the unmeasured state η if there exists a mapping$\beta :{ℝ}^n\times {ℝ}^m\to {ℝ}^n$such that the manifold

$$𝓜=\left\{(\eta ,\hat{\eta },y)\subset {ℝ}^n\times {ℝ}^n\times {ℝ}^m|\beta (\hat{\eta },y)=\eta \right\}$$

has the following properties

• $𝓜$ is positively invariant,
• All trajectories of (13), (14) that start in a neighbourhood of $𝓜$ asymptotically converge to $𝓜$.

The construction of the observer of the form given in Definition 1 requires additional properties on the mapping $\beta (\hat{\eta },y)$, as stated in the following result.

Theorem 1.

Consider the system (13). Suppose that there exist differentiable maps $\beta :{ℝ}^n\times {ℝ}^m\to {ℝ}^n$ such that

A1 

For all $\hat{\eta }$ and y the map $\beta (\hat{\eta },y)$ satisfies

$$det\left(\frac{\partial \beta }{\partial \hat{\eta }}\right)\ne 0$$

A2 

The dynamic system

$$\dot{\tilde{\eta }}=f_1(\tilde{\eta }+\beta (\hat{\eta },y),y)-f_1(\beta (\hat{\eta },y),y)-\frac{\partial \beta }{\partial y}\left(f_2(\tilde{\eta }+\beta (\hat{\eta },y),y)-f_2(\beta (\hat{\eta },y),y)\right)$$

(15)

has a (globally) asymptotically stable equilibrium at $\tilde{\eta }=0$ uniformily in $\hat{\eta }$ and y.

Then, the system (14) with

$$\phi (\hat{\eta },y)={\left(\frac{\partial \beta }{\partial \hat{\eta }}\right)}^{-1}\left(f_1(\beta (\hat{\eta },y),y)-\frac{\partial \beta }{\partial y}{f}_2(\beta (\hat{\eta },y),y)\right)$$

(16)

is a (global) observer for (13).

The proof of this Theorem is presented in Appendix A. The result expressed in Theorem 1 is followed to design the velocity observer and the scale factor estimator.

3. Observer and Estimator Design

This section discusses how the observer and the estimator that reconstruct $V^b$ and K, respectively, are designed from the available measurements of acceleration, scaled position, attitude, angular velocity, and vertical speed.

3.1. Observation and Estimation Problems

The following terms state the observation problem. Assume that the outputs $y_i,$$i=1,\dots ,5$ are measurable. Design two dynamic systems, likely, of the form

$$\begin{array}{ccc}\hfill {\dot{\hat{V}}}^b& =& {\phi }_1({\hat{V}}^b,y_2,y_3,y_4,y_5)\hfill \\ \hfill \dot{\hat{K}}& =& {\phi }_2(\hat{K},{\hat{V}}^b,y_1,y_2,y_3,y_4,y_5)\hfill \end{array}$$

where ${\hat{V}}^b\in {ℝ}^3$ and $\hat{K}\in {ℝ}^3$, such that two functions exist, ${\beta }_1\in {ℝ}^3$ and ${\beta }_2\in {ℝ}^3$, that depend on the available information, and the following identities asymptotically hold

$$\begin{array}{ccc}\hfill \underset{t\to \infty }{lim}{\beta }_1({\hat{V}}^b,y_2,y_3,y_4,y_5)& =& V^b\hfill \\ \hfill \underset{t\to \infty }{lim}{\beta }_2(\hat{K},{\hat{V}}^b,y_1,y_2,y_3,y_4,y_5)& =& K\hfill \end{array}$$

(17)

3.2. Velocity Observer

According to the immersion and invariance technique, the observation error is defined as follows

$${\tilde{V}}^b=V^b-{\beta }_1({\hat{V}}^b,\sigma )$$

(18)

with ${\beta }_1$ element wise reading as

$${\beta }_1=\left[\begin{array}{c}\hfill {\beta }_{1x}\\ \hfill {\beta }_{1y}\\ \hfill {\beta }_{1z}\end{array}\right]$$

(19)

where

$$\dot{\sigma }=\left[\begin{array}{c}\hfill {\displaystyle a_x^b}\\ \hfill {\displaystyle a_y^b}\\ \hfill -w\end{array}\right]=\left[\begin{array}{c}\hfill {\displaystyle -\frac{\mu }{m}u}\\ \hfill {\displaystyle -\frac{\mu }{m}v}\\ \hfill -w\end{array}\right]=-\overline{H}{V}^b$$

(20)

and

$$\overline{H}=\left[\begin{array}{ccc}{\displaystyle \frac{\mu }{m}}& 0& 0\\ 0& {\displaystyle \frac{\mu }{m}}& 0\\ 0& 0& 1\end{array}\right]$$

Equation (18) models the distance to the manifold $𝓜$ of Definition 1, where the velocity $V^b$ is equal to ${\beta }_1({\hat{V}}^b,\sigma )$. This distance must asymptotically converge to zero to complete the observer design. Note that the output $y_2$ is not directly used since the time derivative of ${\tilde{V}}^b$ will require the computation of ${\dot{y}}_2$; this is the reason why the new state $\sigma$ is introduced.

The time derivative of ${\tilde{V}}^b$ is given by

$${\dot{\tilde{V}}}^b={\dot{V}}^b-\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}{\dot{\hat{V}}}^b-\frac{\partial {\beta }_1}{\partial \sigma }\dot{\sigma }$$

(21)

Substituting ${\dot{V}}^b$, the body velocity $V^b$ and the time derivative of $\sigma$ from Equations (12), (18), and (20), respectively, one has

$$\begin{array}{c}\hfill {\displaystyle {\dot{\tilde{V}}}^b=g{y}_3^{\top }{e}_3+y_2-S\left(y_4\right)({\tilde{V}}^b+{\beta }_1)-\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}{\dot{\hat{V}}}^b+\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}({\tilde{V}}^b+{\beta }_1)}\end{array}$$

(22)

Now, the observer state dynamic ${\dot{\hat{V}}}^b$ is defined in terms of the known signals, as

$$\begin{array}{c}\hfill {\displaystyle {\dot{\hat{V}}}^b={\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}}^{-1}\left(g{y}_3^{\top }{e}_3+y_2-S\left(y_4\right){\beta }_1+\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}{\beta }_1\right)}\end{array}$$

(23)

Substituting (23) into (22), one obtains

$${\dot{\tilde{V}}}^b=-S\left(y_4\right){\tilde{V}}^b+\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}{\tilde{V}}^b$$

(24)

Consider the following Lyapunov function to define the function ${\beta }_1({\hat{V}}^b,\sigma )$

$${\nu }_V=\frac{1}{2}{\tilde{V}}^b{}^{\top }{\tilde{V}}^b$$

(25)

it follows that

$${\dot{\nu }}_V={\tilde{V}}^b{}^{\top }\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}{\tilde{V}}^b$$

(26)

Hence, to guarantee that the time derivative of ${\nu }_V$ is negative-definite, the matrix $\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}$ must also be negative-definite. On the other hand, Equation (23) requires the matrix $\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}$ to be invertible. Note that selecting

$${\beta }_1({\hat{V}}^b,\sigma )={\hat{V}}^b-{\Gamma }_v\sigma$$

(27)

with

$${\Gamma }_v=\left[\begin{array}{ccc}{\gamma }_{vx}& 0& 0\\ 0& {\gamma }_{vy}& 0\\ 0& 0& {\gamma }_{vz}\end{array}\right]$$

(28)

and ${\gamma }_{vx}$, ${\gamma }_{vy}$ and ${\gamma }_{vz}$ positive gains, it follows that

$${\dot{\nu }}_V=-{\tilde{V}}^b{}^{\top }{\Gamma }_v\overline{H}{\tilde{V}}^b=-{\tilde{V}}^b{}^{\top }diag\left(h\right){\tilde{V}}^b,\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}=I_3$$

(29)

with $h={\left[\begin{array}{ccc}\frac{{\gamma }_{vx}\mu }{m}& \frac{{\gamma }_{vy}\mu }{m}& {\gamma }_{vz}\end{array}\right]}^{\top }$ and $I_3$ the $3\times 3$ identity matrix. As a result, $\frac{\partial {\beta }_1}{\partial \sigma }\overline{H}$ is negative-definite and $\frac{\partial {\beta }_1}{\partial {\hat{V}}^b}$ is invertible.

3.3. Scale Factor Estimator

In reference [25], the scale factor estimator needs the translational velocity as a measurable output, but in this work it is available through the observer designed (27) according to (17). It is important to note also in this equation that ${\beta }_2$ depends on states expressed in mixed inertial and body coordinates, unlike ${\beta }_1$ which depends only on states expressed in body coordinates. In order to have all the states expressed in inertial coordinates, ${\beta }_1$ needs to be translated with the rotation matrix. The inertial velocity is introduced as follows

$$V^i=y_3{V}^b$$

(30)

Additionally, the inertial velocity observer error is defined

$${\tilde{V}}^i=V^i-{\hat{V}}^i$$

(31)

with

$${\hat{V}}^i=y_3{\beta }_1$$

(32)

Now, the scale factor estimation error is defined in the following form

$$\tilde{K}=K-{\beta }_2(\hat{K},y_1,{\hat{V}}^i)$$

(33)

with ${\beta }_2$ element wise reading as

$${\beta }_2=\left[\begin{array}{c}\hfill {\beta }_{2x}\\ \hfill {\beta }_{2y}\\ \hfill {\beta }_{2z}\end{array}\right]$$

(34)

The derivative with respect to the time of the estimation error is

$$\dot{\tilde{K}}=-\frac{\partial {\beta }_2}{\partial \hat{K}}\dot{\hat{K}}-\frac{\partial {\beta }_2}{\partial y_1}{\dot{y}}_1-\frac{\partial {\beta }_2}{\partial {\hat{V}}^i}{\dot{\hat{V}}}^i$$

(35)

From Equations (1) and (2), it follows that

$$\begin{array}{ccc}\hfill {\dot{y}}_1& =& diag\left(K\right)y_3{V}^b=diag\left(K\right)V^i\hfill \\ \hfill {\dot{y}}_3& =& y_{3}S\left(y_4\right)\hfill \end{array}$$

(36)

By combining Equations (18) and (32), one obtains

$$\begin{array}{ccc}\hfill {\hat{V}}^i& =& y_3(V^b-{\tilde{V}}^b)\hfill \end{array}$$

(37)

thus,

$$\begin{array}{ccc}\hfill {\dot{\hat{V}}}^i& =& g{e}_3+y_3{y}_2-{\Gamma }_v\overline{H}{\tilde{V}}^i\hfill \end{array}$$

(38)

Substituting (36) and (37) into (35), one obtains

$$\begin{array}{c}\hfill \dot{\tilde{K}}=-\frac{\partial {\beta }_2}{\partial \hat{K}}\dot{\hat{K}}-\frac{\partial {\beta }_2}{\partial y_1}diag\left(K\right)V^i-\frac{\partial {\beta }_2}{\partial {\hat{V}}^i}\left(g{e}_3+y_3{y}_2-{\Gamma }_v\overline{H}{\tilde{V}}^i\right)\end{array}$$

(39)

Now, substituting K from (33) and $V^i$ from (31), the scale factor estimation error becomes

$${\displaystyle \dot{\tilde{K}}=-\frac{\partial {\beta }_2}{\partial \hat{K}}\dot{\hat{K}}-\frac{\partial {\beta }_2}{\partial y_1}diag(\tilde{K}+{\beta }_2)\left({\tilde{V}}^i+{\hat{V}}^i\right)-\frac{\partial {\beta }_2}{\partial {\hat{V}}^i}\left(g{e}_3+y_3{y}_2-{\Gamma }_v\overline{H}{\tilde{V}}^i\right)}$$

(40)

The dynamic of the scale factor estimator state is defined in terms of the known signals as follows

$$\begin{array}{c}\hfill {\displaystyle \dot{\hat{K}}={\frac{\partial {\beta }_2}{\partial \hat{K}}}^{-1}\left(-\frac{\partial {\beta }_2}{\partial y_1}diag\left({\beta }_2\right){\hat{V}}^i-\frac{\partial {\beta }_2}{\partial {\hat{V}}^i}\left(g{e}_3+y_3{y}_2\right)\right)}\end{array}$$

(41)

After substituting $\dot{\hat{K}}$ into the scale factor estimator error (40), it follows that

$$\begin{array}{c}\hfill \dot{\tilde{K}}=-\frac{\partial {\beta }_2}{\partial y_1}\left(diag\left(\tilde{K}\right){\hat{V}}^i+diag(\tilde{K}+{\beta }_2){\tilde{V}}^i\right)+\frac{\partial {\beta }_2}{\partial {\hat{V}}^i}{\Gamma }_v\overline{H}{\tilde{V}}^i\end{array}$$

(42)

Once again, the function ${\beta }_2$ needs to be defined to ensure that the estimation error $\tilde{K}$ converges to zero with $\frac{\partial {\beta }_2}{\partial \hat{K}}$ an invertible matrix. Thus, the following vector function is proposed

$${\beta }_2(\hat{K},y_1,{\hat{V}}^i)=\hat{K}+{\Gamma }_{k}diag\left(y_1\right){\hat{V}}^i=\hat{K}+{\Gamma }_{k}diag\left({\hat{V}}^i\right)y_1$$

(43)

with

$${\Gamma }_k=\left[\begin{array}{ccc}{\gamma }_{kx}& 0& 0\\ 0& {\gamma }_{ky}& 0\\ 0& 0& {\gamma }_{kz}\end{array}\right]$$

(44)

and ${\gamma }_{ki},i=x,y,z$ the scale factor estimator gains.

Replacing (43) into (42), one obtains

$$\begin{array}{c}\hfill \dot{\tilde{K}}=-{\Gamma }_{k}diag\left({\hat{V}}^i\right)\left(diag\left({\hat{V}}^i\right)\tilde{K}+diag\left({\tilde{V}}^i\right)K\right)+{\Gamma }_{k}diag\left(y_1\right){\Gamma }_v\overline{H}{\tilde{V}}^i\end{array}$$

(45)

where (3), (4) and (33) had been considered. From (31), it follows that

$$\begin{array}{ccc}\hfill \dot{\tilde{K}}& =& -{\Gamma }_k\left(diag\left(V^i\right)diag\left(V^i\right)+diag\left({\tilde{V}}^i\right)diag\left({\tilde{V}}^i\right)\right)\tilde{K}\hfill \\ & & +2{\Gamma }_{k}diag\left(V^i\right)diag\left({\tilde{V}}^i\right)\tilde{K}-{\Gamma }_k\left(diag\left(V^i\right)-diag\left({\tilde{V}}^i\right)\right)diag\left({\tilde{V}}^i\right)K\hfill \\ & & +{\Gamma }_{k}diag\left(y_1\right){\Gamma }_v\overline{H}{\tilde{V}}^i\hfill \end{array}$$

(46)

The following assumptions are considered to state the main result of this paper.

Assumption 1.

The following identity holds.

$${\displaystyle \underset{t\to \infty }{lim}{\int }_0^{t}diag\left(V^i\left(\tau \right)\right)diag\left(V^i\left(\tau \right)\right)d\tau =\infty I}$$

with $I\in {ℝ}^{3\times 3}$ the identity matrix.

Assumption 2.

There exist control inputs $T_T$ and $M^b$ such that the following quadrotor states can be upper bounded, this is

$$\begin{array}{c}\parallel V^i\parallel \le {\kappa }_0,\parallel y_1\parallel \le {\kappa }_1\end{array}$$

(47)

for some not-necessarily constants ${\kappa }_0$ and ${\kappa }_1$. The notation $\parallel (·)\parallel$ stands for the Euclidean norm for a matrix or vector $(·)$.

Remark 2.

Assumption 1 is the persistence of the excitation condition; in this case, fulfilling this condition implies that the quadrotor must move to estimate the scale factor successfully. Assumption 2 means that a control loop allows the quadrotor to fly stably; consequently, the quadrotor dynamics is forward complete.

The following Proposition summarizes the main result of this work.

Proposition 1.

Under Assumptions 1 and 2, there are matrix gains ${\Gamma }_v$ and ${\Gamma }_k$ such that the dynamic systems (23) and (41) are local observers for the translational velocity and estimators of the scale factor. The translational velocity and scaling factor are rebuilt in (27) and (43).

The proof of this Proposition is reported in Appendix B.

4. Numerical Simulations

A numerical simulation study was performed on different platforms to evaluate the observer and estimator’s performance.

4.1. Matlab-Simulink

The first one was performed using Matlab-Simulink, to avoid problems such as sensors’ noise and external disturbances so that we can evaluate the estimators by themselves. A program was designed to simulate a quadrotor in a closed loop with the controller developed in [28], tracking a circular trajectory on the Cartesian plane and a sinusoidal form on the vertical plane. To fulfill Assumption 1, the desired trajectories were $x_d=Acos\left(\omega t\right)$, $y_d=Asin\left(\omega t\right)$ and $z_d=Acos\left(\omega t\right)$. For the velocity observer, the initial conditions used were $V^b\left(0\right)={\left[\begin{array}{ccc}-0.2& 0.3& 0.2\end{array}\right]}^{\top }$ with a proposed $\mu =0.6$ and gains ${\gamma }_{vx}=1.2$, ${\gamma }_{vy}=1.2$ and ${\gamma }_{vz}=1.2$. Regarding the monocular-vision positioning algorithm for this numerical simulation, any real values for the scale factor K can be used; nevertheless, in more realistic simulations such as the ones in the next section, it is observed that the scaled position is always smaller than the real position, $X_s<X$, so $K<1$; hence, the real values of the scale factor for this simulation where fixed at $K={\left[0.650.70.55\right]}^{\top }$. The gains used for the estimator were ${\gamma }_{kx}=2.0$, ${\gamma }_{ky}=2.0$ and ${\gamma }_{kz}=2.0$.

Figure 2 shows the velocity observer error ${\tilde{V}}^b$ for this simulation, where it can be seen that the velocities converge to zero correctly. Note that the velocity error on the Z axis, $\tilde{w}$, is always zero because the speed on this axis is measurable, (11).

Figure 3 shows the scale factor estimator error $\tilde{K}$ that also proves the correct convergence of the estimator. In this graph the cascade behavior of (46) with (24) can also be seen, which means that $\dot{\tilde{K}}$ will always converge after ${\dot{\tilde{V}}}^b$ due to the interconnection term $\Psi$.

4.2. Gazebo

It is time to put the observers to the test in a simulator, such as Gazebo, which is more like reality after confirming their proper operation in a controlled setting. The Gazebo is an open-source 3D robotics simulator. It incorporates the open dynamics engine (ODE) as a physics engine, OpenGL for graphics, support code for sensor simulation, and actuator control. The robot operating system (ROS) and the Gazebo simulator are wholly linked.

In the previous simulation, it was easy to set simulation values for the constant $\mu$ and the scaled position $X_s$ delivered by a monocular-vision algorithm. In the Gazebo simulation, such values will have to be treated more rigorously as would be performed in an actual experiment.

4.2.1. Calculation of $\mu$

$\mu$ is a term related to the drag coefficient, a positive constant representing a combination of the profile and induced drag forces on the rotors, known in the helicopter literature as “rotor drag” [27,29]. Like any other parameter of the quadrotor, such as its weight (m), it must be measured before experiments. Due to the units of this parameter, listed in Table 1, it is known as the “mass flow rate”.

From Equation (8), it can be seen that

$$\begin{array}{ccc}\hfill a_x^b& =& {\displaystyle -\frac{\mu }{m}u}\hfill \\ \hfill a_y^b& =& {\displaystyle -\frac{\mu }{m}v}\hfill \end{array}$$

(48)

From any of these two equations, the constant $\mu$ can be measured by having access to the accelerometer and translational velocities measurements, for example

$$\begin{array}{ccc}\hfill \mu & =& {\displaystyle -\frac{a_x^b}{u}m}\hfill \end{array}$$

(49)

We have access to both of these measurements in Gazebo, so a circular trajectory tracking simulation was performed to measure these states. The calculated value for the quadrotor used in Gazebo is $\mu =0.18$.

With $\mu$ calculated, Figure 4 shows the Equation (48) on the X axis with the quadrotor following a circular trajectory, where it can be seen that the relation holds, so the calculated $\mu$ is correct. Note that the accelerometer readings ($a_x^b$) have noise added due to the rotors.

4.2.2. Monocular-Vision Algorithm

As mentioned before, the designed scale factor estimator works with any computer-vision algorithm that implements a monocular camera and delivers position measurements. For this simulation, we use ORB-SLAM2 [15] due to its precision and accuracy, it is easy to install and well-documented, and it has ROS integration. Hence, it is easy to add to the Gazebo environment.

4.2.3. Trajectory Tracking Control Using the Scale Factor Estimator

The simulation involves flying the quadrotor running ORB-SLAM2 from the visual information obtained by its onboard monocular camera without position control following a diagonal trajectory in the horizontal plane to obtain the information required to make the identities in (17) hold. After this step, the scale factor K will be known through ${\beta }_2$, so the quadrotor will be able to measure its actual position with $y_1$, (2). Then, the quadrotor will follow a lemniscate trajectory autonomously (closed-loop).

The low-level controller of the quadrotor is driven by [30], which is a driver to interface with Parrot AR-Drone quadrotors through ROS. It takes translational velocities as control inputs.

Figure 5 shows the Gazebo environment with the SLAM algorithm running. The window on the left shows what the onboard camera is seeing with the features (points) detected, and the window on the right depicts the mapping construction.

The first step is to fly the quadrotor in open-loop over a diagonal trajectory from position $(0,0)$ to $(3,3)$, after that to the position $(-3,-3)$ and finally back to the origin $(0,0)$, always maintaining a constant velocity of $u_d=0.2$ and $v_d=0.2$.

Figure 6 and Figure 7 show the real velocity and the estimated by the observer. With the initial conditions of the real velocity and the observer being the same, $V^b\left(0\right)={\left[000\right]}^{\top }$, and the high gains ${\gamma }_{vx}=30$, ${\gamma }_{vy}=30$ and ${\gamma }_{vz}=30$, make the observer converge immediately.

Figure 8 and Figure 9 show the scale factor estimation on the X and Y axes, respectively. Note that from time 0 s to 6 s, the estimator remains in zero because, in that period, the quadrotor takes off and holds in hover for some seconds, so Assumption 1 is not fulfilled. In the period 22 s to 60 s, some peaks appear because the quadrotor reaches the coordinates $(3,3)$ and $(-3,-3)$, respectively. Hence, it changes velocity abruptly to change its direction of movement to reach the next point. These changes in velocity can also be seen in Figure 6 and Figure 7 in the time periods.

After the open-loop routine, ${\beta }_2$ is calculated to converge at $k_x=0.175$ and $k_y=0.13$. It can also be seen in Figure 8 and Figure 9 that $\tilde{K}$ converges exponentially, as it was anticipated in (A7). From this point, knowing the scale factor K, the quadrotor will be able to measure its actual position through $y_1$ in (2) as follows

$$X=diag{\left(K\right)}^{-1}{y}_1$$

(50)

Finally, using the estimated information, the quadrotor will follow a lemniscate trajectory autonomously in closed-loop; that is, $x_d=1.5\cos \left(\omega t\right)$ and $y_d=\sin \left(2\omega t\right)$. Figure 10 and Figure 11 show the real position and the position measured by the quadrotor on the X and Y axis, respectively, along the lemniscate trajectory.

Figure 12 shows the $XY$ graph of the trajectory validating the closed-loop control of the quadrotor using the proposed scale factor estimator. The quadrotor completed the whole lemniscate trajectory twice with the given simulation time. The big arrow indicates the direction the onboard camera faces the whole trajectory, the line on the left represents the buildings’ location, and the small arrows indicate the quadrotor motion direction.

As shown in Figure 12, the SLAM algorithm has better precision in the left part of the lemniscate because the camera is closer to the buildings, so more image features fed the algorithm. On the diagonals, the SLAM algorithm performs better when the quadrotor moves towards the buildings than when it moves away. Although the scale factor estimator recovers the actual position dimension, it does not help in any sense to improve the SLAM algorithm performance.

Changing the position of the house on the right in the Gazebo environment, we ran a second simulation to check if small changes in the initial conditions of the monocular-vision algorithm influence the final value of the scale factor. In this case, the house on the right is closer to the quadrotor, as shown in Figure 13. At the end of the simulation, the calculated scale factor values were $k_x=0.232$ and $k_y=0.166$, proving that the scale factor is different even for the same environment but with minor changes in the initial conditions.

5. Conclusions

This article proposed a velocity observer in cascade with a scale factor estimator. The significant contributions of this work are listed next:

• The velocity observer does not neglect the Coriolis term, offering greater accuracy in fast flights.
• The scale factor estimator allows taking advantage of all the benefits of monocular cameras, obtaining the accuracy of a stereoscopic camera without increasing the processing power.
• Lyapunov’s arguments prove asymptotic convergence to zero of the observer and estimator errors, and the simulations validate the correct performance and use of the proposed theory.
• It is illustrated that the scale factor is not the same in all axes, as some authors assume. It is even different from experimenting in the same environment if the initial conditions change.
• The proposed approach allows for position trajectory tracking to be performed directly using the measurements of a monocular-vision positioning algorithm, removing the limitations of a GPS or a motion capture system.

In future work, experiments will be carried out by a real quadrotor in more complex environments, combined with other kinds of computer vision algorithms such as person recognition or obstacle avoidance.