AUTONOMOUS NAVIGATION OF SMALL UAVS BASED ON VEHICLE DYNAMIC MODEL

This paper presents a novel approach to autonomous navigation for small UAVs, in which the vehicle dynamic model (VDM) serves as the main process model within the navigation filter. The proposed method significantly increases the accuracy and reliability of autonomous navigation, especially for small UAVs with low-cost IMUs on-board. This is achieved with no extra sensor added to the conventional INS/GNSS setup. This improvement is of special interest in case of GNSS outages, where inertial coasting drifts very quickly. In the proposed architecture, the solution to VDM equations provides the estimate of position, velocity, and attitude, which is updated within the navigation filter based on available observations, such as IMU data or GNSS measurements. The VDM is also fed with the control input to the UAV, which is available within the control/autopilot system. The filter is capable of estimating wind velocity and dynamic model parameters, in addition to navigation states and IMU sensor errors. Monte Carlo simulations reveal major improvements in navigation accuracy compared to conventional INS/GNSS navigation system during the autonomous phase, when satellite signals are not available due to physical obstruction or electromagnetic interference for example. In case of GNSS outages of a few minutes, position and attitude accuracy experiences improvements of orders of magnitude compared to inertial coasting. It means that during such scenario, the position-velocity-attitude (PVA) determination is sufficiently accurate to navigate the UAV to a home position without any signal that depends on vehicle environment.


INTRODUCTION
This paper is a shortened version of (Khaghani and Skaloud, 2016).For more details and complementary results and discussions, readers are encouraged to refer to (Khaghani and Skaloud, 2016) here and on several occasions throughout the text.

Motivation
The dominant navigation system for small UAVs today is based on INS/GNSS integration (Bryson and Sukkarieh, 2015).GNSS provides absolute time-position-velocity TPV data for the platform, at relatively low frequency of only a few Hz, while INS provides relative position and attitude data at much higher frequencies than GNSS, typically at few hundreds of Hz.The integration of these data types can provide solutions with enough sort-term and long-term accuracy.However, the problem arises when GNSS outage happens (Lau et al., 2013), which is not a rare situation and can happen due to intentional corruption of GNSS signals (jamming and spoofing), or loss of line of sight to the satellites or interference in satellite signal reception (Groves, 2008).In such cases, the navigation solution is based on standalone INS with possible aiding from navigation aids such as barometric altimeters.The accuracy of the data provided by INS is directly determined by the quality of the IMU that is used in the system.The long-term accuracy of 3D inertial coasting based on small and low-cost IMUs available for small UAVs is so low that after only a minute of GNSS outage, the position uncertainty is too far from being of practical use.In other words, unless this drift is controlled by other means, the UAV is completely lost in space or may even become dynamically unstable.This may cause serious problems especially in non-line-of-sight flights and can lead to loss of the UAV with possible damages to objects or people on ground.* Corresponding author

Available solutions
Many researches have been conducted to address the problem of rapid drift of navigation solution during GNSS outage conditions of minutes.Some have tried to improve INS error modeling using advanced techniques (Noureldin et al., 2009), and some have chosen to employ additional sensors to aid the system (Yun et al., 2013).The first approach does not still provide sufficiently good improvements for aerial vehicles.The solutions related to the second approach add cost and complexity to the system, and more importantly, their performance depends on environmental conditions that are not met all the times, which challenge the autonomy of the system.A widely used (yet partial) solution of the second category is employing vision based methods that provide relative or absolute measurements to inertial navigation (Wang et al., 2008) (Angelino et al., 2012).Apart from adding extra weight and hardware and software complications, their correct function requires some prerequisites on light, visibility, and terrain texture.For example, they might not work at night or in foggy conditions or over ground with uniform texture (vegetation, water, snow, etc.).Therefore, it is a challenge to find a solution that mitigates the quick drift of low-cost inertial navigation during GNSS outage in airborne environment while preserving the autonomy of the system, and avoiding extra cost and additional sensors.Finding a suitable solution can be extremely beneficial for increasing the reliability of autonomous navigation of small UAVs.
There have been some previous research activities related to VDM integration to improve the navigation accuracy, especially in GNSS outage conditions.However, most of the proposed solutions employ the kinematic modeling (INS) as the main process model within the navigation filter (Bryson and Sukkarieh, 2004) (Vasconcelos et al., 2010) (Dadkhah et al., 2008), while using the VDM output either in the prediction phase or in the update phase within this filter.Such approach relies totally on IMU and therefore is not robust if IMU failure occurs.Some authors have considered both INS and VDM at the same level within the filter (Koifman and Bar-Itzhack, 1999), but still the navigation solution at the end is based on filtered INS output.Therefore, IMU failure disables navigation in this case, as well.In many cases the presence of wind is not considered (Bryson and Sukkarieh, 2004) (Vasconcelos et al., 2010) (Dadkhah et al., 2008) (Crocoll et al., 2014) (Sendobry, 2014), or the capability of correcting the parametric errors in dynamic model on-flight is not provided (Bryson and Sukkarieh, 2004) (Vasconcelos et al., 2010) (Dadkhah et al., 2008), or the VDM is included only partially within the filter (Crocoll et al., 2014) (Crocoll and Trommer, 2014) (Müller et al., 2015).Some researchers also consider IMUs of higher accuracy (Koifman and Bar-Itzhack, 1999), which is impractical for small UAVs in terms of size/weight and cost.

Proposed approach
In this research, a solution is proposed that integrates VDM with inertial navigation for its autonomous part, and GNSS or other aiding when available.It improves the accuracy of navigation and significantly mitigates the drift of navigation uncertainty during GNSS signal reception absence.The main idea of this concept is to benefit from the available information of vehicle dynamic modeling and control input within the navigation system that implicitly rejects the physically impossible movements suggested by the IMU.Significant improvements in navigation solution accuracy in case of GNSS outages are reported via simulation studies.The proposed solution requires no additional sensors, so it preserves the autonomy of the navigation system when GNSS outages happen.Adding no extra sensors also means no additional cost and weight on the platform, which is an important aspect in small UAVs.
A key feature in the proposed solution is VDM acting as the main process model within the navigation filter, where its output is updated with raw IMU observations and if available, GNSS measurements.Such architecture avoids the complications of multiprocess model filters (Bryson and Sukkarieh, 2004) (Koifman and Bar-Itzhack, 1999) and thus leads to simpler filter implementation, smaller state vector, and less computation cost.It is also preferred over the architectures in which INS is the main process model that gets updated by VDM, due to the following reasons.In case of IMU failure, the proposed architecture can simply stop using IMU observations, while the architecture with INS as the main process model will fail.On the other hand, the high frequency measurement noise in IMU data causes divergence in navigation solution when integrated within the navigation filter, as analytically shown in (Schwarz and Wei, 1994).The mechanical vibrations on the platform also affect the IMU measurements, but not the VDM output.Therefore, treating the IMU data as observations and avoiding integration of them as a process model is expected to improve the error growth.
The VDM needs to be fed with the control input to the UAV.This information is already available in the control/autopilot system, but it needs to be put in correct time relations with IMU and other measurements.The other input to VDM is the wind velocity.The proposed solution makes it possible to estimate the wind velocity within the navigation filter itself, even in absence of air pressure sensors.This adds certain redundancy to the system in case of air pressure sensor misbehavior when employed.
The VDM requires a proper structure based on the host platform type (fixed wing, copter, etc.) and its control actuators, which is well described in the literature (Cook, 2013) (Ducard, 2009).Some parameters in the model depend on the specific platform.They can be either identified and pre-calibrated, or estimated inflight.This capability for online parameter estimation (dynamic model identification) that does not require pre-calibration, minimizes the effort required in design and operation.
Proof of the proposed concept is performed via Monte Carlo simulation study in several different situations.To make the simulations realistic, errors are introduced to all the a-priori information available to the navigation system, such as initial values of states, dynamic model parameters, and error statistics of IMU and GNSS measurements.Also, real 3D wind velocity data (KNMI and Alterra, 2012) is used in simulations.Results of Monte Carlo simulations on a sample trajectory are presented and analyzed in the paper.More detailed results, along with observability and accuracy prediction discussions can be found in (Khaghani and Skaloud, 2016).

DEFINITIONS
Three coordinate frames are considered in this research, "navigation", "body", and "wind" frames.The navigation frame is a local frame oriented in north, east, and down directions denoted by (xn, yn, zn) or (xN , xE, xD), and considered to be inertial.Definitions of body frame and wind frame (x b , y b , z b ) can be perceived from Figure 1.The rotation matrix to transform vectors from body frame to navigation frame is defined as The wind frame has its first axis in direction of airspeed V , and is defined by two angles with respect to body frame, angle of attack α and sideslip angle β.Velocity of airflow that is due to UAV's inertial velocity v and wind velocity w is denoted by airspeed vector V as v = V + w. (2) The rotation matrix from body frame to wind frame is defined as a function of Euler angles.
The density of the air is calculated based on the International Standard Atmosphere model as a function of local pressure and temperature, which can be expressed as functions of the altitude as detailed in (Khaghani and Skaloud, 2016).

VEHICLE DYNAMIC MODEL
The VDM employed in this research is based on rigid body dynamics for a fixed wing UAV and follows from (Ducard, 2009).It considers polynomial models for aerodynamic forces and moments.A brief description of the model is presented here, along with the key definitions and equations.More details can be found in (Ducard, 2009) and (Khaghani and Skaloud, 2016) The four aerodynamic forces and the three aerodynamic moments are expressed as polynomial functions of navigation states, control inputs, wind velocity, and physical properties of the UAV called dynamic model parameters hereafter.The aerodynamic forces include: • "thrust force" as FT = f (ρ, V, D, n, CF T ...) • "drag force" as F w x = f (ρ, V, S, CF x ..., α, β) • "lateral force" as F w y = f (ρ, V, S, CF y ..., β) • "lift force" as F w z = f (ρ, V, S, CF z ..., α) The aerodynamic moments include: • "roll moment" as M b x = f (ρ, V, S, b, CM x ..., δa, β, ωx, ωz) • "pitch moment" as M b y = f (ρ, V, S, c, CM y ..., δe, α, ωy • "yaw moment" as M b z = f (ρ, V, S, b, CM z ..., δr, ωz, β) Propeller diameter is denoted by D, and S, b, and c represent wing surface, wing span, and mean aerodynamic chord, respectively.Density of air is shown by ρ, and C...'s represent aerodynamic coefficients for associated force and moment components.The vehicle dynamic model parameters are collected in (9).

FILTERING METHODOLOGY
An extended Kalman filter (Gelb, 1974) is chosen to serve as the navigation filter in this research, which is detailed in this section.

Scheme
The proposed navigation system utilizes VDM as main process model within a differential navigation filter.As depicted in Figure 2, VDM provides the navigation solution, which is updated by the navigation filter based on available measurements.Hence, IMU output is treated as a measurement within the navigation filter, just the way GNSS observations are, whenever they are available.Any other available sensory data, such as altimeter or magnetometer output, can also be integrated within the navigation filter as additional observations.VDM is fed with the control input of the UAV, which is commanded by the autopilot and therefore available.Other needed input is the wind velocity as an input, which can be estimated either by the aid of airspeed sensors, or within the navigation system with no additional sensors needed.The latter approach is implemented here.Finally, the parameters of VDM are required within the navigation filter.Pre-calibration of these parameters as fixed values is an option.However, to increase the flexibility, as well as the accuracy of the proposed approach while minimizing the design effort, an online parameter estimation/refinement is implemented.Last but not least, IMU errors are also modeled and estimated within the navigation system as additional filter states.

State space augmentation
The augmented state vector includes the navigation states Xn, the UAV dynamic model parameters Xp excluding mass (m) and moments of inertia (Ix, Iy, Iz, Ixz), the IMU error terms Xe, and the wind velocity components Xw.
The dynamic model parameters are included in a 26 × 1 state vector as in ( 9), and modeled as constant parameters with initial uncertainties.Description of these parameters is provided in the nomenclature, and the numerical values used in simulation can be found in (Ducard, 2009).
Mass and moments of inertia are not included in this vector, since they appear as scaling factors in equations of motion and therefore they are completely correlated with the already included coefficients of aerodynamic forces and moments.
The error in each accelerometer and gyroscope inside the IMU is modeled as a random walk (b ... rw ) process.Therefore, the IMU error terms vector is defined as where ai and gi superscripts denote the i-th accelerometer and gyroscope, respectively.This model has been found sufficient for the low-cost IMU in consideration, but can be extended as needed.
The wind velocity is stated as a vector in local (navigation) frame consisting of the three components of wind velocity in north, east, and down directions.
Wind velocity is also modeled as a random walk process.

Errors and uncertainties
For the purpose of simulation, a MEMS-grade IMU is considered.Random biases with standard deviations of 8 mg for accelerometers and 720 • /hr for gyroscopes are considered, accompanied by white noise and first order Gauss-Markov processes.GNSS error is modelled as independent white noise signals for each channel (north, east, down), with standard deviations 1 m.The sampling frequency is 100 Hz for IMU and 1 Hz for GNSS measurements.
In terms of initialization errors, random errors are considered for different runs of the Monte-Carlo simulations with standard deviations of 1 m for each position component, 1 m/s for each velocity component, 3 • for roll and pitch, 5 • for yaw, 1 • /s for rotation rates, and 15 rad/s for propeller speed.The errors considered for the UAV dynamic model parameters (Xp) are randomly distributed with a standard deviation of 10%.
More details on the process model noise, observation noise, and initial uncertainties can be found in (Khaghani and Skaloud, 2016)

MONTE CARLO SIMULATIONS
To evaluate the performance of the navigation system, a Monte Carlo simulation has been performed with 100 runs, using real 3D wind velocity data (KNMI and Alterra, 2012).While the trajectory and the wind has been kept the same in each realization, the error in observations, initialization, and VDM parameters has changed randomly for each individual run.Figure 3 depicts the reference trajectory, as well as the solution from a sample run.
For more comprehensible presentation, the results are presented in ENU frame, instead of navigation NED frame.The trajectory has a footprint bigger than 2 km × 2 km on the ground and 1km change in altitude.The results of Monte Carlo simulations are presented in sections 5.1 and 5.2.

Navigation States
Figure 4 shows comparison of RMS of position and yaw errors for all the 100 runs between proposed VDM/INS/GNSS approach and classical INS/GNSS navigation algorithm over the whole interval.While the RMS of position error is 11.7km for classical INS coasting after 5 minutes of autonomous navigation during GNSS outage, this is reduced to less than 110m with the proposed VDM/INS/GNSS integration under exactly the same situations, which means an improvement of two orders of magnitude in position accuracy.The attitude determination also shows an improvement of 1 to 2 orders magnitude, which will be detailed shortly.It is worth mentioning here that the improved performance of the proposed filter is noticeable also during the availability of GNSS in estimating yaw.The position error for all the 100 Monte Carlo runs is presented in Figure 5.The graphs show how the error grows as time passes after GNSS outage starts, and how the overall behavior is similar for individual runs.An empirical RMS is calculated from these individual errors and plotted against the predicted confidence level (1σ).It is important to notice that how closely (and slightly conservatively) the error is predicted within the filter, which reveals the correctness of filter setup.

Auxiliary States
Successful estimation of auxiliary states is a key enabler of navigation improvement within the filter.The results are briefly presented in this section, with all the reported values calculated as an RMS of the values for all the 100 Monte-Carlo runs.More detailed results on auxiliary states estimation and associated plots are presented in (Khaghani and Skaloud, 2016).
The time correlated part of the IMU error gets estimated quickly during the first tens of seconds of navigation and remains rather unchanged afterwards, even during the GNSS outage period.The estimation error has an average of 6.0% for the three accelerometers and an average of 14.8% for the three gyroscopes at the end of the whole navigation period.
The mean error in estimation of VDM parameters shows a sharp decrease from the initial value of 10% to 6% during the first 40 seconds with GNSS available, which is followed by a slowly decreasing trend until the end.The reason behind the second regime is the correlation between some parameters within the set.In such situations, the groups of parameters are estimated rather than individual parameters, and those individual errors contribute to increasing the mean error for the whole set.
Finally, the wind velocity is estimated very well during GNSS availability period, reaching an error of only 3.9% for wind speed after 100 seconds.The estimation error starts to grow when GNSS outage begins.However, the rate of this growth is well controlled, and the error is still below 9.6% after 5 minutes of GNSS outage.

CONCLUSION
In this work, a novel method was presented to perform autonomous navigation and sensor integration for unmanned aerial vehicles.
The key concept of this method is employing vehicle dynamic model in navigation system.Unlike the traditional method of kinematic modeling in which IMU serves as navigation process model within navigation filter, here the navigation filter features dynamic model of UAV as navigation process model whose output gets updated using IMU measurements.GNSS measurements can also be used within the filter whenever they are available, as well as other measurements such as those from a barometric altimeter.
In addition to navigation states and error terms related to inertial sensors, UAV dynamic model parameters and wind velocity components can also be estimated within the filter.This is all possible with no extra sensors.The designed filter is thus polyvalent as it can accommodate changes in the platform and/or environmental conditions, such as the wind acting on the platform.
The scenario of GNSS signal reception disruption (e.g., due to electromagnetic interference) is a situation where this method can be most useful.In case of GNSS outage of 5 minutes, the presented Monte Carlo simulations show an improvement of more than 2 orders of magnitudes in position accuracy compared to inertial coasting, for random initial errors of 10% in UAV dynamic model parameters.Such gain of navigation autonomy is internal to UAV, therefore very suitable for limited or zero-visibility operations.Attitude estimation also shows a major improvement, reducing the error on yaw from 16.61 • to only 0.38 • for example.The parameters of the dynamic model are calibrated in flight and such calibration is possible even if GNSS signal reception is not available.The time-correlated errors in accelerometers and gyroscopes are also estimated within the filter as a random walk process, so are the wind velocity components.
Further development of current work will include studies on proposed navigation system in real scenarios.Technical and perhaps scientific challenges can be expected in real implementation.
Proper time stamping of all sensor observations and scaling the control input signals are examples of technical challenges.On the scientific part, the main challenges will probably be related to unmodeled dynamics and disturbances, and the inclusion of additional effects, such as sensor misalignments, actuator dynamics, UAV body elasticity, and asymmetric mass distribution.

Figure 1 :
Figure 1: Navigation, body, and wind frames with airspeed (V ), wind velocity(w), and UAV velocity (v), as weel as roll (φ), pitch (θ), and yaw (ψ) . The states vector Xn = xN , xE, xD, v b x , v b y , v b z , φ, θ, ψ, ωx, ωy, ωz, n T , control input vector U = [nc, δa, δe, δr] T , and wind velocity vector w = [wN , wE, wD] T are related via the dynamic model of the form Ẋn = f (Xn, U , w).Components of UAV velocity vector v are represented by v b x ,v b y , and v b z , while ω b x ,ω b y , and ω b z denote the rate of change for roll, pitch, and yaw, respectively.Deflections of aileron, elevator, and rudder are denoted by δa, δe, and δr, respectively.Propeller speed is denoted by n, where nc shows the commanded value for that, and τn is the time constant for its dynamics.Kinematic equations, Newton's equations of motion, and a first order model for propeller dynamics form the vehicle dynamic model as follows.
Figure 2: Navigation system architecture

Figure 3 :
Figure 3: Reference trajectory and the solution from a sample rum with GNSS signals available during first 100s only Position Error (left y-axis) VDM/INS/GNSS Position Error INS/GNSS Yaw Error (right y-axis) VDM/INS/GNSS Yaw

Figure 4 :
Figure 4: Comparison between INS/GNSS and VDM/INS/GNSS: RMS of position and yaw errors from 100 Monte Carlo runs

Figure 5 :
Figure 5: Position errors for all the 100 Monte Carlo runs Figure 6 depicts the empirical RMS of attitude errors for all the 100 runs, with associated predicted values of confidence (1σ).The results are very satisfactory in terms of preserved navigation accuracy, with the RMS of error to be only 0.0072 • for roll, 0.020 • for pitch, and 0.38 • for yaw after 5 minutes of autonomous navigation during GNSS outage.In comparison, the classical INS coasting would result in errors of 2.6 • for roll, 1.5 • for pitch, and 16.6 • for yaw under exactly the same situations.

Figure 6 :
Figure 6: RMS of position and attitude errors from 100 Monte Carlo runs ).The results are very satisfactory in terms of preserved navigation accuracy, with the RMS of error to be only 0.0072 • for roll, 0.020 • for pitch, and 0.38 • for yaw after 5 minutes of autonomous navigation during GNSS outage.In comparison, the classical INS coasting would result in errors of 2.6 • for roll, 1.5 • for pitch, and 16.6 • for yaw under exactly the same situations.