ON-LINE INERTIA MEASUREMENT OF UNMANNED AERIAL VEHICLES USING ON-BOARD SENSORS AND BIFILAR PENDULUM

Identification of a dynamical model and its parameters is one of the fundamental problems in the field of robotics and system dynamics modelling. For the general situation of an object motion with six degrees of freedom (6-DOF), such as in the case of the Unmanned Aerial Vehicle (UAV), the key physical parameters are vehicle mass and moment of inertia. Even though UAV mass and its geometry/topology are easily obtainable, it is difficult to identify the inertia tensor considering that it is not measurable by static tests. This article presents a simple and effective method for on-line estimation of a rigid-body inertia based on a two-wire pendulum and an on-board integrated sensor system. Herein, the test subject (i.e. UAV) is suspended by two thin parallel wires in such a way to form a bifilar torsional pendulum about the vertical axis. Using on-board sensors from the UAV flight controller (FC) unit, the pendulum oscillations are recorded and processed in order to obtain trendfree and noise-free signals used in the final inertia estimation phase. The proposed identification algorithm is verified experimentally for two typical cases of suspended objects related to the UAV control box and a full UAV configuration.


INTRODUCTION
For the purpose of UAV control systems development, knowledge about mathematical model of the system is required.In order to attain the requisite precision of such system model, all important parameters are needed to be known with sufficient accuracy [1,2].Since moment of inertia cannot be estimated through static tests, its identification could pose certain difficulties.For geometrically simple objects, inertia can be derived analytically or obtained from a model in CAD software.For more complex bodies such as the UAVs, equations tend to get more complex, which makes the calculation of inertia quite impractical, so it would be more convenient to estimate it from dynamical tests.Various approaches in [3][4][5][6][7] introduce identification of inertia based on measurements of the oscillation period of a bifilar pendulum.Although all these methods enable very precise results, they are based on offline analysis of measured data.This article proposes the implementation of an on-line inertia estimation procedure using Kalman filter techniques and angle estimate from the inertial measurement unit (IMU) integrated inside the UAV flight controller (FC).In order to eliminate unwanted signal drift and noise from measurements, a recursive differentiating filter is used, cascaded to the lowpass filter implemented in the form of a scalar Kalman filter.Such processed signal is used to determine the period of a pendulum-suspended body oscillations, through detection and measurement of angle rise condition.As a final result, estimation result is based on analytical relationships of a pendulum oscillation period and inertia.
For the purpose of experimental identification of the UAV inertia, test setup is developed and assembled from two vertical beams forming the base of the pendulum, one horizontal beam and the test body (UAV)suspended via a two thin wires.When the test body is displaced from the initial condition and set free, a restoring torque twists it back toward the equilibrium position, and periodic motion is initiated, characterized by specific oscillation frequency.
The article is organized as follows.At first, a pendulum dynamics model considering the test body is introduced.Following by the design of an inertia estimation algorithm that includes the body deflection angle measurement, sensor drift compensation (detrending), a low-pass filtering to remove the sensor noise, and a pendulum oscillation period detection.Final section gives results of experimental verification for the proposed moment of inertia identification procedure and the concluding remarks.

PENDULUM MODEL
Dynamic modelling based on equations of motion is used to relate the measurable parameters of the pendulum's configuration to the inertia of the object suspended from the pendulum (see e.g.[3][4][5][6][7]).
For modelling purposes, some non-dominant physical factors are neglected as a trade-off between simplicity and estimation accuracy.The following assumptions are introduced: pendulum is centred around UAV Center of Gravity (CoG), wires are ideally parallel, and angular displacement of the wires from the vertical axis is negligible, wires are considered stiff and with negligible mass, displacement angle is kept small, there are no unbalanced vertical forces and rotation of pendulum is considered only about the vertical axis.
Figure 1a shows the following defined geometric constrains: distance between suspension wires, d = 2r; length of wires, h; twist angle around vertical axis, α; tilt angle due rotation, β. amplitude to small angles wherein sin β ≈ β.Thus, angle of twist α can be related to rotation angle β via distance rα obtained from arc length formula: Tension force in wires is derived from object weight.At stationary conditions (equilibrium), net torque T z (Fig. 1) about vertical axis is zero and each wire holds half of object's weight.
When the object is rotated around vertical axis by a small angle, a horizontal reaction occurs due to the tension in the wires, given by the following relationships: From Figure 1b, reaction torque is induced by horizontal component of the tension.Since each string produces equal forces, and thus total torque equals 2F x r, where: Using Newton-Euler formulation, with T z = Iα̈, equations of motion are derived [6,7].
By solving this equation analytically, the solution for undamped oscillation case can be found in the standard oscillatory form: That oscillatory motion is characterized by the following period and frequency:

INERTIA ESTIMATION
The inertia estimation algorithm is implemented by using a Matlab Embedded Coder TM , wherein estimator components are written as functions and are verified by computer simulations utilising Matlab/Simulink environment.By using Pixhawk Development Support, appropriate firmware for the flight controller (FC) is generated and uploaded.The angle information is obtained by attitude estimator embedded within the UAV [8,9].

MEASURING ANGLE OF OSCILATION
By utilising the integrated inertial MEMS sensors connected to the microcomputer, it is possible to determine vehicle attitude in the form of corresponding Euler angles.Moreover, by using the Estimation and Control Library (ECL) from Pixhawk PX4 flight controller, the angle is obtained by sensor fusion of magnetometer, gyroscope and accelerometer [8,9].Thus, obtained data is fed to the external computer through serial port by using External mode option for running the UAV model [9].For that purpose, a custom firmware containing estimator application is generated and uploaded by using Matlab/Simulink Support library for Pixhawk Development [9].

INERTIAL SENSOR DRIFT COMPENSATION
As discussed above, the oscillation angle is obtained from the IMU sensors integrated within the flight controller unit.These sensors provide fast and precise response, but may suffer from accumulated error, i.e. they may tend to 'drift'.Due to aforementioned integrator properties of sensor dynamics, a constant error in acceleration measurement results in a linear (ramp) error in velocity and a quadratic (parabolic) error growth in terms of the UAV position.In order to remove (filter out) these drift effects, and, thus, to increase the robustness of inertia estimation, a recursive drift removal filter is introduced [10]: where y is the filter output, x is its input and a f is the filter tuning parameter typically chosen between 0,9 and 1.The corresponding discrete-time (z-domain) transfer function of the detrending filter is given as: Figure 3a shows the pseudo code of the proposed detrending filter.Its effectiveness in terms of drift removal for typical IMU sensor measurements is illustrated by the results in Figure 3b.
In particular, the deflection angle measurement linear trend superimposed to the actual pendulum oscillations is effectively removed, thus revealing the actual pendulum oscillatory movement in terms of displacement angle.

DISCRETE-TIME SCALAR KALMAN FILTER
To filter out the measurement noise, a discrete-time scalar type Kalman filer (KF) is implemented.For the case of single-dimensional signals, such as the angle measurement, scalar Kalman filter may be convenient since it provides an optimal estimate of the state variable under assumption that measurement noise variance and state variable perturbations are known in advance [11].The scalar KF is the special case of the more general matrix wherein, wherein the model parameters and measurements are scalar-valued [12,13].The filter operates by interchanging two characteristic phases: "predict" and "update".The predict phase uses the state estimate from the previous time step to produce an estimate of the state at the current time step (a-priori estimate).In the update phase, the current a-priori prediction is combined with current observation in order to refine the state estimate.This corrected estimate is termed a-posterior state estimate [11,14].
The internal state of the scalar Kalman filter is modelled by a random-walk process model [11]: which includes a Gaussian perturbation (noise) ν, characterized by perturbation variance Q.The measurement noise is assumed to be Gaussian, and its variance R is derived from sensor technical specifications.The final scalar Kalman filter implementation is given by the following set of equations.
Predict phase: ) Update phase: Since the measurement noise variance R is assumed to be known in advance, the state perturbation variance Q represents the scalar Kalman filter tuning parameter.Its choice is usually a trade-off between parameter variations tracking ability and attenuation of noise.
Scalar Kalman filter implementation pseudo code is shown in Figure 4a.The filter response to an oscillatory signal with noise characterized by variance R = 1 is shown in Figure 4b, for the case of the Kalman filter tuning parameter Q = 10 -3 .The responses in Figure 4b indicate that the random-walk model-based scalar Kalman filter may be characterized by a certain response delay, but the general oscillatory behaviour is captured well.

OSCILLATION PERIOD ESTIMATION
Inertia estimation algorithm is based on the detection of oscillation rise and fall state with respect to time, and related oscillation period estimation.Angle evolution in time-domain can be separated into following stages: rise, fall, local maximum and local minimum.To detect if signal is in the state of increase (rise) or decrease (fall), a simple test algorithm is proposed.Input takes n samples from the inertial sensors and stores them in an array.When rise condition is detected a function initiates the counter that counts a number of samples until angle decrease condition is detected, wherein the counting is halted and the resulting number of counts is proportional to the oscillation period.By multiplying the counter final value and sample rate algorithm, the duration of rise condition is computed.If the last point is equal or higher than mean of first and second point, the angle has been in the state of growth, and the algorithm output is 1, otherwise the output is 0.
The proposed algorithm is given by pseudo code in Figure 5a, while the illustration of signal growth detection vs. oscillation plot is shown in Figure 5b.In this particular case, the angle growth detection initiates a counter when the input signal hits a drop point.The value of the counter multiplied by the sampling time provides an estimate of the oscillation period.Inertia is obtained based on the initial system parameters of the pendulum and the detected period of the oscillation.In order to obtain smooth (noise-free) estimation, the estimator output (inertia estimate) is further filtered by means of a narrow-bandwidth low-pass filter.

INERTIA ESTIMATOR
From previously derived pendulum equations ( 4)-( 6), expression for the moment of inertia in terms of oscillation period (or frequency) [3][4][5][6][7][8] can be obtained as: A two second delay is introduced before each inertia value is recorded, in order to eliminate initial oscillations in the estimate, that may otherwise lead to error in estimation.For every calculated inertia, data is averaged and final inertia estimation is acquired.Figure 8 show pseudo-code implementation and the results of period detection and average inertia calculation.

MEASUREMENTS AND RESULTS
For the validation of the proposed inertia estimation method, a moment of inertia is calculated analytically.Measurements are taken for two test case scenarios including the control unit box (Fig. 7a), and the overall UAV system (Fig. 7b).
In the first test case, the box, inertia depends on width , length , height , and the mass m with respect to the perpendicular axis to the rod, which passes through the middle of the rod (Fig. 7a) [15].The flight computer and the box are centred with respect to the rotational axis, and, hence, the total inertia of the system about this rotational axis is calculated as:  Figure 8 shows the principal schematics of the experimental setup for the case of control unit box and the UAV suspended by the pendulum test wires.In the former case (Fig. 8a), the suspension point is at the edge of the box, while in the latter case (Fig. 8b), the overall UAV is suspended at the axles of two opposing quad rotor propellers.
The quad rotor inertia model consists of the geometric bodies such as the cross-beam, boxes and the cylindrical elements corresponding to motors and propellers (propulsion).Since the quad rotor UAV mechanical structure shows a high degree of symmetry, body inertia tensor can be reduced to a diagonal matrix [16].Further approximation is to define the following parts: one cross-beam structure, modelled as two solid narrow cylinders fastened in the middle forming a cross, one electronics box and one FC, modelled as a rectangular parallelepiped, four motors, modelled as solid cylinders, four propellers, modelled as the flat cylinders (i.e.inertia of the rotating blade).The inertia of individual elements is listed in Table 1.
The total UAV inertia (Fig. 8b) is obtained by combining the inertia contributions of each body: frame inertia, electronics, flight computer, motors and propellers.Using parallel axis theorem and equations from literature [16], the following final set of equations is obtained for inertia values: The setup is shown in Figure 9, wherein additional stability of the frame is assured by tripod elements at the ground level.Inertia is measured about vertical (yaw) axis.Undertaken measurements and the final results are shown in Table 2.The procedure is similar for other axes of motion, i.e. the body is rotated according to referenced axis of measurement.Frame  The inertia estimates based on the pendulum period estimation show good consistency and low sensitivity to measurement noise, i.e. smooth steady-state estimates of moment of inertia are obtained by the proposed identification method.Box inertia about the particular axis of rotation is verified by means of a single experiment, due to high confidence in the relatively simple analytical expressions for box inertia (Table 1).On the other hand, for more complex body shapes, repeated tests would be worthwhile in order to establish the consistency of the experimentally obtained results, and, thus, to confirm the results of rather complex analytical calculations.

CONCLUSIONS
This article shows the hardware implementation of a simple and effective method for on-line estimation of a solid-body moment of inertia, with particular application to the UAVs.Using inexpensive (and readily available) MEMS sensors and interface with computer, this method is portable to many hardware platforms.Moreover, by using automated on-line inertia estimation procedure, the effect of human error is excluded from the results.
Experimental results were presented for a two test cases, a homogeneous box and a 500 mm wide experimental UAV.Analysis of calculated and measured inertia was given for the purpose of verification of results.Developed method of inertia estimation has been experimentally verified, and it has been characterized by simplicity of implementation, adequate accuracy, high repeatability and low costs in terms of the hardware requirements.User interaction is limited only in the starting phase of measurement by imposing initial conditions (twist angle of 5°-10°).

FromFigure 1 .
Figure 1.Bifilar pendulum: a) principal schematic, b) UAV deflections from normal and c) deflection angle representation related to body displacement and acting forces.

Figure 2 Figure 2 .
Figure2shows the individual components of the inertia estimation algorithm.

Figure 3 .
Figure 3. a) Pseudo code of offset removal function, b) offset removal function response.

Figure 9 .
Figure 9. Photograph of the UAV inertia measurement setup.

Figure 10 .
Figure 10.First test case, boxoscillations of body, period and inertia estimator response.

Figure 11 .
Figure 11.Second test case, UAVoscillations of body, period and inertia estimator response.

Table 1 .
Inertia calculation for test cases a) and b).

Table 2 .
Results of measurements.

Table 3 .
Abbreviations used in the text.UAV principal moments of inertia I xx , I yy , I zz