Model-Based Attitude Estimation for Multicopters

The paper deals with model-based attitude estimation for multicopters and is mainly focused on investigation of accuracy degradation due to wind and inaccurate model parameters which are conditions always present when using in real world. At first the need for model-base estimation is motivated. Then the multicopter model is described. Based on the mathematical model of multicopter, the estimation algorithm utilizing the extended Kalman filter is constructed. The main contribution of the paper is the investigation of the negative impact of the wind and of inaccurate knowledge of the model parameters.


Introduction
Multicopters are VTOL (Vertical Take-Off and Landing) aerial vehicles (AVs) included within the class of multirotor helicopters.These vehicles differ from standard helicopters in that they use rotors with fixed-pitch blades, and thus their rotor pitch does not vary as the blades rotate.Currently existing versions comprise 4 or more rotors (x-copters); the hexacopter uses 6 rotors (Fig. 1).
There also exist versions with fewer rotors, but they require additional moving components for stabilization, thus they do not possess the mechanical simplicity feature.
At present, these types of AVs are used as a standard platform for robotics research.The first objective in the construction of automatic control systems for these AVs is to ensure stable flight at low velocities, particularly during the hovering phase.The design of x copter control elements has hitherto been discussed by a large number of authors.Generally, the con- trollers are designed as linear SISO systems [1], PID systems [2], special nonlinear controllers [3] or [4], and even neural net controllers [5].The most important component of the control loop is a good feedback signal providing correct information about the AV's attitude.Although the majority of the above-mentioned systems use on-board sensors, the necessary feedback can be also provided by off-board sensing elements [6].The most widely used devices for attitude estimation are inertial sensors and magnetic field sensors based on MEMS technology, namely MEMS accelerometers, gyroscopes and magnetometers.In fact, these devices form a strap-down inertial measurement unit (IMU), and they have become the main attitude sensors due to their low cost.Other sensors, such as sonar range finders, cameras, lasers, or GPS, are used especially for position feedback.
A significant factor in the design of a control system is a good mathematical model which describes both dynamics of the AV itself and dynamics of the IMU as well.In most papers, the presented models of controlled AVs consider only the forces and moments caused by propellers in hovering and neglect all other aerodynamic effects because of the low linear velocities.Such models do not produce accelerometer data usable for good feedback ensuring accurate information on the attitude of an AV [7].
The basic sensors of IMU do not provide direct information about the attitude of the AV.To provide the right information about the attitude of the AV, the sensor signals must be appropriately handled.At present, the processing of such signals is performed by the algorithm of Kalman filtering.This algorithm is used in the calculation of the attitude in form of the combination of prediction and correction.For the prediction a suitable dynamic model is used, the correction is carried out by direct measurement.Thus well chosen model of prediction can improve information about the attitude of the AV.The following article shows and discusses characteristics of such a model and its use for estimation.

Model Based Attitude Estimation
The typical attitude estimation algorithms [10] use no assumptions about dynamics of the examined object.They rely only on the used sensors (typically accelerometers, gyroscopes, magnetometers, GPS etc.).With good model including the knowledge of the dynamic properties can provide more accurate results.But any unmodeled effects or inaccurate model parameters can degrade the accuracy and even make the results worse than without the dynamic model.
Multicopters are obviously good adepts (inputs to the system are known, several dynamics models were constructed) where including the dynamic model should improve the accuracy.The analysis of inaccurate model parameters and unmodeled effects (namely wind) is subject of this paper.
In the following section multicopter dynamic model based on the recent papers is described.In the fourth section the dynamic model based attitude estimation algorithm is constructed.The results of simulations, where mainly the effects of wind and inaccurate model parameters are studied, are in section 5.
Finally in the section 6 the wind effect mitigation is described.

Multicopter Model
In this chapter the mathematical model of multicopter is mentioned.This model was used for generation of input data for simulation of attitude estimation algorithm.Additionally the model was also used for designing the estimation algorithm itself.The biggest advantage of using mathematical model for generation of testing data is the knowledge of the true values of all states.This feature enables direct comparison of the estimated state with the true state.
Mathematical model of multicopter is studied in many recent publication focused mainly on attitude and position control [1], [2], [3], [4], [5] but also for attitude estimation [10].They differ in level of precision and in number of modeled effects.Here mentioned model is based on all cited works, and the main effects of interest are aerodynamic drag, wind and precise sensor models.
This particular model assumes the multicopter has 6 motors with propellers arranged equidistantly on a circle with radius L. The scheme of the multicopter is in Fig. 2. Two coordinate frames are used throughout the description of the multicopter model.Body frame is the coordinate system rigidly linked with the multicopter.The orientation of body frame axes with respect to multicopter is depicted in Fig. 2. Reference frame is a coordinate system with respect to which the attitude of body frame is investigated.Usually this frame is linked to local vertical direction and to true north direction.In this work this frame is assumed to be inertial frame.It means that Coriolis force, earth rotation and transport rate, which are common for frames linked with earth surface, are neglected.This approximation is valid when the usage of low-cost sensors is expected since these effects are beyond the resolution of the sensors.

Forces and Torques
Forces and torques are driving the motion of the multicopter.The total force and torque are composed of different sources: thrust of individual propellers, aerodynamic drag and gravity.

1) Motors with Propellers
Thrusts of individual propellers are related to the control signal.This relation is modeled by this equation.
where T i is thrust of i-th motor, c T is a positive constant parameter and S i is control value for i-th motor.Each motor also generates reactive torque, which has opposite direction than the angular rate vector of the motor shaft.The magnitude of the reactive torque is assumed to be proportional to motor thrust.
where m i is the reactive torque vector of i-th motor, c R is a positive constant parameter and ω i is the angular rate vector of i-th motor shaft.The resulting force and torque vector generated by thrusts of motors are computed using.
where F z is the z-component of force vector, F BT expressed in body frame respecting the coordinate system defined in Fig. 2, M x , M y , M z are components of torque M BT expressed again in body frame, L is length of the multicopter arm and c R is constant parameter defined in Eq. ( 2).The matrix A can be derived using simple mechanics and Fig. 2.

2) Aerodynamic Drag
Aerodynamic drag is very often neglected in multicopter models.However, this phenomenon enables use of the accelerometer measurements in the attitude estimation.This is the main reason why this drag is implemented in this multicopter model.
The aerodynamic drag force is modeled as linearly dependent on the relative velocity with respect to wind.
where F BA is drag force vector expressed in body frame, k i are positive constant parameters and v w ib are components of relative velocity with respect to wind expressed in body frame.This is a simplified version of propeller induced aerodynamic drag from [7].The reason why the parameters k i are constant in body frame comes from the constant rotation axis of propellers with respect to this frame.The parameter k z is assumed to be of order smaller than the k x and k y , because the z body axis is parallel to the axis of rotation of the propellers and the main contributor to the aerodynamic drag is the blade-flapping phenomenon which occurs only in the axis perpendicular to the propeller rotation [7].
The aerodynamic drag torque is similarly modeled as linearly dependent on angular rate.
where M BA is drag torque vector expressed in body frame, k M is positive constant parameter and ω B is angular rate vector expressed in body frame.The aerodynamic drag torque is based on the experience that angular rate settles on some finite value when constant torque is applied.In many recent multicopter models this effect is omitted.

3) Total Acceleration and Angular Acceleration
All the mentioned forces and torques are summed and divided by mechanical properties to form the final acceleration a I and angular acceleration ε B .
where a I is acceleration expressed in inertial frame, g I is gravitational acceleration expressed in inertial frame, R BI is transformation matrix from body to inertial frame and m is the mass of the multicopter.
where ε B is angular acceleration, ω B is angular rate and I B is inertia matrix of the multicopter, all variables expressed in body frame.

Multicopter Motion
The motion of multicopter is modeled as the 6-DoF rigid body motion driven by forces and torques described above.The motion state is described by velocity and position for translational motion and by angular rate and attitude quaternion for rotational motion.This part is valid for any aerial vehicle and is expressed by the following differential equations.

1)
Position and Velocity where p is position vector, v I is velocity vector expressed in inertial frame and a I is acceleration vector expressed in inertial frame.
2) Attitude and Angular Rate where q is attitude quaternion and ω x , ω y , ω z are components of angular rate vector expressed in body frame.
where ω B is angular rate vector expressed in body frame and ε B is angular acceleration vector expressed in body frame.

Wind
Since the aerodynamic drag force is dependent on velocity with respect to wind and the acceleration measured by accelerometers (used for attitude estimation) is the time derivative of the inertial velocity, it is reasonable to model the wind to be able to reveal possible negative impact of this fact on the accuracy of attitude estimation.
The wind is specified by instant velocity vector expressed in inertial frame and in this work is assumed to be only function of time.The total wind velocity consists of static and dynamic part.Static part is a constant velocity vector expressed in inertial frame and corresponds to dominant constant wind experienced in real condition.Dynamic part is time dependent and each component of the dynamic velocity is modeled as a first order Gauss Markov process [11].The dynamic component corresponds to short time variation of the wind (wind gust).The behavior of the dynamic part can be adjusted by steady state variance and by time constant of Gauss Markov process.

Sensors
With respect to attitude estimation these sensors are of interest: • Gyroscope -measuring angular rate.
Each of this sensor senses the physical quantity along three perpendicular axes parallel to body frame axes.True values of these sensors can be easily computed from true state available for multicopter model.True values are then deliberately corrupted to have similar characteristics like the real sensors of this type.
The same sensor model but with different parameters is used for each of these sensors.
where x T RU E is true value, x OU T is modeled senor output, b S is constant bias, b D is dynamic bias modeled as first order Gauss-Markov process and n is a white noise.The qualitative characteristics of the sensor model can be seen in Fig. 4, where zero true signal is corrupted by all terms of the sensor model.

Implementation of Model-Based Attitude Estimation
The model-based attitude estimation is not new.Is discussed in some papers including [12], [13].The new contribution is the identification and investigation of main causes which could make the estimation less accurate in real world.The model based attitude estimation for multicopters assumes that this information is available: • Data from gyroscope, accelerometer and magnetometer.
• Control values for motors.
The traditional algorithms which process the sensors from the first bullet use so-called vector matching method.This method assumes that the accelerometer measures gravitational field only (which holds some information about attitude).But this is generally not true during the whole flight and it is especially not true during aggressive maneuvers and changes of attitude.
On the other hand the model based attitude estimation use mathematical model with modeled aerodynamic drag to predict the true accelerometer measurement.The accuracy of this approach is then theoretically independent of the maneuvers flown by the aerial vehicle.
The model based attitude estimator is based on the mathematical model of multicopter mentioned in the previous chapter.In Fig. 5 there is a scheme of the full multicopter model.
The method used for estimation is extended Kalman filter.It provides the best scalable framework for implementation of such a model-based estimation problem.Extended Kalman filter is an iterative algorithm.Each of its iteration consists of two steps.In prediction step, the next value of the state is predicted based on the inputs and the previous state.In update step the corrections are computed using the measurements and applied to the predicted state.
According to Fig. 5 the full model could be used as a core of a Kalman filter estimator.It means use motor signals and wind speed for prediction of the state and then use all three sensors (gyroscope, accelerometer and magnetometer) for correction of the predicted state.
However the full model is slightly simplified for model based attitude estimation for various reasons.At first it is assumed that the multicopter is not equipped with any kind of sensor capable of measuring wind speed.For this reason the wind speed input has to be neglected.Next, the motor signals are not used for prediction of angular rate (measured directly by gyroscopes), despite it is possible.This prediction is not used in the presented estimation algorithm, as the accuracy of the prediction is very low compared to the accuracy even of low cost gyroscope.This optimization leads to lower number of states of the filter and in lower computation complexity.Additionally the knowledge of inertia matrix of the multicopter is not required.
Therefore the Kalman filter based on the simplified model uses the gyroscope and motor signal for prediction step and the accelerometer and magnetometer for update step.The scheme of the simplified model is in Fig. 6.The estimated wind is assumed to be zero in the first version.Chapter 6 discuss the effect of estimation of the wind speed.
The general equations of discrete extended Kalman filter are:

1) Prediction
Step 2) Update Step where x, u, z are the state, input and measurement vectors respectively, P, Q, R are the covariance matrices of state, inputs and measurements respectively, F and H are Jacobian matrices of functions f and h and finally k is the sample time index.
The estimation algorithm utilizing extended Kalman filter is uniquely defined by the functions f and h and by covariance matrices Q and R. The state, input and measurements vectors for here presented model based attitude estimation are defined as follows.
where q is the four element attitude quaternion, v I is the velocity vector expressed in inertial frame, b is the gyroscope bias vector, ω g is the angular rate vector sensed by gyroscope, S is the vector of motor control inputs, a B is the specific force vector sensed by accelerometer and m B is the magnetic field sensed by magnetometer.In this case, of model based attitude estimation algorithm, the function f consists of the following subparts: • Discrete quaternion integration based on Eq. ( 9) where the angular rate vector has the following format to incorporate estimated gyroscope bias.
The function h which predicts the measurement values have the following parts.
• The prediction of magnetic field in body frame.
where R IB is transformation matrix from inertial to body frame and m I is supposed to be known Earth magnetic field vector expressed in inertial frame.The measurement model for magnetometer assumes that magnetometer is measuring the Earth magnetic field only which is generally not true and care must be taken when using especially in indoor environments.
The covariance matrices Q and R are defined as follows [13].
where σ g is standard deviation vector of gyroscope white noise, σ V is standard deviation vector representing the accuracy in computation of the velocity increment, σ B is standard deviation vector of white noise driving the Gauss-Markov process for gyroscope biases, σ A is standard deviation of accelerometer which overbounds all error sources of this sensor and σ M is standard deviation vector of magnetometer which overbounds all error sources of this sensor.

Simulations
The computer simulations of model-based attitude estimation algorithm, described in the previous chapter, were targeted to find the limits of use of such algorithm in real conditions.With respect to this aim, the effects of wind and the inaccurate model parameters on accuracy of attitude estimates were investigated.
At first the parameters of the full multicopter model was defined.The summary of the model parameters are in the Tab. 1.This model was then used for generation of two, 60 s long, testing trajectories.First is low speed normal trajectory with tilts less than 45 degrees.The second one is high speed aggressive trajectory with tilts up to 85 degrees.The estimation errors for attitude recomputed to Euler angles for both trajectories are in Fig. 7 and Fig. 8.The error is computed as a difference between estimated value and the true value.Red lines in the figures are 1 − σ accuracy of the estimated state directly computed from the Kalman filter covariance matrix.The model parameters used in estimation algorithm match the parameters used in the generation of testing data.Wind is not present in these cases.

Wind Effect
The reason that one can assume that wind should have negative impact on accuracy of attitude estimation  comes from the fact, that in real world the aerodynamic drag is proportional to relative velocity with respect to wind while the estimation algorithm assumes it is proportional to inertial speed.
It is appropriate to assume that constant wind will have no effect on the accuracy of estimation because with respect to attitude the constant wind will cause no change of measurements in any used sensor.However the changes of wind can cause significant change in accelerometer measurements which are not predicted by the model.This can obviously lead to decreased accuracy.The following simulations are performed to verify above hypothesis.On both testing trajectories, four different settings of wind were simulated.The measure of negative impact is the RMSE error of estimated attitude expressed in Euler angles.The different settings of wind parameters are in Tab.The values in Tab. 3 indicate how the wind affects accuracy of the attitude estimates.It can be concluded that static part of the wind has almost no effect on estimation accuracy while dynamic part (the wind speed changes) causes significant decrease of the accuracy.Another conclusion is that the type of trajectory has a little influence on accuracy when wind disturbance is introduced.

Model Parameters
The inaccurate model parameters can also have negative impact on accuracy of attitude estimates.In order to determine the quantitative information about this effect, another set of simulations was performed.
The model parameters required in estimation algorithm are listed in Tab. 4, where also the corruption scenarios of selected parameters are defined.For each scenario the corresponding parameter was corrupted in estimation algorithm from its true value by the factor shown in Tab. 4 and then the RMSE value for both trajectories was computed which are listed in Tab. 5. Mass and arm length are excluded from simulations, since these quantities can be with ease very accurately measured.In other words it is not expected that these parameters would be inaccurate.The force drag y parameter is excluded because it is assumed that the behaviour in x and y axis will be very similar.
From Tab. 5 many interesting conclusions can be made.At first the accuracy of estimation strongly depends on the type of trajectory when some of the parameter is known inaccurately.Further the accuracy of the force drag z parameter is more critical than the ones for x and y axes.Finally the most critical parameter is constant relating the motor control signal to motor thrust.
The results of this chapter show the accuracy degradation effects which can be expected when using the model-based estimation algorithm in real application.The wind and model parameters are only a part of all effect which can degrade the accuracy.Even the model equations could be just approximations of real world behavior (which is obviously true) but here mentioned results can help suggesting on which part the deep research should be performed to gain better accuracy.

Wind Effect Mitigation
As shown in section 5.1 the wind has negative impact on attitude estimation accuracy using dynamic model approach.One possible way how to inhibit this effect is to include the wind speed vector into the estimated states.In the previous case the model assumed that velocity with respect to wind equals to inertial velocity.
In this case they differ by amount of the estimated wind speed.The estimated wind speed is expressed in inertial frame.By rewriting Eq. ( 4) one gets.
where v iB is velocity vector with respect to inertial frame expressed in body frame, R IB is transformation matrix from inertial to body frame and v w I is estimated wind speed vector expressed in inertial frame.Additionally the individual components of wind velocity vector are modeled as Gauss Markov process.In the following simulations the parameters of Gauss-Markov process for estimated state is the same as the one used for generating the wind speed vector.Results of simulations are in Tab. 6.The results show that including the wind speed vector into estimated state space significantly reduce the RMSE values for last two rows where dynamic wind is present.

Conclusion
This paper reveals some properties of so-called model based attitude estimation for multicopters.The standard model of multicopter with induced drag forces was presented as a baseline for designing the estimation algorithm.In simulations the performance of the filter was verified on data generated by the full model with advanced sensor models.Further the effect of wind and inaccurate model parameters were investigated.The results can be effectively used for focusing future research on the key parts which have the biggest influence on estimation accuracy, namely the wind state estimation and online parameter estimation.
there is the output of the wind model simulation with time constant τ = 7 s, constant speed vector b = [2, 0, −2] m•s −1 and steady state deviation of Gauss-Markov process σ = 0.3 m•s −1 .

Fig. 6 :
Fig. 6: Scheme of the simplified model used for attitude estimation.
2. The resulting RMSE values are in Tab. 3. Full multicopter model parameters.Tab.3: Resulting RMSE values of wind impact simulations.Resulting RMSE values for algorithm with or without the wind vector state.