# GPS-Denied Relative Motion Estimation For Fixed-Wing UAV Using the Variational Pose Estimator

## Abstract

Relative pose estimation between fixed-wing unmanned aerial vehicles (UAVs) is treated using a stable and robust estimation scheme. The motivating application of this scheme is that of “handoff” of an object being tracked from one fixed-wing UAV to another in a team of UAVs, using onboard sensors in a GPS-denied environment. This estimation scheme uses optical measurements from cameras onboard a vehicle, to estimate both the relative pose and relative velocities of another vehicle or target object. It is obtained by applying the Lagrange-d’Alembert principle to a Lagrangian constructed from measurement residuals using only the optical measurements. This nonlinear pose estimation scheme is discretized for computer implementation using the discrete Lagrange-d’Alembert principle, with a discrete-time linear filter for obtaining relative velocity estimates from optical measurements. Computer simulations depict the stability and robustness of this estimator to noisy measurements and uncertainties in initial relative pose and velocities.

## 1 Introduction

Onboard estimation of relative motion between unmanned vehicles and spacecraft is an important enabling technology for autonomous operations of teams and formations of such vehicles. A stable relative motion estimation scheme that is robust to measurement noise and requires no knowledge of the dynamics model of the vehicle being observed, is presented here. This estimation scheme can enhance the autonomy and reliability of teams of unmanned vehicles operating in uncertain GPS-denied environments. Salient features of this estimation scheme are: (1) use of only onboard optical sensors for estimation of relative pose and velocities; (2) robustness to uncertainties and lack of knowledge of dynamics model of observed vehicle; (3) low computational complexity such that it can be implemented with onboard processors; and (4) proven stability with large domain of attraction for relative motion state estimation errors. Stable and robust relative motion estimation of unconstrained motion of teams of unmanned vehicles in the absence of complete knowledge of their dynamics, is required for their safe, reliable, and autonomous operations in poorly known environments. In practice, the dynamics of an observed vehicle may not be perfectly known, especially in outdoor environments where the vehicle may be under the action of unknown forces and moments. The scheme proposed here has a single, stable algorithm for the naturally coupled relative translational and rotational motion between unmanned vehicles, using measurements from onboard optical sensors. This avoids the need for measurements from external sources, like GPS, which may not be available in indoor, underwater or cluttered environments [2, 16, 21].

Relative pose (position and attitude) estimation of one vehicle from another vehicle is treated here. Determining the relative attitude requires that at least three feature points on the observed vehicle are available. Attitude estimation and control schemes that use generalized coordinates or quaternions for attitude representation are usually unstable in the sense of Lyapunov, as has been shown in recent research [3, 5, 25]. One adverse consequence of these unstable estimation and control schemes is that they end up taking longer to converge compared to stable schemes with the same initial conditions and same initial transient behavior. Attitude observers and filtering schemes on and have been reported in, e.g., [4, 14, 15, 17, 18, 19, 24, 27, 31, 32]. These estimators do not suffer from kinematic singularities like estimators using coordinate descriptions of attitude, and they do not suffer from the unstable unwinding phenomenon encountered by continuous estimators using unit quaternions. Recently, the maximum-likelihood (minimum energy) filtering method of Mortensen [23] was applied to attitude and pose estimation on and , resulting in nonlinear estimation schemes that seek to minimize the stored “energy” in measurement errors [1, 34, 35]. This led to “near optimal” filtering schemes that are based on approximate solutions of the Hamilton-Jacobi-Bellman (HJB) equation and do not have provable stability. The estimation scheme obtained here is shown to be almost globally asymptotically stable. Moreover, unlike filters based on Kalman filtering, the estimator proposed here does not make any assumptions on the statistics of initial state estimate or sensor noise.

For the relative pose estimation problem analyzed in this paper, it is assumed that one vehicle can optically measure a known pattern fixed to the body of another vehicle whose relative motion states are to be estimated. From such optical (camera) measurements, the relative velocities (translational and angular) are also estimated. The variational attitude estimator recently appeared [10, 11], where it was shown to be almost globally asymptotically stable. The advantages of this scheme over Kalman-based schemes are reported in [9]. A companion paper extends the variational attitude estimator to estimation of coupled rotational (attitude) and translational motion. Maneuvering vehicles, like UAVs tracking ground targets, have naturally coupled rotational and translational motion. In such applications, designing separate state estimators for the translational and rotational motions may not be effective and could lead to poor navigation. For relative pose estimation between such vehicles operating in teams, the approach proposed here for robust and stable estimation will be more effective than Kalman filtering-based schemes. The estimation scheme proposed here can be implemented without any velocity measurements, which is useful when Doppler lidar sensors are not available onboard or rate gyros are corrupted by high noise content and bias [6, 7, 8].

## 2 Relative Navigation Using Optical Sensors

### 2.1 Motivation

When multiple UAV perform surveillance and target tracking missions in a GPS-denied environment, they need to ensure that they are tracking the same target of interest. When necessary, the tracking responsibility may need to be handed off from one UAV to another. When GPS signals are available, such a handoff procedure can be achieved by a tracking UAV geo-locating the target and sending the global coordinates of the target to a handoff UAV. In GPS-denied environments, such a handoff procedure faces several challenges. The most significant challenge is the following: because no GPS signals are available, the handoff UAV may not know the position of the tracking UAV. Therefore, it needs to use on-board sensors to detect and navigate towards the tracking UAV. Moreover, global information about the target is not available. Because the tracking UAV does not have GPS, it can only geo-locate the target in its own navigation frame. Since the handoff UAV has a different coordinate system than the tracking UAV, the target information from the tracking UAV cannot be directly used by the handoff UAV to track the target. The handoff UAV has to perform a coordinate transformation that converts the target information to its own navigation frame. This task is carried out by the relative pose estimation technique presented here.

### 2.2 Relative Pose Measurement Model

Let denote the observed vehicle and denote the vehicle that is observing . Let denote a coordinate frame fixed to and be a coordinate frame fixed to . Let be the rotation matrix from frame to frame and denote the position of origin of expressed in frame . The pose (transformation) of frame to frame is

(1) |

The positions of a fixed set of feature points or patterns on vehicle are observed by optical sensors fixed to vehicle . Velocities of these points are not directly measured, but may be calculated using a simple linear filter as in [10]. Assume that there are feature points, which are always in the sensor field-of-view (FOV) of the sensor fixed to vehicle , and the positions of these points are known in frame as , . These points generate unique pairwise relative position vectors, which are the vectors connecting any two of these points.

Denote the position of the optical sensor on vehicle and the vector from that sensor to an observed point on vehicle as and , , respectively, both vectors expressed in frame . Thus, in the absence of measurement noise

(2) |

where , are positions of these points expressed in . In practice, the are obtained from proximity optical measurements that will have additive noise; denote by the measured vectors. The mean values of the vectors and are denoted as and , and satisfy

(3) |

where , and is the additive measurement noise obtained by averaging the measurement noise vectors for each of the . Consider the relative position vectors from optical measurements, denoted as in frame and the corresponding vectors in frame as , for , . Therefore,

(4) |

where , with . Note that the matrix of known relative vectors is assumed to be known and bounded. Denote the measured value of matrix in the presence of measurement noise as . Then,

(5) |

where is the matrix of measurement errors in these vectors observed in frame .

### 2.3 Relative Velocities Measurement Model

Denote the relative angular and translational velocity of vehicle expressed in frame by and , respectively. Thus, one can write the kinematics of the rigid body as

(6) |

where and and is the skew-symmetric cross-product operator that gives the vector space isomorphism between and . In order to do so, one can differentiate (2) as follows

(7) |

where has full row rank. From vision-based or Doppler lidar sensors, one can also measure the velocities of the observed points in frame , denoted . Here, velocity measurements as would be obtained from vision-based sensors is considered. The measurement model for the velocity is of the form

(8) |

where is the additive error in velocity measurement . Instantaneous angular and translational velocity determination from such measurements is treated in [26]. Note that , for . As this kinematics indicates, the relative velocities of at least three beacons are needed to determine the vehicle’s translational and angular velocities uniquely at each instant. The rigid body velocities are obtained using the pseudo-inverse of :

(9) | ||||

(10) |

When at least three beacons are measured, is a full column rank matrix, and gives its pseudo-inverse. For the case that only one or two beacons are observed, is a full row rank matrix, whose pseudo-inverse is given by .

## 3 Dynamic Estimation of Motion From Proximity Measurements

In order to obtain state estimation schemes from measurements as outlined in Section 2 in continuous time, the Lagrange-d’Alembert principle is applied to an action functional of a Lagrangian of the state estimate errors, with a dissipation term linear in the velocities estimate error. This section presents the estimation scheme obtained using this approach. Denote the estimated pose and its kinematics as

(11) |

where is rigid body velocities estimate, with as the initial pose estimate and the pose estimation error as

(12) |

where is the attitude estimation error and . Then one obtains, in the case of perfect measurements,

(13) |

where for . The attitude and position estimation error dynamics are also in the form

(14) |

### 3.1 Lagrangian from Measurement Residuals

Consider the sum of rotational and translational measurement residuals between the measurements and estimated pose as a potential energy-like function. Defining the trace inner product on as

(15) |

the rotational potential function (Wahba’s cost function [33]) is expressed as

(16) |

where is a positive diagonal matrix of weight factors for the measured . Consider the translational potential function

(17) |

where is defined by (3), and is a positive scalar. Therefore, the total potential function is defined as the sum of the generalization of (16) defined in [11, 26] for attitude determination on , and the translational energy (17) as

(18) |

where is positive definite (not necessarily diagonal) which can be selected according to Lemma 3.2 in [11], and is a function that satisfies and for all . Furthermore, where is a Class- function [13] and denotes the derivative of with respect to its argument. Because of these properties of the function , the critical points and their indices coincide for and [11]. Define the kinetic energy-like function:

(19) |

where is an artificial inertia-like kernel matrix. Note that in contrast to rigid body inertia matrix, is not subject to intrinsic physical constraints like the triangle inequality, which dictates that the sum of any two eigenvalues of the inertia matrix has to be larger than the third. Instead, is a gain matrix that can be used to tune the estimator. For notational convenience, is denoted as from now on; this quantity is the velocities estimation error in the absence of measurement noise. Now define the Lagrangian

(20) |

and the corresponding action functional over an arbitrary time interval for ,

(21) |

such that . A Rayleigh dissipation term linear in the velocities of the form where is used in addition to the Lagrangian (20), and the Lagrange-d’Alembert principle from variational mechanics is applied to obtain the estimator on . This yields

(22) |

which in turn results in the following continuous-time filter.

### 3.2 Variational Estimator for Pose and Velocities

The nonlinear variational estimator obtained by applying the Lagrange-d’Alembert principle to the Lagrangian (20) with a dissipation term linear in the velocities estimation error, is given by the following statement.

###### Theorem 3.1

The nonlinear variational estimator for pose and velocities is given by

(23) |

where with defined by

(24) |

and is defined by

(25) |

where is defined as (16), and

(26) |

where is the inverse of the map.

The proof is presented in [12, 22]. In the proposed approach, the time evolution of has the form of the dynamics of a rigid body with Rayleigh dissipation. This results in an estimator for the motion states that dissipates the “energy” content in the estimation errors to provide guaranteed asymptotic stability in the case of perfect measurements [11]. The variational pose estimator can also be interpreted as a low-pass stable filter (cf. [30]). Indeed, one can connect the low-pass filter interpretation to the simple example of the natural dynamics of a mass-spring-damper system. This is a consequence of the fact that the mass-spring-damper system is a mechanical system with passive dissipation, evolving on a configuration space that is the vector space of real numbers, . In fact, the equation of motion of this system can be obtained by application of the Lagrange-d’Alembert principle on the configuration space . If this analogy or interpretation is extended to a system evolving on a Lie group as a configuration space, then the generalization of the mass-spring-damper system is a “forced Euler-Poincaré system” with passive dissipation, as is obtained here.

## 4 Discretization for Computer Implementation

For onboard computer implementation, the variational estimation scheme outlined above has to be discretized. Since the estimation scheme proposed here is obtained from a variational principle of mechanics, it can be discretized by applying the discrete Lagrange-d’Alembert principle [20]. Consider an interval of time separated into equal-length subintervals for , with and is the time step size. Let denote the discrete state estimate at time , such that where is the exact solution of the continuous-time estimator at time . Let the values of the discrete-time measurements , and at time be denoted as , and , respectively. Further, denote the corresponding values for the latter two quantities in inertial frame at time by and , respectively. The discrete-time filter is then presented in the form of a Lie group variational integrator (LGVI) in the following statement.

## 5 Numerical Simulations

This section presents numerical simulation results of the discrete time estimator described in Section 4, which is a Lie group variational integrator. Consider two vehicles performing spatial maneuvers, as shown in Fig. 1. These trajectories are generated using the equations of motion for these two vehicles and in turn generate the “true” relative states of one vehicle with respect to another. The UAV at higher altitude has a camera that has the lower UAV in its FOV at all instants. The initial relative attitude and relative position of the lower vehicle with respect to the higher vehicle, are:

(32) |

The initial relative angular and relative translational velocity of these two vehicles are:

(33) |

There are three feature points on the lower vehicle’s body, and their positions expressed in the lower vehicle’s body frame are

(34) |

Relative position vectors of these points are measured by the camera on the upper vehicle. Velocities of these points are calculated using the linear filter introduced in [10]. The relative velocities can be computed using these measurements by (9). All the camera readings contain random zero mean signals whose probability distributions are normalized bump functions with the width equal to 1 mm in each coordinate. The “inertia-like” gain matrices for the estimator are selected to be:

(35) |

The “dissipation” gain matrices for the estimator are set to:

(36) |

could be any function with the properties described in Section 3, but is selected to be here. The initial estimated states have the following values:

(37) | ||||

an |

The discrete-time estimator (27)-(31) is simulated over a time interval of s with time stepsize s. At each instant, (27) is solved using Newton-Raphson iterations to find . Then, the rest of the equations (all explicit) are solved consecutively to generate the estimated states. The principal angle of the relative attitude estimation error and components of the relative position estimate error are plotted in Fig. 2. Components of the relative angular and translational velocities are depicted in Fig. 3.

As can be noticed from the figures, all the estimated relative states converge to a bounded neighborhood of the corresponding true relative states, where the size of this neighborhood depends on the level of measurement noise and estimator gains. This confirms the stability and convergence properties of the estimator.

## 6 Conclusion

This article proposes an estimator for relative pose and relative velocities of one vehicle with respect to another vehicle that uses only optical measurements from onboard optical sensor(s). The sensors are assumed to provide measurements in continuous-time or at a high frequency, with bounded measurement noise due to limited fields of view. A Lagrangian in terms of measurement residuals and which can be expressed in terms of state estimation errors when perfect measurements are available, is proposed. Applying the Lagrange-d’Alembert principle to this Lagrangian with a dissipation term linear in relative velocity estimation errors, an estimator is designed on the Lie group of relative motions between two rigid vehicles. In the case of perfect measurements, this estimator is shown to be almost globally asymptotically stable with a domain of convergence that is open and dense in the state space. The continuous estimator is discretized by applying the discrete Lagrange-d’Alembert principle to the discretized Lagrangian and dissipation terms for rotational and translational motions. In the presence of measurement noise, numerical simulations with this discrete estimator show that state estimates converge to a bounded neighborhood of the true relative motion states. Future work will be directed towards creating higher-order discretizations of the continuous-time filter given here.

### References

- Aguiar, A., & Hespanha, J. (2006). Minimum-energy state estimation for systems with perspective outputs. IEEE Transactions on Automatic Control, 51(2), 226–241.
- Amelin, K. S., & Miller, A. B. (2014). An algorithm for refinement of the position of a light UAV on the basis of Kalman filtering of bearing measurements. Journal of Communications Technology and Electronics, 59(6), 622–631.
- Bayadi, R., & Banavar, R. N. (2014). Almost global attitude stabilization of a rigid body for both internal and external actuation schemes. European Journal of Control, 20(1), 45–54.
- Bonnabel, S., Martin, P., & Rouchon, P. (2009). Nonlinear symmetry-preserving observers on Lie groups. IEEE Transactions on Automatic Control, 54(7), 1709–1713.
- Chaturvedi, N. A., Sanyal, A. K., & McClamroch, N. H. (2011). Rigid-body attitude control. IEEE Control Systems Magazine, 31(3), 30–51.
- Goodarzi, F. A., Lee, D., & Lee, T. (2013). Geometric nonlinear PID control of a quadrotor UAV on SE(3). In Proceedings of the European Control Conference (pp. 3845–3850). Zurich, Switzerland.
- Goodarzi, F. A., Lee, D., & Lee, T. (2014). Geometric Adaptive Tracking Control of a Quadrotor UAV on SE(3) for Agile Maneuvers. ASME Journal of Dynamic Systems, Measurement and Control, 137(9), 091007.
- Goodarzi, F. A., Lee, D., & Lee, T. (2014). Geometric stabilization of a quadrotor UAV with a payload connected by flexible cable. In Proceedings of the American Control Conference (pp. 4925–4930). Portland, OR, USA.
- Izadi, M., Samiei, E., Sanyal, A. K., & Kumar, V. (2015). Comparison of an attitude estimator based on the Lagrange-d’Alembert principle with some state-of-the-art filters. In Proceedings of the IEEE International Conference on Robotics and Automation (pp. 2848–2853). Seattle, WA, USA.
- Izadi, M., Sanyal, A. K., Samiei, E., & Viswanathan, S. P. (2015). Discrete-time rigid body attitude state estimation based on the discrete Lagrange-d’Alembert principle. In Proceedings of the American Control Conference (pp. 3392–3397). Chicago, IL, USA.
- Izadi, M., & Sanyal, A. K. (2014). Rigid body attitude estimation based on the Lagrange-d’Alembert principle. Automatica, 50(10), 2570–2577.
- Izadi, M., & Sanyal, A. K. (2015). Rigid body pose estimation based on the Lagrange-d’Alembert principle. To appear in Automatica.
- Khalil, H. K. (2001). Nonlinear Systems (3 edition). Prentice Hall, Upper Saddle River, NJ.
- Khosravian, A., Trumpf, J., Mahony, R., & Hamel, T. (2015). Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements. In Proceedings of the American Control Conference (pp. 3199–3205). Chicago, IL, USA.
- Khosravian, A., Trumpf, J., Mahony, R., & Lageman, C. (2015). Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs. Automatica, 55, 19–26.
- Leishman, R. C., McLain, T. W., & Beard, R. W. (2014). Relative navigation approach for vision-based aerial GPS-denied navigation. Journal of Intelligent & Robotic Systems, 74(1-2), 97–111.
- Mahony, R., Hamel, T., & Pflimlin, J. M. (2008). Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, 53(5), 1203–1218.
- Maithripala, D. H., Berg, J. M., & Dayawansa, W. P. (2004). An intrinsic observer for a class of simple mechanical systems on a Lie group. In Proceedings of the American Control Conference (pp. 1546–1551). Boston, MA, USA.
- Markley, F. L. (2006). Attitude filtering on SO(3). The Journal of the Astronautical Sciences, 54(4), 391–413.
- Marsden, J. E., & West, M. (2001). Discrete mechanics and variational integrators. Acta Numerica, 10, 357–514.
- Miller, A., & Miller, B. (2014). Tracking of the UAV trajectory on the basis of bearing-only observations. In Proceedings of the 53rd Annual Conference on Decision and Control (pp. 4178–4184). Los Angeles, CA, USA.
- Misra, G., Izadi, M., Sanyal, A. K., & Scheeres, D. J. (2015). Coupled orbit-attitude dynamics and relative state estimation of spacecraft near small Solar System bodies. Advances in Space Research.
- Mortensen, R. E. (1968). Maximum-likelihood recursive nonlinear filtering. Journal of Optimization Theory and Applications, 2(6), 386–394.
- Rehbinder, H., & Ghosh, B. K. (2003). Pose estimation using line-based dynamic vision and inertial sensors. IEEE Transactions on Automatic Control, 48(2), 186–199.
- Sanyal, A. K., Fosbury, A., Chaturvedi, N. A., & Bernstein, D. S. (2009). Inertia-free spacecraft attitude tracking with disturbance rejection and almost global stabilization. Journal of Guidance, Control, and Dynamics, 32(4), 1167–1178.
- Sanyal, A. K., Izadi, M., & Butcher, E. A. (2014). Determination of relative motion of a space object from simultaneous measurements of range and range rate. In Proceedings of the American Control Conference (pp. 1607–1612). Portland, OR, USA.
- Sanyal, A. K., Lee, T., Leok, M., & McClamroch, N. H. (2008). Global optimal attitude estimation using uncertainty ellipsoids. Systems & Control Letters, 57(3), 236–245.
- Shen, S., Mulgaonkar, Y., Michael, N., & Kumar, V. (2013). Vision-based state estimation and trajectory control towards aggressive flight with a quadrotor. In Proceedings of the Robotics Science and Systems.
- Shen, S., Mulgaonkar, Y., Michael, N., & Kumar, V. (2013). Vision-based state estimation for autonomous rotorcraft MAVs in complex environments. In Proceedings of the IEEE International Conference on Robotics and Automation (pp. 1758–1764). Karlsruhe, Germany.
- Tayebi, A., Roberts, A., & Benallegue, A. (2011). Inertial measurements based dynamic attitude estimation and velocity-free attitude stabilization. In Proceedings of the American Control Conference (pp. 1027–1032). San Francisco, CA, USA.
- Vasconcelos, J. F., Cunha, R., Silvestre, C., & Oliveira, P. (2010). A nonlinear position and attitude observer on SE(3) using landmark measurements. Systems & Control Letters, 59, 155–166.
- Vasconcelos, J. F., Silvestre, C., & Oliveira, P. (2008). A nonlinear GPS/IMU based observer for rigid body attitude and position estimation. In Proceedings of the IEEE Conference on Decision and Control (pp. 1255–1260). Cancun, Mexico.
- Wahba, G. (1965). A least squares estimate of satellite attitude, Problem 65-1. SIAM Review, 7(5), 409.
- Zamani, M. (2013). Deterministic Attitude and Pose Filtering, an Embedded Lie Groups Approach. Ph.D. Thesis. Australian National University, Canberra, Australia.
- Zamani, M., Trumpf, J., & Mahony, R. (2013). Minimum-energy filtering for attitude estimation. IEEE Transactions on Automatic Control, 58(11), 2917–2921.