Eric Heitz

A Simpler and Exact Sampling Routine for the GGX Distribution of Visible Normals

.

In this document, we propose an alternative sampling routine for the GGX VNDF that is simpler and exact. The solution provided by Heitz and d'Eon was approximate for GGX because it required a fitted curve: the derivations were made in slope space and the conditional inverse CDF of GGX does not have a closed form with this parameterization. This improved sampling routine uses the property that for unit roughness α = 1 the GGX distribution is a uniform hemisphere [HDCD15]. Sampling the GGX VNDF is thus equivalent to sampling the projected area of a hemisphere, which can be done exactly.

V T 2 T 1 V N P
Figure 1: Importance sampling the projected area of a hemisphere. The half disks yield the silhouette of the hemisphere projected onto direction V . We sample its projected area by sampling points P on these half disks and projecting them on the hemisphere onto direction V . § ¤ Stretch configuration As explained by Heitz and d'Eon [START_REF] Heitz | Importance sampling microfacet-based BSDFs using the distribution of visible normals[END_REF], we obtain the view vector in the configuration where α x = α y = 1 by scaling its x and y components respectively α x and α y and renormalizing it.

Orthonormal basis around the view vector We construct an orthonormal basis (V, T 1 , T 2 ) where T 1 is in the tangent plane:

T 1 = V × (0 0 1) V × (0 0 1) , (1)
T 2 = T 1 × V. ( 2
)
Projected areas of the half disks The projected area of the hemisphere is the sum of the projected areas of the two half disks show in Figure 1. The projected area of the half disk located in the tangent plane (in green) is proportional to Z • V = z v and the projected area of the other half disk (in blue) is proportional to

T 2 • V = 1.
Sampling the half disks We sample the projected area by generating samples on the half disks proportionally to their respective projected areas, i.e. with respective probabilities a = 1 1+zv and 1 -a. For this, we use a polar parameterization of the disk (r, φ) in which we scale the angle φ in order to account for the difference of projected areas of its two halves (see Figure 2):

r = U 1 (3) φ =      U 2 a π if U 2 < a, U 2 -a 1 -a π + π otherwise. (4)
If the sampled point is in the tangent half disk (U 2 ≥ a, in green) we obtain its projection in the vieworthogonal plane (T 1 , T 2 ) by scaling its second component by z v : P = r cos(φ) T 1 + r sin(φ) T 2 if U 2 < a, r cos(φ) T 1 + r sin(φ) z v T 2 otherwise.

(5)

φ r U 1 ∈ [0, a] U 1 ∈ (a, 1]
Figure 2: Sampling a point in the two half disks with different weights.

  vec3 sampleGGXVNDF(vec3 V_, float alpha_x, float alpha_y, float U1, float U2) { // stretch view vec3 V = normalize(vec3(alpha_x * V_.x, alpha_y * V_.y, V_.z)); // orthonormal basis vec3 T1 = (V.z < 0.9999) ? normalize(cross(V, vec3(0,0,1))) : vec3(1,0,0); vec3 T2 = cross(T1, V); // sample point with polar coordinates (r, phi) float a = 1.0 / (1.0 + V.z); float r = sqrt(U1); float phi = (U2<a) ? U2/a * M_PI : M_PI + (U2-a)/(1.0-a) * M_PI; float P1 = r*cos(phi); float P2 = r*sin(phi)*((U2<a) ? 1.0 : V.z); // compute normal vec3 N = P1*T1 + P2*T2 + sqrt(max(0.0, 1.0 -P1*P1 -P2*P2))*V; // unstretch N = normalize(vec3(alpha_x*N.x, alpha_y*N.y, max(0.0, N.z))); return N; }

Unstretch the normal Finally, to obtain the normal in the original configuration, we scale the x and y components of N by respectively α x and α y and we renormalize.

Remarks

The view direction is located in the upper hemisphere This sampling routine works under the assumption that if the view direction V is located in the upper hemisphere, i.e. z v ≥ 0. This is usually an assumption of the BRDF models. However, there are special cases where this sampling routine cannot be used as is, for instance the Smith multiple-scattering BRDF model [START_REF] Heitz | Multiple-scattering microfacet bsdfs with the smith model[END_REF] models incident rays that can be located in the lower hemisphere after their first bounce on the microsurface.

Floating-point precision In the proposed implementation, the function has two max(0.0, ..) instructions in order to avoid errors due to floating-point precision.

Improved sampling routine for the Beckmann distribution See also Wenzel Jakob's improvement for the Beckmann distribution [Jak14].


Jean-Philippe Condomines

Cédric Seren

Gautier Hattenberger

Invariant Unscented Kalman Filter with application to attitude estimation

The Invariant UKF, named IUKF, is a recently introduced algorithm dedicated to nonlinear systems possessing symmetries as illustrated by the quaternion-based kinematics modeling of a mini-UAV (Unmanned Aircraft Vehicle) considered in this paper. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetry-preserving terms, without requiring any compatibility condition such as proposed in the π-IUKF, by introducing both notion of invariant output errors and UKF algorithm formulation. We propose in this paper to evaluate the applicability of our proposed IUKF observer to the case of attitude estimation for small UAVs using low-cost sensors. The IUKF algorithm is successfully validated in experiments and demonstrates that nonlinear state estimation converges on a much bigger set of trajectories than for more traditional approaches.

I. INTRODUCTION

Many recent progresses in the miniaturization of electromechanical sensors have led to the design of small and cheap integrated navigation system hardwares (complete IMU: Inertial Measurement Unit, GPS : Global Positioning System module, etc.), which have, for their part, contributed to boost significantly the market of mini-UAVs (Unmanned Air Vehicles) over the last decades, making them more accessible to everyone. Nevertheless, this accessibility is frequently inconsistent with good measurement performances. For instance, the GPS modules commonly used with the Paparazzi 1 autopilot deliver an absolute position with an average accuracy of 5 meters, up to 10m under certain flight conditions. Therefore, a need for multiple microelectromechanical sensors (MEMS) data fusion arises, especially when the final objective consists in developing robust and powerful advanced control strategies for mini-UAVs that can be viewed as complex autonomous electromechanical system. So much so that full state (or estimated state) feedback designs (cf. LQG/LTR syntheses) provide full authority to control efficiently in terms of stability and performances UAVs for accomplishing various missions. To this aim, nonlinear estimation offers several well-proven algorithmic techniques which permit to recover an acceptable level of accuracy on some key flight parameters (anemometric angles, orientation/attitude, linear and angular speeds, position, etc.) for mini-UAVs closed-loop handling qualities.

a Assistant-professor at ENAC, University of Toulouse, Toulouse, France jean-philippe.condomines@recherche.enac.fr b Researcher at the Department of Control Systems and Flight Dynamics, ONERA-The French Aerospace Lab, Toulouse, France cedric.seren@onera.fr c Assistant-professor at ENAC, University of Toulouse, Toulouse, France gautier.hattenberger@enac.fr 1 https://paparazziuav.org/ An overview of nonlinear estimation methods can be found in the literature from many surveys or books (e.g., [START_REF] Crassidis | Survey of Nonlinear Attitude Estimation Methods[END_REF], [START_REF] Crassidis | Optimal Estimation of Dynamic Systems[END_REF]). Figure 1 attempts to propose a classification of these latter and positions the article's topic in it (white terms in grey boxes). As they merge different nonlinear estimation principles, Kalman-based invariant observers can be qualified as hybrid filters. Although dynamical systems possessing symmetries have been studied in control theory, few results taking benefit of system invariances for observers design exist today. Invariant nonlinear estimation theory appears so as a young research area in which the first main contributions can be dated from the beginning of 2000s ([1], [START_REF] Barczyk | Invariant Observer Design for Helicopter UAV Aided Inertial Navigation System[END_REF], [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF], [START_REF] Barrau | Invariant particle filtering with application to localization[END_REF], [START_REF] Barrau | Intrinsic filtering on Lie groups with applications to attitude estimation[END_REF], [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF], [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF], [START_REF] Bonnabel | Symmetry-preserving observers[END_REF], [START_REF] Diemer | An invariant linear quadratic Gaussian controller for a simplified car[END_REF], [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF], [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF], [START_REF] Khosravian | Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs[END_REF], [START_REF] Martin | Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors[END_REF]). An important drawback in this method is that it requires to linearize a system of differential equations which govern the invariant state estimation error dynamics. Such an operation appears 3 Initially, research was going on in the development of constructive methods to derive invariant observers for nonlinear estimation purposes which preserve systems' symmetries. If this kind of non-systematic approaches keeps physical readiness, it requires however to tune an important number of setting parameters potentially when computing estimation gains, which can be cumbersome for complex system modelings. That is why, researchers have then tried to develop more systematic techniques which are able to facilitate estimators' gains computation. There exist two major approaches to perform Bayesian filtering for a state evolving on an Euclidean space : the Kalman filter (KF) such as (Extended Kalman filter or Unscented Kalman filter) and the particle filters. However among those methods only a few works tried to extend them to manifolds. The Invariant Extended Kalman Filter (IEKF -cf. bibliographical references [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF], [START_REF] Barrau | Intrinsic filtering on Lie groups with applications to attitude estimation[END_REF]) permits to determine gain matrices for minimum variance estimation. This optimality must be considered here w.r.t. an invariant state estimation error which will be defined precisely further. An important drawback in this method is that it requires to linearize a system of differential equations which govern the invariant state estimation error dynamics. Such an operation appears suitable for simple system modelings only s.t. UAVs whose dynamics can be represented easily based on kinematics relationships. Indeed, this kind of nonlinear state space representation can be differentiated analytically towards its state vector. For more complex system modelings, this linearization may be difficult to carry out. Nevertheless, the IEKF, and more generally invariant observers, are characterized by a larger convergence domain, due to the exploitation of systems' symmetries within the estimation algorithm (i.e., within filter equations and gains computation), and present very good performances in practice. In order to derive more tractable nonlinear invariant state estimation algorithms, motivated by the practical problems encountered by the authors with mini-UAVs flight control and guidance, civil Aircraft modeling and identification and dynamic system fault detection, isolation and recovery, an hybridization of the Unscented KF (UKF) principles [START_REF] Van Der Merwe | The square-root unscented Kalman filter for state and parameter estimation[END_REF], [START_REF] Julier | Unscented filtering and nonlinear estimation[END_REF], [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continous-time nonlinear systems[END_REF] with invariant observers theory has been recently proposed in [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF], [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF]. Among other things, it has been proved in these bibliographical references that an Invariant UKF-like estimator (named IUKF) could be simply designed by introducing both notions of invariant state estimation and invariant output errors within any UKF algorithm formulation, whatever this latter corresponds to the standard version of the algorithm or to some square-root/UD factorized ones. This article focuses on these recent research works and details the theoretical aspects in the sequel.

In the sequel, §II presents the theoretical background of our proposed IUKF estimation algorithm and an illustrative example fitted out with a tilt sensor system. §III gathers all the results obtained after solving the Attitude and Heading Reference System (AHRS) estimation problem in real conditions.

II. THE INVARIANT UNSCENTED KALMAN FILTER

Inspired by the theory of continuous-time symmetry preserving observer [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF] a novel and original UKF-based approach has been developed in [START_REF] Condomines | Optimal Invariant Observers Theory for Nonlinear State Estimation[END_REF] to address the approximation issue of the invariant EKF without requiring any linearization of the dynamical systems equations or compatibility condition such as proposed in the π-IUKF algorithm [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF], [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF]. The IUKF relies on the basic theoretical principles developed by Julier and Uhlmann at the beginning of 2000s (see [START_REF] Julier | Unscented filtering and nonlinear estimation[END_REF]) which have been since widely applied to various nonlinear state estimation problems (cf. [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continous-time nonlinear systems[END_REF]).

Proposition 1: Considering a whole state space representation, the composite transformation φ gPG " pψ g , ϕ g , ρ g q and starting from initial values x0 " Erx 0 s, P xx 0 " Erηpx 0 , x0 qη T px 0 , x0 qs the two-steps procedure (prediction/correction) permit to design the following invariant nonlinear state observer in discrete time: @i P rr 0 ; 2n ss,

X piq k`1|k " f d pX piq k|k , u k q ñ xk`1|k " 2n ÿ i"0 W piq m X piq k`1|k S xx k`1|k " $ ' ' ' ' ' & ' ' ' ' ' % qr " a W p1q c ´ηpx k`1|k , X p1q k`1|k q ¨. . . ηpx k`1|k , X p2nq k`1|k q ¯V1{2 k  cholupdate ´Sxx k`1|k , ηpx k`1|k , X p0q k`1|k q, W p0q c @i P rr 0 ; 2n ss, ŷpiq k`1|k " h d pX piq k|k , u k q ñ ŷk`1|k " 2n ÿ i"0 W piq m ŷpiq k`1|k S yy k`1|k " $ ' ' ' ' ' ' ' ' ' & ' ' ' ' ' ' ' ' ' % qr " a W p1q c ˆEpX p1q k`1|k , I X p1q k`1|k u k
, ŷk`1|k q . . .

EpX p2nq k`1|k , I X p2nq k`1|k u k , ŷk`1|k q ˙W1{2 k  cholupdate ´Syy k`1|k , . . . EpX p0q k`1|k , I X p0q k`1|k u k , ŷk`1|k q, W p0q c P xy k`1|k " 2n ÿ i"0 W piq c ηpX piq k`1|k , xk`1|k qE T pX piq k`1|k , I X piq k`1|k u k
, ŷk`1|k q @i P rr 1 ; n ss, s K i rEs " i th row of K " pP xy k`1|k {pS yy k`1|k q T q{S yy k`1|k

F : xk`1|k`1 " xk`1|k `n ÿ i"1 s K i rEs ¨Epx k`1|k , I xk`1|k u k , z k`1 q.w i px k`1|k q S xx k`1|k`1 " cholupdate ´Sxx k`1|k , KS yy k`1|k , ´1P
revious matricial computations rely on both QR decomposition and rank 1 update to Cholesky factorization (cholupdate). Local transformations pψ g , ϕ g , ρ g q are here defined as for a dynamical system preserving symmetries [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF]. In this formulation, state, output and crossed error covariances are now defined from system modeling invariants. It is clear by transitivity that these matricial quantities are left unchanged by the composite transformation φ gPG " pψ g , ϕ g , ρ g q. Unlike the Invariant Extended Kalman Filter (IEKF), the proposed IUKF does not require a linearization of 9 ηpx t , xt q w.r.t η for its gain matrix computation step. When any given permanent trajectory t Þ Ñ px p ptq, u p ptqq is followed (i.e., s.t. @t, I xp up ptq " Ī), 1 st order approximation of the dynamical error η shows that if K is also determined s.t. matrix BΥp0, Īq{Bη is stable, then observer F will converge locally around px p ptq, u p ptqq. Reuse of system modeling invariances within invariant observer design also guarantees that it will converge for any group action image pψ g pu p ptqq, ϕ g px p ptqqq gPG . This property is remarkable especially for dynamical systems described by kinematics relationships whose dynamics is invariant by translation and rotation movements inside an invariant frame. By doing this, correction step procedure relies on the determination of the n additive gain which depend on system fundamental invariants and invariant innovation terms. Moreover, the invariant correction terms are projected on each component of the dynamical equations by considering the canonical basis of R n such as Bpx k`1|k q " tω i px k`1|k qu iPrr 1 ; n ss vectors form an invariant frame for each x P X . Thus, the IUKF algorithm relies on a multiple parametrization defined by local transformation groups. Considering the transformation group φ g " pϕ g , ψ g , ρ g q each inverse of sigma point can be defined as a local parameter of p2n`1q invariant frame which project each sigma point on the neutral element e thought the local application ϕ g . As no specific autopilot hardware device has been designed for this experiment, it is noteworthy that data fusion will merge low quality measurement signals delivered directly by the cheap MEMS sensors which equip any Parrot quadrotor. The interest of the following results relies less on the ability of the IUKF algorithm to estimate systems' states and outputs than on the practical verification of the theoretical properties asserted by the invariant observers framework when dealing with real data. To point out these latter, the results obtained with the IUKF algorithm have been systematically compared with the ones provided by a standard UKF approach. To lead a fair comparison, both techniques share identical setting parameters values i.e., similar estimated process and measurement covariances values for matrices V and W. Figures 3 and6 gather some estimation results due to a lack of space. Figure 3 shows the estimation results of the quaternion state components obtained by both UKF and IUKF algorithms. It is noticeable that both methods provide correct estimates w.r.t. the absolute references plotted in solid red lines. The differences between the two algorithms appear when we consider the dispersion around the estimated state trajectory. Indeed, the lines plotted on figure 5 , which correspond to the qptq˘3ˆσ q ptq standard deviations around the mean estimated value, tend to prove that the IUKF estimation algorithm calculates more trustful quaternion estimates, or at least reduces the dispersion of these state estimates, due to the invariant framework used.

III. BENCHMARK AND APPLICATION ON ATTITUDE

Based on these quaternion estimates, the instantaneous Euler attitude angles values, which describe at any time brought by the invariant observer theory used to design our IUKF algorithm can be observed on Figures 5 and6, which display, through time, both computed theoretical standard deviations and filters correction gains. By merging these results, it can be concluded that our proposed IUKF estimation technique is characterized by quasi-constant estimated standard deviations and correction gains w.r.t. any standard UKF estimation algorithm. Exploiting system's dynamics invariances in order to design nonlinear state estimation approaches allows to construct powerful nonlinear observers whose properties will be quasi-independent from the current followed trajectory.Therefore, state estimation uncertainties can be quantified by quasi-constant values through time (see for instance standard deviations on q). This paves the way for designing less conservative, but robust, estimated state-feedback control laws in order to improve mini-UAVs flying and handling qualities. It is noteworthy that the reasoning made previously about the residual estimation uncertainties which impair p ωb , âs , bs q for both techniques is still valid when analyzing the results of Figure 5. Although it has not been demonstrated by any theoretical proof, we can notice that, when they are consistent with UKF-based results (i.e., not for the state components related to gyrometer biases), the theoretical standard deviations computed by our proposed IUKF estimation technique minimize along the whole experiment the ones determined by the standard UKF methodology. This is especially remarkable for quaternion components estimates q " pq 0 q1 q2 q3 q T . Similarly to the theoretical standard deviations computed by the IUKF observer, the correction gains (cf. Fig. 6) appear less sensitive to the non-stationary noises levels, so that we can conclude that the invariant framework offers a better high frequency perturbations rejection in terms of filtering capabilities. In the case of the IUKF estimator, these gains could be also approximated by constant values, after a given transient regime, rather than in the case of the standard UKF algorithm.

IV. CONCLUSION AND FUTURE PROSPECTS

This article has presented an innovative procedure to derive an invariant observer for nonlinear state estimation. This latter, named IUKF, combines both invariant observers theory and unscented filtering principles. The experimental results presented and gathered in §III-C have shown an equivalent capability of our proposed IUKF technique in comparison with an UKF method for nonlinear state estimation. These results have also permitted to check in realistic conditions some invariance properties which characterize our designed observer. Among these latter, stability of estimated standard deviations, which characterize estimated state trajectory uncertainties, must be highlighted since it could facilitate new control strategies design with less conservatism. Future works will investigate the possibility to use constant gain matrices, optimized offline by the IUKF, into a complementary observer. The benefit of this solution would be to take advantage of the computational simplicity of the complementary observer but with optimal correction terms provided by the IUKF.

Fig. 1 .

 1 Fig. 1. Classification of existing nonlinear state estimation techniques and paper topic positioning.

Fig. 1 .

 1 Fig. 1. Classification of existing nonlinear state estimation techniques and paper topic positioning.

Fig. 3 .

 3 Fig. 3. Quaternion states estimation.

4 .

 4 Euler angles attitude estimation on φ and associated errors instant the orientation of the flying Parrot, have been deduced through time and compared with the absolute references determined by the OptiTrack system. It appears that both algorithms allow to reconstruct a suitable attitude estimation for control purposes. Due to a lack of space only the φ-axis estimation state error w.r.t. the absolute references is drawn in figure4using a logarithmic scale and show comparable results for both techniques. Expected differences

Fig. 5 .

 5 Fig. 5. Estimated theoretical standard deviations: comparison UKF/IUKF (state components).

Fig. 6 .

 6 Fig. 6. Correction gains: comparison UKF (red) /IUKF (blue) (state components).

Unscented filters: UKF, sigma

  points and particles filters. . .

	Approach		State manifold	System	Filter
	[Bonnabel et al. (2009)]	Lie groups		Continuous	EKF
	[Barrau and al. (2013)]		SO(3)		Continuous-Discrete EKF
	[Snousi and al. (2006)]		Riemannian		Discrete	PF
	[Tompkins and al. (2007)]	Stiefel		Discrete	PF
	[Rentmeesters and al. (2010)] Grassmann		Discrete	PF
	[Bourmaud and al. (2013)]	Matrix Lie groups Discrete	EKF
	[Bourmaud and al. (2014)]	Matrix Lie groups Continuous-Discrete EKF
	[Hauberg and al. (2013)]	Riemannian		Discrete	UKF
	This paper Table 1 Categorization of the state of the art approaches on Kalman and Particle filtering for a state evolving on a Lie groups Discrete UKF manifold (PF:Particle Filter, EKF: Extended Kalman Filter, UKF: Unscented Kalman Filter
	Kalman filter (KF) such as (Extended Kalman filter or Unscented Kalman filter) and the particle
	filters. Howerver among those methods only a few works tried to extend them to manifolds (see
	table 1). The Invariant Extended Kalman Filter (IEKF -cf. bibliographical references Bonnabel et
	al. (2009); Bonnabel (2007)) permits to determine gain matrices for minimum variance estimation.
	This optimality must be considered here w.r.t. an invariant state estimation error which will be
	defined precisely further.
			NONLINEAR ESTIMATION
	Model-Based Methods	Data-Based Methods
	State Estimation	Parameter Estimation	Mixed ‹ Estimation
	Kalman approaches: EKF, MEKF, additive EKF, QUEST. . .
	Nonlinear observers: high-gains, normal form, backstepping, moving
	horizon, sliding mode, pseudospectral, invariant. . .
	Adaptive techniques: noise/nonlinear adaptive estimators. . .
	‹ "Mixed" means joint state/parameter estimation.

ESTIMATION

A. Dynamic system modeling

This subsection details the generic modeling used to tackle and solve the issue of estimating some key flight variables (attitude-orientation, angle rates, etc.) of mini-UAVs fitted out with an Attitude and Heading Reference System (AHRS). UAVs dynamics representation corresponds here to a pure quaternionial kinematics modeling (whose related quaternion will be denoted by q), supplemented by additive state variables which represent low frequency sensors' imperfections (such as slowly varying biases denoted by a s and b s ). Thereby, we consider:

where ω m is seen as an imperfect and noisy, but known, measured input, like B. Constant A " p0 0 gq T refers to the local Earth's gravity vector. Nonlinear state space representation of Eq. (1) can be described in a compact form s.t. 9

x " f px, uq and y " hpx, uq where u " ω m , x " pq T ω T b a s b s q T and y " py T A y T B q T are the input, state and output vectors respectively. The nonlinear state estimation problem makes use of 3 triaxial sensors which deliver a total of 9 scalar measurement signals: « 3 magnetometers permit to obtain a local measurement of Earth's magnetic field, which is known constant and expressed in the body-fixed frame s.t. vector y B " q ´1˚B˚q (where B " pB x B y B z q T ) can be considered as an output of the observation equations; « 3 gyroscopes produce the measurements associated with the instantaneous angular rates gathered in ω m P R 3 s.t. ω m " pω mx ω my ω mz q T ; « and 3 accelerometers provide the measured output signals corresponding to the specific acceleration, denoted by a m P R 3 with a m " pa mx a my a mz q T . As no velocity and position informations are available (no GPS, nor airspeed sensor), this AHRS is often qualified as non-aided.

B. IUKF estimator derivation

Let's consider the observer in [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF] for the IUKF algorithm s.t.: 9 q " q ˚pω m ´ω b q 2 `. . . The invariant state estimation error dynamics 9 ηpx t , xt q is given by:

rEs.E B q As it was mentioned, the reader can notice that the invariant state estimation error dynamics depends on system's trajectory t Þ Ñ px t , u t q through the invariant quantity I x u which is a major difference with most of nonlinear estimators. Unlike the Invariant Extended Kalman Filter (IEKF -see references [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF]), the proposed IUKF does not require a linearization of 9 ηpx t , xt q w.r.t. η for its gain matrix computation step. This linearization can appear as a difficult operation in itself and especially for any practical implementation.

C. Experimental results

We evaluate the IUKF performances experimentally by post-processing a set of experimental data on the basis of both the dynamical modeling of Eq. ( 1) and the filtering equations of Eq. (2). Figure 2 quadrotor mini-UAV under test and an image of the indoor flight performed to gather these real data. It also illustrates that this experiment has been made using an OptiTrack system which permits to have at disposal absolute references (see http://www.optitrack.com/).


Jean-Philippe Condomines
email: jean-philippe.condomines@recherche.enac.fr

Cédric Seren
email: cedric.seren@onera.fr

Gautier Hattenberger
email: gautier.hattenberger@enac.fr

The Invariant Unscented Kalman Filter

This article proposes a novel approach for nonlinear state estimation. It combines both invariant observers theory and unscented filtering principles whitout requiring any compatibility condition such as proposed in the π-IUKF algorithm. The resulting algorithm, named IUKF (Invariant Unscented Kalman Filter), relies on a geometrical-based constructive method for designing filters dedicated to nonlinear state estimation problems while preserving the physical invariances and systems symmetries. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetry-preserving terms without requiring any linearization and highlighting remarkable invariant properties. As a result, the estimated covariance matrices of the IUKF converge to quasi-constant values due to the symmetry-preserving property provided by the invariant framework. This result enables the development of less conservative robust control strategies. The designed IUKF method has been successfully applied to some relevant practical problems such as the estimation of attitude for aerial vehicles using low-cost sensors reference systems. Typical experimental results using a Parrot quadrotor are provided in this paper.

I. INTRODUCTION

An overview of nonlinear estimation methods can be found in the litterature from many surveys or books [START_REF] Crassidis | Survey of Nonlinear Attitude Estimation Methods[END_REF], [START_REF] Crassidis | Optimal Estimation of Dynamic Systems[END_REF]. As they merge different nonlinear estimation principles, Kalman-based invariant observers can be qualified as hybrid filters. Although dynamical systems possessing symmetries have been studied in control theory, few results taking benefit of system invariances for observers design exist today. Invariant nonlinear estimation theory appears so as a young research area in which the first main contributions can be dated from the beginning of 2000s [START_REF] Aghannan | On invariant asymptotic observers[END_REF], [START_REF] Bonnabel | Symmetry-preserving observers[END_REF], [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF], [START_REF] Barrau | Intrinsic filtering on Lie groups with applications to attitude estimation[END_REF], [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF], [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF], [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF], [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF], [START_REF] Khosravian | Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs[END_REF], [START_REF] Martin | Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors[END_REF]. Initially, research was going on in the development of constructive methods to derive invariant observers for nonlinear estimation purposes which preserve systems' symmetries. If this kind of non-systematic approaches keeps physical readiness, it may require to tune an important number of setting parameters when computing estimation gains, which can be cumbersome for complex system modelings. That is why, more systematic techniques have been developped which are able to facilitate estimators' gains computation. There exist two major approaches to permform Bayesian filtering for a state evolving on an Euclidean space : the Kalman filter (KF) such as (Extended Kalman filter or Unscented Kalman filter) and the particle filters. Howerver among those methods only a few works tried to extend them to manifolds (see table I). The Invariant Extended Kalman Filter (IEKF - [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF], [START_REF] Barrau | Intrinsic filtering on Lie groups with applications to attitude estimation[END_REF]) permits to determine gain matrices for minimum variance estimation. This optimality must be considered here w.r.t. an invariant state estimation error which will be defined precisely further. An important drawback in this method is that it requires to linearize the system of differential equations which govern the invariant state estimation error dynamics. Such an operation appears suitable for simple system modelings only s.t. UAVs whose dynamics can be represented easily based on kinematics relationships. Indeed, this kind of nonlinear state space representation can be differentiated analytically towards its state vector. For more complex system modelings, this linearization may be difficult to carry out. Nevertheless, the IEKF, and more generally invariant observers, are characterized by a larger convergence domain, due to the exploitation of systems' symmetries within the estimation algorithm (i.e., within filter equations and gains computation), and present very good performances in practice. In order to derive more tractable nonlinear invariant state estimation algorithms, motivated by the practical problems encountered by the authors with mini-UAVs flight control and guidance, civil aircraft modeling and identification and dynamic system fault detection, isolation and recovery, an hybridization of the Unscented KF (UKF) principles [START_REF] Julier | Unscented filtering and nonlinear estimation[END_REF], [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continous-time nonlinear systems[END_REF] with invariant observers theory has been recently proposed but requiring a compatibility condition see [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF], [START_REF] Condomines | Nonlinear state estimation using an invariant unscented Kalman filter[END_REF].

Approach

State manifold System Filter [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF] Lie groups Continuous EKF [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF] SO(3) Cont-Discrete EKF [START_REF] Snoussi | Particle filtering on Riemannian manifold[END_REF] Riemannian Discrete PF [START_REF] Tompkins | Bayesian filtering on the Stiefel manifold[END_REF] Stiefel Discrete PF [START_REF] Rentmeesters | An efficient particle filtering technique on the Grassmann manifold[END_REF] Grassmann Discrete PF [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF] Matrix Lie groups Discrete EKF [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF] Matrix Lie groups Cont-Discrete EKF [START_REF] Hauberg | Unscented Kalman filter on Riemannian manifolds[END_REF] Riemannian Discrete UKF This paper Lie groups Discrete UKF This article focuses on these recent research works and proves that an Invariant UKF-like estimator (named IUKF) could be simply designed by introducing both notions of invariant state estimation and invariant output errors within any UKF algorithm formulation, without requiring any compatibility condition such as proposed in the π-IUKF [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF]. Besides, it has been shown that, for some well-known navigation problems devoted to UAVs, equations of any IUKF-based observer in discrete-time could be expressed quite simply whitout requiring any compatibility condition which is the main contribution. Similarly, an extension of nonlinear invariant observers has been made for Rao-Blackwellized Particle Filters (PF) that can be used for nonlinear state estimation [START_REF] Barrau | Intrinsic filtering on Lie groups with applications to attitude estimation[END_REF]. Invariant PFs (IPF) rely on the notion of conditional invariance which corresponds to classical system invariance properties, but once some state variables are assumed to be known. It is those known states that will be sampled throughout the estimation process. It is noteworthy that, for the obtained IPF, the Kalman gains computed are identical for all particles which drastically reduces the computational effort usually needed to implement any PF. All the previous estimation methodologies have allowed the invariant observers theory to be applied in many application fields since the beginning of the 2000s. Rather than enumerating all of them, which would be out of the scope of this technical paper, we prefer focusing here on the use, become popular in the domain of electro-mechanical systems in robotics, of the invariant observers for solving nonlinear attitude estimation problems from both inertial/vision multisensors data fusion. Many bibliographical references, such as for instance [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF], [START_REF] Li | Invariant Observer-Based State Estimation for Micro-Aerial Vehicles in GPS-Denied Indoor Environments Using an RGB-D Camera and MEMS Inertial Sensors[END_REF], [START_REF] Khosravian | Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs[END_REF], tackle this specific issue exploiting nonlinear invariant observers. Both properties and capabilities of this peculiar class of method make any invariant observer-based estimation scheme dedicated to dynamical system navigation appealing, especially when there exists, in addition, hardware redundancy. In that case, automated vehicles can reach an acceptable level of robustness w.r.t. degraded operating conditions such as, for example, in indoor or GPS-denied environments, and in case of single or multiple sensor faults. Using an invariant observer-based algorithm to merge an extended (and potentially redundant) set of measurements can still provide good performances and convergence properties in such situations.

In the sequel, §II presents the theoretical background of our proposed IUKF estimation algorithm and an illustrative example fitted out with a tilt sensor system. §III gathers some results obtained after solving the Attitude and Heading Reference System (AHRS) estimation problem in real conditions.

II. THE INVARIANT UNSCENTED KALMAN FILTER A. IUKF algorithm

Inspired by the theory of continuous-time symmetry preserving observer [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF] a novel and original UKF-based approach has been developed in [START_REF] Condomines | Optimal Invariant Observers Theory for Nonlinear State Estimation[END_REF] to adress the approximation issue of the invariant EKF without requiring any linearization of the dynamical systems equations or compatibility condition such as proposed in the π-IUKF algorithm [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF]. The idea is to exploit the UKF principles within a continuous-time invariant framework. This section presents briefly the main theoretical principles of some research works dealing with dynamical system symmetries, invariant observer and IUKF algorithm. Without considering any system description, the theory of invariant observer is formulated using both differential geometry and transformation groups theory presented in [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF].

Definition 1: Considering a continuous nonlinear Ginvariant/equivariant dynamical system modelling Σ, the general form of a nonlinear continuous-time symmetrypreserving state observer will be defined s.t.:

9 x-f px, uq `n ÿ i"1 ! s K i rEs ¨Epx, I x u , zq ) .w i pxq (1)
In (1), x refers to the estimated state vector. z is the measurements vector. All the measurements are assumed to be corrupted by noises and some of them are subject to bias-type errors. Both assumptions on noises and additive state variables will permit to account for these disturbances for invariant nonlinear state estimation. Equation (1) follows the standard expression of many nonlinear state estimators (such as Luenberger observers or Kalman filters) in which a model-based prediction, calculated here from G-invariant process equations, is corrected to produce estimation time derivative. For invariant nonlinear state estimation however, correction must be constructed s.t. Eq. ( 1) will be also Ginvariant. In other words, observer's dynamics must verify similar invariance properties w.r.t. the original system. Thus, in formulation (1), the gain matrix @i P rr 1 ; n ss, s K i rEs " s K i rEpx, I x u , zqs depends on the system's trajectory only through a known complete set of invariant Ipx, uq " ψ x´1 puq and on the invariant output error E :" ρ x´1 phpx, uqq ρx ´1 pzq. w i pxq :" " Dϕ γpxq pxq ‰ ´1 ¨B{Bx i is an invariant vector which projects the set of invariant correction terms on each component of f px, uq (i.e. the tangent state space). pB{Bx i q is the i-th canonical vector field of R n .

The convergence properties of p1q depend on the choice of s K i rEs and in the way the state estimation error is defined. Instead of considering the usual "linear" state estimation error x´x, the invariant observer theory defines an invariant state estimation error denoted ηpx, xq " x ´1 x which has invariant properties.

Definition 2: The asymptotic convergence of x to x is equivalent to the stability of the invariant state error dynamic which takes the general form:

9 η " Υpη, Ipx, uqq (2)
where Υ is a smooth function. It appears that η depends on the system's trajectory only through the invariant Ipx, uq.

For numerous applications, the invariant observer gain(s) calculation can be addressed ad hoc by first, investigating the observer detailed nonlinear equations, and then, by choosing gain value(s) which will meet some predefined requirements in terms of: -convergence (guarantee and domain); -decoupling purposes; -subsystems settling time/damping ratio; -etc. This calculation can also be carried out with more genericity by adapting well-proven optimal filtering techniques. This has led to the development of the so-called Invariant Unscented Kalman Filter (IUKF).

The IUKF relies on the basic theoretical principles developed by Julier and Uhlmann at the beginning of 2000s [START_REF] Julier | Unscented filtering and nonlinear estimation[END_REF] which have been since widely applied to various nonlinear state estimation problems [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continous-time nonlinear systems[END_REF]. The standard UKF algorithm exploits a deterministic sampling technique, known as the unscented transform, in order to pick a minimal set of sample points, also called sigma points, around the mean state vector. These latter are then propagated through the nonlinear state f and output h equations, from which both estimated mean and covariance are then recovered. The resulting filter captures the true mean and covariance with more accuracy than any other Kalman filtering techniques. In addition, this method removes the requirement to explicitly calculate the Jacobian matrices Bf {Bx and Bh{Bx w.r.t. standard Extended Kalman Filter (EKF), which can be a difficult task in itself for complex systems. Besides, to improve its computational efficiency the standard UKF algorithm can be derived in several factorized versions. In the sequel, the square-root formulation will be considered. The developed IUKF algorithm (see [START_REF] Condomines | Optimal Invariant Observers Theory for Nonlinear State Estimation[END_REF]) permits to design a nonlinear discrete-time invariant state observer by a numerical scheme using a fourth order Runge-Kutta integration. Σ d is defined as following : Proposition 1: Considering the whole state space representation of Σ d , the composite transformation φ gPG " pψ g , ϕ g , ρ g q and starting from initial values x0 " Erx 0 s, P xx 0 " Erηpx 0 , x0 qη T px 0 , x0 qs the two-steps procedure (prediction/correction) permit to design the following invariant nonlinear state observer in discrete time:

@k P N, Σ d : " x k`1 " f d px k , u k q `vk y k " h d px k , u k q `wk
@i P rr 0 ; 2n ss, X piq k`1|k " f d pX piq k|k , u k q ñ xk`1|k " 2n ÿ i"0 W piq m X piq k`1|k S xx k`1|k " $ ' ' ' ' ' & ' ' ' ' ' % qr " a W p1q c ´ηpx k`1|k , X p1q k`1|k q ¨. . . ηpx k`1|k , X p2nq k`1|k q ¯V1{2 k  cholupdate ´Sxx k`1|k , ηpx k`1|k , X p0q k`1|k q, W p0q c @i P rr 0 ; 2n ss, ŷpiq k`1|k " h d pX piq k|k , u k q ñ ŷk`1|k " 2n ÿ i"0 W piq m ŷpiq k`1|k S yy k`1|k " $ ' ' ' ' ' ' ' ' ' & ' ' ' ' ' ' ' ' ' % qr " a W p1q c ˆEpX p1q k`1|k , I X p1q k`1|k u k , ŷk`1|k q . . . EpX p2nq k`1|k , I X p2nq k`1|k u k , ŷk`1|k q ˙W1{2 k  cholupdate ´Syy k`1|k , . . . EpX p0q k`1|k , I X p0q k`1|k u k , ŷk`1|k q, W p0q c P xy k`1|k " 2n ÿ i"0 W piq c ηpX piq k`1|k , xk`1|k qE T pX piq k`1|k , I X piq k`1|k u k
, ŷk`1|k q @i P rr 1 ; n ss,

s K i rEs " i th row of K " pP xy k`1|k {pS yy k`1|k q T q{S yy k`1|k F : xk`1|k`1 " xk`1|k `n ÿ i"1 s K i rEs ¨Epx k`1|k , I xk`1|k u k , z k`1 q.w i px k`1|k q S xx k`1|k`1 " cholupdate ´Sxx k`1|k , KS yy k`1|k ,
´1P revious matricial computations rely on both QR decomposition and rank 1 update to Cholesky factorization (cholupdate). Local transformations pψ g , ϕ g , ρ g q are here defined as for system Σ d . In this formulation, state, output and crossed error covariances are now defined from system modelling invariants. It is clear by transitivity that these matricial quantities are left unchanged by the composite transformation φ gPG " pψ g , ϕ g , ρ g q.

Unlike the Invariant Extended Kalman Filter (IEKF), the proposed IUKF does not require a linearization of 9 ηpx t , xt q w.r.t η for its gain matrix computation step. When any given permanent trajectory t Þ Ñ px p ptq, u p ptqq is followed (i.e., s.t. @t, I xp up ptq " Ī), 1 st order approximation of Eq. (2) shows that if K is also determined s.t. matrix BΥp0, Īq{Bη is stable, then observer F will converge locally around px p ptq, u p ptqq. Reuse of system modelling invariances within invariant observer design also guarantees that it will converge for any group action image pψ g pu p ptqq, ϕ g px p ptqqq gPG .

This property is remarkable especially for dynamical systems described by kinematics relationships whose dynamics is invariant by translation and rotation movements inside an invariant frame. By doing this, correction step procedure relies on the determination of the n additive gain which depend on system fundamental invariants and invariant innovation terms. Moreover, the invariant correction terms are projected on each component of the dynamical equations by considering the canonical basis of R n such as Bpx k`1|k q " tω i px k`1|k qu iPrr 1 ; n ss vectors form an invariant frame for each x P X . Thus, the IUKF algorithm relies on a multiple parametrization defined by local transformation groups. Considering the transformation group φ g " pϕ g , ψ g , ρ g q each inverse of sigma point can be defined as a local parameter of p2n `1q invariant frame which project each sigma point on the neutral element e thought the local application ϕ g .

The developped IUKF is a natural approach, by combining both invariant observers theory and unscented filtering principles, to dertermine all the summetry-preserving correction terms, without requiring any linearization of the differential equations or compatibility condition such as proposed in [START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF]. It can be seen as a generic algorithm without involving any form of the observation equation or relations defining the transformation group ρ g .

B. illustrative example

In this section, we illustrate and prove that the proposed algorithm retains the invariance of the problem, and that the error's evolution is independent of the system's trajectory, inheriting the properties of the deterministic continuoustime case [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF]. Thus, we consider a tilt sensor system as a simple case study applied to an object attitude estimation where we desire to determine only the pitch angle θ. The nonlinear state estimation makes use of 3 accelerometers give a measurement of the specific acceleration denoted by a m " pa 1 , a 2 , a 3 q T . ⃗ g θ a1 a3

(K) (k) O Θ a1 a3 (K ′ ) (k) -Θ0
Fig. 1. A flying object in a vertical plane : the system remains unchanged under the action group SO [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF] All these measurements are obviously corrupted by additive noises for which it appears reasonable to assimilate their stochastic properties to the ones of Gaussian processes. Based on the application of Euler angles and direction cosine matrix transformation, the pitch angle θ can be determined from the following system of non-linear equations s.t :

ˆya1 y a3 ˙" ˆ´sinpθq cospθq ˙" hpxq (3)
If the platform is stationary (the tilt angle do not change throughout the measurement period), it is possible to assume that the pitch angle is constant. The process equation becomes :

9 θ " 0 (4)
The nonlinear state space representation can be described in a compact form such as: 9

x " f px, uq and y " hpx, uq where x " θ et y " py a1 , y a3 q T . Considering the expressions of system modelling and the Lie-group G defined s.t. G " R, the following input, state and output transformations prove that the system is both G-invariant and G-equivariant. These latter read @g 0 " θ 0 P G and @pu, x, yq P U ˆX ˆY: ψ g0 puq " 0 ϕ g0 pxq " pθ `θ0 q ρ g0 pyq " ˆya1 cos θ 0 ´ya3 sin θ 0 y a3 cos θ 0 `ya1 sin θ 0

˙(5)

Therefore, the moving frame γpxq which conveys any state vector to e is given by x ´1 " ´θ. Consequently, the analytical expression of the invariant output error Epx, I x u , zq reads in this applicative case: E " ρ x ´1 pŷ a1 , ŷa3 q ´ρx ´1 py a1 , y a3 q " ˆcos θ 0 ´sin θ 0 sin θ 0 cos θ 0 ˙ˆŷ a1 ŷa3 ˙´ˆc os θ 0 ´sin θ 0 sin θ 0 cos θ 0 ˙ˆy a1 y a3

"ˆc os θ sin θ ´sin θ cos θ˙ˆŷ a1 ´ya1 ŷa3 ´ya3 ẇe note that the invariant output error correspond to a classical output error projected in the Frenet frame. Based on these results, the observer considered in the IUKF algorithm takes the following form:

9 θ " 0 `K ˆcos θ sin θ ´sin θ cos θ˙ˆŷ a1 ´ya1 ŷa3 ´ya3 ˙(6)
where K is a smooth 1ˆ2 gain matrix whose entries depend on the invariant error E but also on the invariants. ig. 2. The location of the sigma points errors and the effect of the rotation θ for the UKF (cross) and the IUKF (circle, red) Figure 2 illustrates the previous explanations by applying the developed IUKF algorithm to simulated data corrupted by a Gaussian white noises whose noise covariance matrices are set to : Q θ " 1e ´2 and R y " 0.5rad. The sigma points output errors calculated by the IUKF converge as expected in a linear way (i.e, @θ) after a slight convergence whereas those of the UKF have an irregular evolution due to an estimation output error which is not projected into a Frenet frame which does not preserve the symmetries of the system.

III. BENCHMARK AND APPLICATION

A. Dynamic system modeling

This subsection details the generic modelling used to tackle and solve the issue of estimating some key flight variables (attitude-orientation, angle rates, etc.) of mini-UAVs fitted out with an Attitude and Heading Reference System (AHRS). UAVs dynamics representation corresponds here to a pure quaternionial kinematics modelling (whose related quaternion will be denoted by q), supplemented by additive state variables which represent low frequency sensors' imperfections (such as slowly varying biases). Thereby, we consider:

Σ : $ ' ' ' ' ' ' ' & ' ' ' ' ' ' ' % 9 x" ¨9 q " q ˚pω m ´ωb q{2 9 ω b " 0 9 a s " 0 9 b s " 0 ‹ ‹ ' , y " ˆyA " a s q ´1 ˚A ˚q y B " b s q ´1 ˚B ˚q ˙(7)
where ω m is seen as an imperfect and noisy, but known, measured input, like B. Constant A " p0 0 gq T refers to the local Earth's gravity vector. Nonlinear state space representation of Eq. ( 7) can be described in a compact form s.t. 9

x " f px, uq and y " hpx, uq where u " ω m , x " pq T ω T b a s b s q T and y " py T A y T B q T are the input, state and output vectors respectively. The nonlinear state estimation problem makes use of 3 triaxial sensors which deliver a total of 9 scalar measurement signals: « 3 magnetometers permit to obtain a local measurement of Earth's magnetic field, which is known constant and expressed in the body-fixed frame s.t. vector y B " q ´1˚B˚q (where B " pB x B y B z q T ) can be considered as an output of the observation equations; « 3 gyroscopes produce the measurements associated with the instantaneous angular rates gathered in ω m P R 3 s.t. ω m " pω mx ω my ω mz q T ; « and 3 accelerometers provide the measured output signals coresponding to the specific acceleration, denoted by a m P R 3 with a m " pa mx a my a mz q T . As no velocity and position information is available (no GPS, nor airspeed data fusion), this AHRS is often qualified as non-aided. Thus, to keep the whole nonlinear state representation observable given these available measurements, the assumption that the linear acceleration 9

V remains negligible is also made i.e., 9

V " 0. Consequently, the specific acceleration vector, expressed in the body-fixed frame, can be approximated by ´as q ´1 ˚A˚q " ´yA and compared with its corresponding imperfect and noisy measurement a m . Taking into account the maximum number of sensors' imperfections (such as low frequency disturbances) within the estimation process requires the introduction of 2 additive state variables due to a 1 st -order observability analysis (see [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF] for more calculation details). These 2 additive variables correspond to positive constant scaling factors, denoted by a s and b s adjust and correct the predicted outputs y A and y B respectively. All these sensor imperfections are modelled as pseudo-Gaussian random walks which can be physically interpreted as slowly varying parameters.

B. IUKF estimator derivation

Considering the expressions of system modeling given in Eq. ( 7) and the Lie-group G defined s.t. G " H 1 ˆR5 (where H 1 designates the differentiable manifold composed of quaternions with unit norm which is homeomorphic to R 3 ), the following input, state and output transformations prove that system modeling Σ is both G-invariant and Gequivariant (see definition in [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF]). These latter read @g 0 " pq T 0 ω T 0 a 0 b 0 q T P G and @pu, x, yq P U ˆX ˆY: ψ g0 puq " q ´1 0 ˚ωm ˚q0 `ω0 ϕ g0 pxq " ppq ˚q0 q T pq ´1 0 ˚ωb ˚q0 `ω0 q T . . . a s .a 0 b s .b 0 q T ρ g0 pyq " ppa 0 .q ´1 0 ˚yA ˚q0 q T pb 0 .q ´1 0 ˚yB ˚q0 q T q T (8) From Eq. ( 8), one can deduce easily that the composite transformation φ g " pψ g , ϕ g , ρ g q is equivalent to timeconstant rotations and translations in both Earth-and bodyfixed frames. By posing Q " q˚q 0 , Ω b " q ´1 0 ˚ωb ˚q0 `ω0 and Ω m " q ´1 0 ˚ωm ˚q0 `ω0 , it can be demonstrated that, for instance, the 1 st equation of 9

x " f px, uq is indeed Ginvariant:

2 9 Q " 2
. hkkkikkkj pq ˚q0 q" q ˚pω m ´ωb q ˚q0 " q ˚pq 0 ˚q´1 0 ˚ωm ´q0 ˚q´1 0 ˚ωb q ˚q0 " Q ˚pΩ m ´Ωb q It follows that the neutral element e of G associated with ϕ g0 is given by p1 T 0 T 1 1q T (where 1 " p1 0 0 0q T and 0 " p0 0 0q T ). Therefore, the moving frame γpx t q which conveys any state vector to e is given by x ´1 " pq ´T p´q ˚ωb q´1 q T 1{a s 1{b s q T . Consequently, the analytical expression of the invariant output error E reads in this applicative case:

Epx, I x u , zq " hpe, I x u q ´ρx ´1 pzq " ˆEA " A ´â ´1 s .q ˚am ˚q ´1 E B " B ´b ´1 s .q ˚bm ˚q ´1 İn
Eq. (III-B), b m is the magnetic field measurement. Besides, the invariant basis vectors can be also clarified. By posing Wpxq " pw q i q iPrr 1 ; 3 ss pw ωb i q iPrr 1 ; 3 ss w âs w bs ( the invariant vectors basis and considering B " pv i q iPrr 1 ; 3 ss the canonical basis of R 3 , we have:

¨vi ˚q 0 0 0 ‹ ‹ ' iPrr1;3ss ¨0 q´1 ˚vi ˚q 0 0 ‹ ‹ ' iPrr1;3ss ¨0 0 a s 0 ‹ ‹ ' ¨0 0 0 b s ‹ ‹ '
Mixing all these results allows to derive the observer considered in the IUKF algorithm s. p s K 1:3 i rEs.E A `s K 4:6 i rEs.E B q ˚η 9 α " ´αp s K 1:3 7 rEs.E A `s K 4:6 7 rEs.E B q 9 γ " ´γp s K 1:3 8 rEs.E A `s K 4:6 8 rEs.E B q As it was beforementioned, the reader can notice that the invariant state estimation error dynamics depends on system's trajectory t Þ Ñ px t , u t q through the invariant quantity I x u which is a major difference with most of nonlinear estimators. Unlike the Invariant Extended Kalman Filter (IEKF - [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF]), the proposed IUKF does not require a linearization of 9 ηpx t , xt q w.r.t. η for its gain matrix computation step. This linearization can appear as a difficult operation in itself and especially for any practical implementation.

C. Experimental results

Due to a lack of space, we briefly evaluate the IUKF performances experimentally by post-processing a set of experimental data on the basis of both the dynamical modelling of Eq. ( 7) and the filtering equations of Eq. [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF]. Figure 3 displays a picture of the Parrot quadrotor mini-UAV under test and an image of the indoor flight performed to gather these real data. It also illustrates that this experiment has been made using an OptiTrack system which permits to have at disposal absolute references (see http://www. optitrack.com/). As no specific autopilot hardware device has been designed for this experiment, it is noteworthy that data fusion will merge low quality measurement signals delivered directly by the cheap electromechanical sensors which equip any Parrot quadrotor. The interest of the following results relies less on the ability of the IUKF algorithm to estimate systems' states and outputs than on the practical verification of the theoretical properties asserted by the invariant observers framework when dealing with real data. To point out these latter, the results obtained with the IUKF algorithm have been systematically compared with the ones provided by a standard UKF approach. To lead a fair comparison, both techniques share identical setting parameters values i.e., similar estimated process and measurement covariances values for matrices V and W. Figure 4 shows the estimation results of the quaternion state components obtained by both UKF and IUKF algorithms. It is noticeable that both methods provide correct estimates w.r.t. the absolute references plotted in solid red lines. The differences between the two algorithms appear when we consider the dispersion around the estimated state trajectory. Indeed, the black dashed lines plotted on each subfigure, which correspond to the qptq ˘3 ˆσ q ptq standard deviations around the mean estimated value, tend to prove that the IUKF estimation algorithm calculates more trustful quaternion estimates, or at least reduces the dispersion of these state estimates, due to the invariant framework used.

Based on these quaternion estimates, the instantaneous Euler attitude angles values, which describe at any time instant the orientation of the flying Parrot, have been deduced through time and compared with the absolute references determined by the OptiTrack system (Fig. 5). It appears that both algorithms allow to reconstruct a suitable attitude estimation for control purposes. The 3-axis pφ, θ, ψq estimation state errors w.r.t. the absolute references are also drawn using a logarithmic scale and show comparable results for both techniques.

Expected differences brought by the invariant observer theory used to design our IUKF algorithm can be observed on Figure 5, which display, through time, both computed theoretical standard deviations and filters correction gains. By merging these results, it can be concluded that our proposed IUKF estimation technique is characterized by quasi-constant estimated standard deviations and correction gains w.r.t. any standard UKF estimation algorithm. Exploiting system's dynamics invariances in order to design nonlinear state estimation approaches allows to construct powerful nonlinear observers whose properties will be quasiindependent from the current followed trajectory. Therefore, state estimation uncertainties can be quantified by quasiconstant values through time (see for instance standard deviations on q). This paves the way for designing less conservative, but robust, estimated state-feedback control laws in order to improve mini-UAVs flying and handling qualities. Similarly to the theoretical standard deviations computed by the IUKF observer, the correction gains (cf. Fig. 5) appear less sensitive to the non-stationary noises levels, so that we can conclude that the invariant framework offers a better high frequency perturbations rejection in terms of filtering capabilities. In the case of the IUKF estimator, these gains could be also approximated by constant values, after a given transient regime, rather than in the case of the standard UKF algorithm.

IV. CONCLUSION AND FUTURE PROSPECTS

This article has presented an innovative procedure to derive an invariant observer for nonlinear state estimation. This latter, named IUKF, combines both invariant observers theory and unscented filtering principles. Its methodological foundation, which forms the main contribution of this paper, consists in adapting the computational steps of any UKF-like technique (standard or square-root version) to calculate the estimation correction terms. This adaptation relies firstly on the introduction of an invariant innovation vector in the observer filtering equation. Then, an invariant state estimation error is also defined and used jointly to update through time all covariance matrices. It is noteworthy that, by construction, these covariances are left unchanged by dynamical systems' symmetries (i.e., all combinations of translation and rotation motions). This confers to K some properties of invariance which leads, by transitivity, to design an IUKF symmetrypreserving state observer. In comparison with the stateof-the-art, our proposed IUKF nonlinear state estimation algorithm presents one main advantage when considering computational aspects. Indeed, it does not require any differential equations linearization unlike IEKF or compatibility condition such as proposed in the π-IUKF. The experimental results presented in §III-C have shown an equivalent capability of our proposed IUKF technique in comparison with an UKF method for nonlinear state estimation. These results have also permitted to check in realistic conditions some invariance properties which characterize our designed observer. Among these latter, stability of estimated standard deviations, which characterize estimated state trajectory uncertainties, must be highlighted since it could facilitate new control strategies design with less conservatism. Future works will be on the theoretical development and mathematical justification of our proposed filter. We will also investigate the possibility to use constant gain matrices, optimized offline by the IUKF, into a complementary observer. The benefit of this solution would be to take advantage of the computational simplicity of the complementary observer but with optimal correction terms provided by the IUKF.

Fig. 3 .

 3 Fig. 3. Experimental materials: Parrot quadrotor mini-UAV and OptiTrack device.

Fig. 4 .

 4 Fig. 4. Estimated standard deviations on quaternion and Euler angles associated errors: comparison UKF/IUKF.

Fig. 5 .

 5 Fig. 5. Correction gains and estimated theoretical standard deviations: comparison UKF/IUKF.

TABLE I CATEGORIZATION

 I OF THE STATE OF THE ART APPROACHES ON KALMAN AND PARTICLE FILTERING FOR A STATE EVOLVING ON A MANIFOLD.

  t.: η " p

	3
	ÿ i"1 p s K 1:3 i rEs.E A `s K 4:6 i rEs.E B qv i q ˚η	´1 2	η	˚β
	9 β " pη ´1 ˚Ix u ˚ηq ˆβ `. . .
	η ´1	˚3 ÿ
		i"1

q " q ˚pω m ´ω b q 2 `. . .

`s K 1:3 i rEs.E A `s K 4:6 i rEs.E B ˘vi ˚q `Cq

`s K Thereby, we consider the invariant state estimation error s.t.:

In the previous equation, the notation s K j:k i rEs (with i P rr 1 ; n ss and pj, kq P pN ‹ q 2 ) designates the gain submatrix obtained by concatenating the columns of s K i rEs between the j th and the k th positions. The additive (and invariant) vector C q, which reads p1 ´}q} 2 qq, permits to keep }q} " 1 through time along the estimation process. By denoting ηpx, xq " pα β µ νq T " ppq ˚q ´1q T pq ˚p ωb ´ωb q q´1 q T a s {â s b s { bs q T , the invariant state estimation error dynamics is given by: