In this section the system equations will be derived and listed including the decision about the representation of A/C orientation. The first part includes the equations used in the initialization step (MODE 1). The second part derives the filter equations used in the subsequent modes and checks the observability of the system.
4.3.1 Filter initialization
In the initialization step (MODE 1), the initial Euler angles and rate gyro biases are calculated together with the components of Earth’s magnetic vector. The mean measured values of acc., magn. and angular rate data are used to do this meanwhile aircraft stands undisturbed on the ground. These values should be calculated using a recursive formula to avoid unnecessary memory usage to store the input values:
¯
v1 =v1, v¯k+1 = ¯vk
k
k+ 1 +vk+1
1
k+ 1 (4.3)
Here, v represents the vector of measured data, ¯vis the computed mean and k is the time index of a sampled variable. In the following steps it is assumed that the IMU is perfectly aligned in body frame and there are no biases on the measured acc. and magn.
values.
The initial angular rate sensor biases are the calculated mean angular rate values (the real A/C angular rate is zero during data acquisition neglecting the Earth rotation rate).
The initial roll (φ) and pitch (θ) angles can be calculated from the mean acc. values as follows considering the fact that the measured acc. in body frame is the Earth’s gravity vector:
φ = arctan ¯ay
¯ az
, θ= arcsin (−¯ax) (4.4) Here, ¯ax, ¯ay, ¯az represent the components of the mean body acc. vector ¯aB. Using these angles the mean body magnetic vector (¯hB) can be transformed into earth frame (¯hE′) assuming zero yaw angle. This way, the magnetic yaw angle (ψh) can be calculated from the horizontal magnetic components in earth frame. Finally, the magnetic yaw angle corrected with magnetic declination will be the real yaw angle (ψ) of the aircraft (assuming that magnetic north plus declination gives geographic north). The magnetic declination Dis a fixed parameter in the autopilot program through a year. It is obtained and fixed from World Magnetic Model at the beginning of every year (flights are conducted every time at the same place and the yearly change of declination is about 0.1◦ at Hungary’s longitude and latitude).
h¯E′ =TTψ|ψ=0TTθTTφh¯B ψh = arctan
−h¯E′(2) h¯E′(1)
, ψ =ψh+D hE =TTψTTθTTφh¯B
(4.5)
In (4.5) finally, the Earth’s magnetic vector (hE) is calculated in earth frame. This is used later in the measurement equations (see (4.12)) and is assumed to be constant throughout the whole flight of the aircraft. Tφ, Tθ, Tψ are the rotation matrices related to the roll, pitch and yaw Euler angles respectively (see Appendix 8.18 for details). ()T denotes the transpose of a matrix. ¯hE′(1) and ¯hE′(2) are the first and second coordinates of the ¯hE′ vector,D is the magnetic declination.
4.3.2 Filter equations and system observability
The first stage of estimator design is to select the theoretical framework. The Extended Kalman Filter (EKF) is a well known and widely used [36, 56, 58, 77, 81] method for A/C state estimation. [36] points out that in attitude estimation it is at least as accurate as other (unscented and particle filter) algorithms but computationally less expensive. So, the EKF framework is selected.
The choice of orientation representation has large impact on the solution. In aerospace applications, mostly Euler angles [1, 24, 56, 77], quaternions [42, 58, 81] or representations involving the direction cosine matrix (DCM) [5, 39, 55] are used. The decision between them should be made considering their effects on accuracy. In this work, the decision between Euler angles and quaternions is done, the DCM possibility is not considered.
The differential equations for Euler angles and quaternion representations are repeated here from [79] considering measurement noise and bias in the angular rate (of the body frame) components:
φ˙ θ˙ ψ˙
=
¯
p+ tanθsinφ·q¯+ tanθcosφ·r¯ cosφ·q¯−sinφ·r¯
sinφ
cosθq¯+coscosφθr¯
=f(ρ, φ, θ) (4.6) In (4.6), ¯p represents the real roll rate which means the measured roll rate corrected with the bias and zero mean Gaussian white noise of the sensor, that is, ¯p=p−bp−vp. The same convention is used for the pitch (q) and yaw (r) rate variables and ρ denotes the parameter vectorρ=
¯
p q¯ ¯rT
.
˙
̺=−1 2
0 p¯ q¯ r¯
−p¯ 0 −r¯ q¯
−q¯ ¯r 0 −p¯
−r¯ −q¯ p¯ 0
̺=A(ρ)̺ (4.7)
In (4.7) ̺ =
̺0 ̺1 ̺2 ̺3
T
is the unit quaternion representing the same rotation as the Euler angles above.
(4.6) shows that Euler angles have a singularity property atθ=π/2+kπ k ∈Z+. This is usually avoided in non-aerobatic flight, but means a risk in the estimation. Another issue is the implementation of the estimator which requires as simple equations as possible to provide real time applicability on microcontrollers as was considered in the development of the control algorithms also. In (4.6) the trigonometric functions of Euler angles are applied, while in (4.7) only the simple linear combinations of the angular rate components with the quaternion elements are taken.
From the continuous time (CT) nonlinear differential equations (4.6), (4.7) a discrete time (DT) linearized dynamics should be created to be applicable in a discrete time EKF.
[61] points out that the use of Heun scheme (trapezoidal integration) can improve the accuracy of EKF. For a nonlinear, state (x) and parameter (ρ) dependent and noise (n) affected system, the Heun scheme results as follows [61]:
x˙ =f(x, ρ,n) xk+1 ≈xk+x˙k+ ˙xk+1
2 ∆t=
=xk+ f(xk, ρk,nk) +f(xk+1, ρk+1,nk+1)
2 ∆t
(4.8)
Here, ∆t represents the discrete time step. Equation (4.8) is usually hard to solve because of the nonlinear relation with the unknown xk+1 state through f(x, ρ,n). This
is the case with the Euler angle dynamics in (4.6). However, considering the quaternion dynamics (4.7), a nice closed form solution can be derived as follows.
(4.7) can be reorganized expanding ¯p, ¯q and ¯r to select p, q, r measured angular rates and the bias and noise terms:
˙
̺=A1(ρ)̺+A2(̺)b+L1(̺)v̺=
=A(ρ,b)̺+L1(̺)v̺ where ρ=
p q r−br
T
b= bp bq
T
v̺=
vp vq vrT
(4.9)
Here, A1(ρ), A2(̺), A(ρ,b) and L1(̺) are presented in Appendix 8.18. In (4.9) ρ is the measured parameter vector (br is known from initialization),b is the vector of roll and pitch rate sensor biases andv̺is the vector of sensor Gaussian noises. The dynamics of the slowly varying (theoretically constant) biases can be modelled as a system driven by zero-mean Gaussian white noise:
b˙ =vb where vb =
vbp vbqT
(4.10) Considering (4.8), (4.9) and (4.10) and applying some manipulation the discrete time dynamic equations of the quaternion and bias dynamics result as:
̺k+1
bk+1
| {z }
xk+1
=
M+k+1−1
M−k ∆t2 M+k+1−1
A2(̺k)
0 I2
| {z }
Ak
̺k
bk
| {z }
xk
+
+
∆t M+k+1−1
L1(¯̺k+1) 0
0 ∆tI2
| {z }
Vk
v¯̺k+1
¯ vbk+1
(4.11)
Here, the state of the system consists of the quaternion and bias vector (xk ∈ R6).
For the details of the derivation see Appendix 8.18. If a rotational transformation is represented by a quaternion, there is a unit norm constraint on it. The satisfaction of this constraint should be provided by normalization in every time step, because system dynamics does not guarantee unit norm. After deriving the state equation, the output equations for the different measurements should be derived together with the Jacobian matrices required in EKF dynamics.
For the magn. measurement, the nonlinear equation and its Jacobian are as follows:
yH =hB =TBE(̺)hE =h1(̺,hE), C1 = ∂h1(̺,hE)
∂̺ (4.12)
Here, TBE(̺) represents the quaternion based rotation matrix from earth to body frame (see Appendix 8.18) (yH ∈R3, C1 :R4 →R3).
For the measurement of Earth’s gravity vector in body frame (static case):
ya =aB ≈eBG =TBE(̺)eEG =h2(̺), C2 = ∂h2(̺)
∂̺ (4.13)
(ya ∈ R3, C2 : R4 → R3). In the dynamic case (MODE 4) ya is approximately corrected with the IAS according to (4.1) applyingeBG ≈aB+ωB×
IAS
0 0
The nonlinear output equation for GPS azimuth angle and the Jacobian are:
yGP S =ψGP S = arctan vE
vN
=
= arctan
TBE(1,2) TBE(1,1)
=h3(̺), C3 = ∂h3(̺)
∂̺
(4.14)
Here, TBE(1, i) are selected elements from the rotation matrix (yGP S ∈R, C3 :R4 → R).
The general structure of the linearized, DT output equation is shown below (with w zero-mean Gaussian measurement noise vector).
yk+1 =Ck+1xk+1+wk+1 (4.15)
The elements of this output equation in the different modes are listed below
• MODE 2,4,5: y(2,4,5) =h yHT
(ya)T iT
∈R6 and C(2,4,5) =
C1 0 C2 0
:R6 →R6
• MODE 3/A: aerial mode with magn. & GPS correction y(3A) =h yHT
yGP S iT
∈ R4 and C(3A) =
C1 0 C3 0
:R6 →R4
• MODE 3/B: aerial mode with only magn. correction if GPS is invalid for short duration or not available (due to the different update rates of IMU and GPS).
y(3B) =yH ∈R3 and C(3B) =
C1 0
: R6 →R3
After deriving the state and output equations, the observability of the system should be examined. This is done by checking the observability of the nonlinear system according to [41].
The observability of the nonlinear system for every applied input (measurement) com-bination is checked considering the observability distribution in several points of the state space. A set of Euler angles is used to grid the quaternion space (using Euler angle to quaternion conversion) to provide a systematic test:
φ =
−90 −80 −70 . . . 90 −45 45
[deg]
θ =
−60 −50 −40 . . . 60 −45 45
[deg]
ψ =
−180 −170 −160 . . . 180 −45 45
[deg]
These values are selected to cover the flight envelope of a UAV with limited aerobatic capability (no loops and inverted flight). The observability distribution can be calcu-lated according to [41] pages 69-76. The method involves symbolically calculating the
observability distribution, then to calculate its numeric values with the given gridding of the state space (the pure symbolic calculations could not include zero values and so, the system is always observable from any measurement). The following measurement config-urations are examined (with the six dimensional state space (4 quaternion elements and two biases)):
• MODE 3/B: with nonzero angular rates and biases the rank of the observability distribution (od.) is 5 in every state space point, so the system is not observable from pure magnetic measurements
• MODE 2,4,5: with nonzero and also with zero angular rates and biases the rank of od. is 6 in every state space point, so the system is observable from magnetic and acceleration measurements
• MODE 3/A: with nonzero and also with zero angular rates and biases the rank of od. is 6 in every state space point, so the system is observable from magnetic and GPS measurements
Consequently, the system is not observable from the pure magnetic measurements according to the nonlinear observability test. The last test done is to delete the bias values from the state vector and check the observability of the resulting four state system from pure magnetic measurements. Nonzero angular rates and biases are substituted, the rank is 3 in every state space point, so the reduced system is also not observable.
As a summary, it can be stated that the system is not observable from pure magnetic measurements and, so the GPS or acc. measurement is also required to provide con-vergence. Between the GPS corrections some divergence can be observed (because pure magnetic measurement is used), but the 4Hz GPS frequency is enough to successfully limit the possible errors.
A hybrid automata representation of the algorithm was constructed in [BB11a]. After deriving the system and measurement equations and checking observability, the tuning and testing of the algorithm can be done.