A Methodology for Design and Analysis of Sensor Fusion with Real Data in UAV platforms

This paper presents a methodology to design sensor fusion parameters using real performance indicators of navigation in UAVs based on PixHawk flight controller and peripherals. This methodology and the selected performance indicators allows to find the best parameters for the fusion system of a determined configuration of sensors and a predefined real mission. The selected real platform is described with stress on available sensors and data processing software, and the experimental methodology is proposed to characterize sensor data fusion output and determine the best choice of parameters using quality measurements of tracking output with performance metrics not requiring ground truth. Keywords— UAVs sensor fusion, EKF, Real Data Analysis, System Design


I. INTRODUCTION
Unmanned aerial systems (UAS) have recently evolved with exponential growths thanks to development of technologies of navigation, sensing or obstacle avoidance based on available micro-electro-mechanical systems (MEMS).The research has focused in getting higher degrees of autonomy thanks to enhanced navigation, but also in robust methods to protect missions against intended attacks, mainly interference (jamming) and supplant (spoofing) of GNSS signal.
The main problem in navigation focuses on improving GPS with the ability to provide accurate navigation output when GPS data becomes unavailable due to unexpected outages or intentional problems (jamming or spoofing) in certain environments.Therefore, an approach based on the fusion of complementary sensors is essential, resorting to the fundamental equations of navigation and the characterization of the errors committed by each data source.This area has become popular due to the ubiquity of GPS and the availability of inertial sensors based on inexpensive MEMS components [1], [2], [3].The integration of these complementary technologies allows compact and robust navigation solutions to determine attitude and location, so that the vehicle can determine its state in a robust way and use appropriate control techniques for autonomy.Other more drastic options for non-dependence on the GPS signal involve the deployment of autonomous localization systems such as the recognition of the environment by artificial vision [4] or location by means of electromagnetic beacons [5], with the associated cost of developing a complementary infrastructure.
Complementary to navigation technologies, the use of lasers in combination with other range detection sensors (sonar, radar, video), allows to extend the navigation conditions and obstacle avoidance.In the air vehicles (UAVs), the integration requirements (consume, weight, dimensions) are much more restrictive but, even so, it is a line in continuous development [6], [7], [8].Regarding protection against spoofing, there is also a noticeable research activity [9].Protections include diverse strategies ranging from simple actions such as monitor the communication channel, to cryptographic authentication, discrimination based on the level of signal, time of arrival, multi-antenna systems, wave polarization, Doppler shift and arrival angle, etc., as described in [10], [11].

A Methodology for Design and Analysis of Sensor Fusion with Real Data in UAV platforms
Therefore, research of robust and general techniques to integrate complementary data sources has become essential for this type of systems.In addition to theoretical developments, it is of vital importance the availability of equipment and experimental environments to validate the robustness of the solutions working in real-world conditions.The integration of sensors has to be based on the definition of parameters of the tracking system that should be adjusted to improve performance in a predefined set of missions with a defined set of sensors.The methodology proposed in this paper assumes that real system adjustments will be based on a real platform with predefined flight missions so that, in this context, the best parameters could be obtained analyzing the real operation of sensors and real output.Simulation of UAV environments are not enough to evaluate in a thorough way these systems in real conditions of real missions.This paper presents the selected environment for real experimentation, the proposed methodology and a selection of available data sources and effects of data processing techniques on the quality of the navigation solution.
The main contribution of this paper is the briefing of a methodology used for adapting filter parameters to real conditions, and further systematic analysis of available real data.Section II introduces the selected working platform, detailing the architecture of its software and the vehicles we have made to test its capacities and collect data.Section III presents the proposed methodology and evaluation metrics, section IV explains experimental environment and analyses the recorded sensor data, and section V contains the characterization and analysis of the PixHawk Px4 system filter and fusion algorithms following the presented methodology.Finally, section VI contains summarizes the conclusions derived from this work.

A. Architecture
Unmanned vehicles must be able to control their attitude and position by means of automatic control algorithms, they are controlled by a computer that integrates data from some electro-mechanical sensors and any local or global positioning system, and applies any output control system to change its location using any locomotion system.This controller is usually an embedded microcontroller that performs the core of all vehicle components.This research is based in the study of the PixHawk flight controller performance.An open-hardware computer designed by 3D Robotics specifically to create autopilot vehicles, that arises from the combination of PX4FMU and PX4IO boards.Both cards, from their version v2, are integrated in the same PCB (Printed Circuit Board) giving origin to PixHawk.

B. Sensors and data sources
The PixHawk board has several sensors integrated, shown in Table I, which serve as data sources to the PX4 stack and include some processing functions 1 .

C. Connectivity and external sensors
Besides, PixHawk counts with high connectivity for external devices and peripherals that may increase the vehicle capabilities.At the right side, there are some specific connectors for certain peripherals, such as: 2 connectors for telemetry communication, 1 for GPS, and 1 Spektrum receiver socket.At the left side, there are some generalpurpose ports and buses like 2 UART ports, 2 SPI, 1 I2C connector, 1 USB connector, 1 CAN bus connector and 3.3V and 6.6V ADC connectors.The standard configuration of a PixHawk UAV counts with interfaces to external sensors specified in the next table : These sensors allow enhancing navigation capabilities and increase the accuracy of the stabilization system measurements, what is quite important when we want to create an unmanned vehicle, because allows a more faithful image of the flying environment.The following diagram shows the main configuration of sensors used in our researching vehicles:   PX4 is the control software of PixHawk processor.It is a real-time operating system based on NuttX 2 and consists of two main layers: PX4 Flight Stack and PX4 Middleware.PX4 Flight Stack is the complete collection of applications embedded in PixHawk hardware for drone control, while PX4 Middleware is the interface that allows the flow of data from sensors to applications through a publish/subscribe system called uORB.uORB allows to publish the data coming from the sensors and make them available to the applications of the Flight Stack, obtaining a reactive system and totally parallelized.The outstanding modules are flight controller and sensor data processing [12].
Regarding the data processing, Px4 implements an AHRS (attitude and heading reference system) that implements different algorithms to estimate the vehicle attitude and creates a direction vector that allows the unmanned displacement.In this section we will overview some basic algorithms that run into the system during the flights.

a) Direction Cosine Matrix (DCM)
This program allows the analysis of the triaxial accelerometers and gyroscopes data to obtain a Direction Cosine Matrix [13].It makes possible the conversion of real-time measurements into instantaneous orientation parameters of the vehicle to deliver roll, pitch and yaw angles or variations:

Inertial Navigation System (INS)
This algorithm calculates the trajectories and corrections that allows the vehicle to move between single points using the DCM data.It is used to estimate the vehicle attitude with high frequency, so it is especially useful to complement the global position obtained from the GPS data.

c) Extended Kalman Filter (EKF)
All measurements are affected by noise that should be taken into account in the estimation of attitude and cinematic parameters.The Px4 system counts with several Extended Kalman Filter algorithms to process all sensor data in a compensation function that depends of the specific noise and accuracy characterization of each sensor, throwing high accuracy estimations of the vehicle attitude.The Px4 application counts with the possibility of applying different EKF solutions running in parallel, using different sensor measurements and states.With this implementation, it is possible to increase the accuracy and consistence of estimates even if the vehicle losses the GPS signal in certain time intervals.Next table shows the three different available EKF modes:

III. PROPOSED METHODOLOGY
In many real problems, simulated environments are used to define UAV sensors and the system parameters to optimize the system performance [14], [15].Some problems appear with this kind of methodologies, basically how to represent in simulation all effects appearing in real conditions and the way to evaluate the parameters configuration.UAV simulation have been applied to design the control subsystem for predefined missions, but simulation of real sensors is a major problem in this kind of approach.Real UAV conditions are not easy to model in simulators.UAV are affected for atmospheric conditions and random movements of UAV platforms, so accurate simulation of input data is extremely complicated for designing system parameters.Besides, in simulation, evaluation of system is based on ground truth, comparing simulated output with desired real output, and in real conditions the ground truth is not available.On the other hand, there are analysis done with real data [16], but a systematic analysis to compare alternative configurations is of high value to take design decisions.
In this scenario, the proposed methodology tries to avoid simulated evaluation and test directly the system parameters under real conditions.The first step of this methodology is the definition of the UAV platform, the type, cinematic characteristics, set of sensors and the tracking algorithms.Once the UAV platform is defined, the methodology is composed by the following steps, as displayed in Fig. 3: 1.-Mission definition.Parameters are selected from a set of possible values for a specific mission, defined by means of several waypoints.These waypoints are used to repeat the same mission every time that a new evaluation is done.2.-Each time a flight of the UAV passing through the predefined waypoints (mission) is carried out, the values of the sensor data are stored (position and velocity taken from GPS, inertial data, magnetometers, etc.) 3.-A set of flights, with the same waypoints defining the mission, are carried out and the corresponding sets of sensors data are stored together with system output, using several configurations of parameters for filtering.These values are postprocessed offline.
4.-The best configuration of parameters is selected, analyzing the performance metrics for the set of missions carried out over the same waypoints.This point is crucial because evaluation needs to evaluate the sensor fusion performance based on real data not in ground truth.
5.-The selected parameters are introduced in UAV system to perform real mission In order to assess the sensor fusion performance, with no available ground truth, the following indicators have been computed:

Averaged innovation
The innovation, or residual, is computed in the tracking filter each time an update is done for the prediction.For instance, for horizontal XY position, denoting with sub-index p the predicted track and sub-index m for measurement, and considering the average along a time window with N T measurements, it is defined as:

(
) ( ) The normalized innovation also employs the covariance matrices considering both the predicted and observation uncertainties, matrix S. The averaged value of normalized innovations defined as:

Design test
Execute flight

Save real time data
Extract log

Analyze
This value is a-dimensional, and represents the discrepancy between observations and predictions, averaged along the measurements contained in NT.Sometimes, if full covariance matrices are not available, a simplification is done and only the variances (diagonal terms) are considered:

Averaged innovation Fusion break rate
The rate of fusion break, tFB, is the number of times some navigation source is declared as inconsistent in the integrity analysis and therefore de-fused (the less consistent component is removed from the system track).This may lead to tracker re-initialization or keep the system track with a component less, the faulty sensor.It is computed as: The value, averaged along N T measurements, is obtained counting the total number of fusion break events during the interval.The test to decide the removal of a data source is done using the innovation, this time normalized by a higher number of standard deviations, typically 5.

A. Data acquisition
The process of data acquisition was based on several flight test missions that have taken place on circuits like the one sketched in Fig. 4: For each specified mission, we applied different configuration parameters in order to analyze the performance differences between each setting up of tracking filters.The flight controller log data was saved together with the configuration settings to be analyzed.For instance, table IV, shows different tests carried out on the previous circuit and the analyzed elements of the navigation system (sensors and controllers)3

B. Data characterization
Before exploiting sensor data for navigation enhancement, it is important to know the behaviour, limitations and main characteristics of the data we are working with, to establish a measure of the confidence and the consistence of the data.These properties are conditioned by the sensors and measurement processes.The logged data can be useful to debug the estimation algorithms and the flight performance but, in this work we will only pay attention to those data that could explain the local position estimations, the navigation system performance or the filter capabilities.Table V shows the data analysis for the main data sources available [17].

C. Data analysis
The logged data from different sensors are presented in this section.Data from triad of gyroscopes along body axes of UAV are displayed in Fig. 5, with different colors corresponding to six flights with the same programmed mission.We can appreciate some outliers and no significant biases  The corresponding histograms of previous data (with outliers removed) are presented in Fig. 6, here we can see the data is affected of Gaussian shape noise, and the mean value of turns corresponding to maneuvers can be seen in the z axis.Similar comments can be said about data from triad of accelerometers, presented in Fig. 7, with some significant outliers which were removed from tracking filter process.The histograms (outliers removed) are presented in Fig. 8, in this case the turns performed in flights are reflected in the accelerations done along the y axis.Similar analysis can be done for the other sensors (magenetometer, barometer, range finder, etc.), not presented here to save space.

D. Data filtering evaluation
Finally, other aspect related with sensor data is the availability in Px4 of some complementary filters that decrease the ambient noise by applying a low pass filter that gets only those data with an oscillation frequency that underneath the cutoff barrier and discard all data with a higher frequency.This approach works based in the principle that the vehicle movements that we wish to register, never surpass a certain speed, and those that overtake it can be directly considered as environment noise and vibrations.The use of complementary filters may increase the accuracy of the estimation and the efficiency of the system by reducing the amount data that the other algorithms like the EKF and the INS would have to deal.
Due to this relevance, during the configuration and calibrating process of our vehicles, we have performed an analysis of the max motor vibration frequency and those perturbations that travel through the frame of the multicopter up to the flight controller sensors.After this analysis we have tested the performance with different cutoff values to optimize the adjustment of the Low Pass Filter and ensure not to acquire useless data.
Figure 9 shows the band of dominant frequencies collected by the gyroscope sensor during an real flight.At lower frequencies we can observe the vehicle intentioned movements with frequencies that oscillate between [0-2] Hz, then with values that reach 90Hz are the movements produced by the fast speed corrections that are made on each motor to maintain the stability of the vehicle.The higher values than this frequency performs the vibration noise, introduced by the small unbalances of the motors, the propeller turbulence shocking with the chassis and a phenomenon of resonance between propellers denominated propwash.

Figure 9: Gyroscope data frequency analysis ( y-axis is amount of data)
To reduce the effect of the environment noise, the Px4 system counts with some Extended Kalman Filter algorithm that process all readed data using estimation states that increases the precision of the data thanks to the application of specific corrections on each measure based in known characteristic error of the sensors.

A. GPS and INS local position integration:
The most typical task carried out by the data fusion process of the Pixhawk (EKF2 filter) is the attitude estimation using magnetometer, gyroscope and accelerometer data (attitude and heading reference system), and then fuse with accelerometers and GPS data to estimate position and velocity.Fig. 10 a) shows an illustrative example of position estimation on the tangent plane XY (North-East) using the integrated measurements of accelerometers.We can see how the attitude changes in a superposition of the X and Y projections, especially on the last values.So, the sensor fusion system is based on a loosely coupled architecture which uses GPS position and velocity measurements to aid the INS, typically used in navigation solutions [18].In this way, the IMU sensors are used extrapolate position, velocity, and attitude at high frequency (50 Hz), while updates from GPS measurements at low frequency (1 Hz) allows refinement of cinematic estimates and inertial sensor biases.Typically, the estimated state vector resulting in the output for the GNSS/INS filter contains the attitude vector represented with a quaternion, 3D position and velocity, three gyro biases and three accelerometer biases.The selected coordinate frame for position and velocity is the ENU frame (East, North, Down) with respect to the tangent plan with origin defined by the arming point in the start of the mission Both the estimation of cinematic parameters and sensor biases critically depend on the filter process noise, whose parameters affect to the predicted error covariance and impact in the weights given to the observations with respect to the predicted estimates.A higher value for these parameters imply higher values of predicted covariance and so higher gain to observations (since the confidence on prediction decreases).Conversely, lower values imply lower gain to observations (higher confidence on predictions).The values have been systematically analyzed in the six scenarios used, with flights repeating the programmed mission (waypoints), but changing the parameters affecting to EKF performance shown in table VI.
These parameters are complemented with those used to characterize the measurement noise.The default values for sensor noise were used in all flights, as presented in table VII Finally, the sensor fusion logic includes protection values for self-assessment of performance, as mentioned in section 3, and the sensors will be removed from fusion each time the threshold value for residuals are exceeded, the default value is 5 times the standard deviation, as shown in table VIII.
The analysis was carried out by postprocessing the data collected from six flight tests using the described UAV system and operated autonomously (besides, the UAV was monitored remotely by a pilot in visual line of sight for safety reasons).Figure 11 present the horizontal position estimated by tracking filter and GPS observations (circles).As can be appreciated in the zoomed figure 12, the flights corresponding to higher values of parameters (like flight 1, in blue, flight 5, in black) present lower deviations during turns, and, conversely, are affected more by the GPS noise.The same effects can be observed with the GPS velocity observations.Figures 13 and present the observations and filtered values, with zoomed graphics for flights 2,6 to appreciate the difference response with respect to observations.

B. EKF vertical filter performance
Regarding vertical estimation, it can be obtained from different sensors, like barometer, GPS and range finders.Figure 15 a) shows an example of values estimated GPS and barometer.Notice that, the GPS value describes a discrete scale with high precision but low resolution and a large latency.On the other side, the barometer measurement allows higher frequency data but requires a calibration with respect to the typical atmospheric pressure.The result of the fusion process throws an estimated value with the accuracy benefits of both measurements, allowing a consistent altitude.
Regarding the use of range finders for height estimation, illustrative data in a flight test are presented in Fig. 15 b).As can be seen, a typical problem with these sensors are the short range of data or the errors introduced when the vehicle changes its pitch angle during flight.For this reason, the height calculation is made using the data from the barometer and the GPS if these sensors pass quality checks, and the measurement of the ground sensors are used only during landing and take-off procedures.For the flights analyzed in this section, figure 16 presents the GPS and barometer data input and the estimated output.As can be seen in the details in Fig. 17

VI. CONCLUSIONS
This paper presented a platform (Pixhawk PX4) and methodology to experiment with real data for UAV navigation.Based on data analysis and characterization the algorithms can take advantage of available sources.The quality of all inputs was systematically analyzed, and three processing algorithms, DCM, LPF and EKF, were evaluated with different parameters to exploit the data in the appropriate way considering the output analysis and So, this work presents a methodology to test and configure UAV navigation systems in real conditions, illustrated with an open environment for experimentation.The analysis of real data in a systematic way will allow successive improvements and parametrization, considering, among others, the following aspects: -Data filtering to reduce perturbations and remove outliers -Quality analysis to weight data uncertainty -Analysis of biases and calibration previous to fusion -Parameter adjustment to optimize performance (PID gains, filter parameters, observation and plant noises, etc.)

Figure 7 :Figure 8 :
Figure 7: Accelerometer data for all flights

Figure 10 :
Figure 10:Attitude estimation through GPS, INS and magnetometer data fusion

Figure 15 :
Figure 15: EKF vertical filtering using a) GPS and barometer fusion, b) barometer and range finder fusion

Figure 16 :Figure 17 :
Figure 16: Barometric and GPS height inputs and EKF output for all flights

Figure 18 :
Figure 18: Innovations in position for all flights

Figure 20 :
Figure 20: innovations in velocity for all flights

Figure 22 :
Figure 22: normalized innovations for all flightsTable IX summarizes the innovation analysis for the 6 flights