SlideShare a Scribd company logo
22nd International Congress of Mechanical Engineering (COBEM 2013)
November 3-7, 2013, Ribeirão Preto, SP, Brazil
Copyright c 2013 by ABCM
ATTITUDE DETERMINATION OF MULTIROTORS USING CAMERA
VECTOR MEASUREMENTS
Pedro Filizola Sousa Maia Gonçalves
Roberto Brusnicki
Davi Antônio dos Santos
Instituto Tecnológico de Aeronáutica, Praça Marechal Eduardo Gomes, 50, Vila das Acácias, 12228-900, São José dos Campos, SP,
Brazil
pedrofsm@ita.br, rbrusnicki@gmail.com, davists@ita.br
Abstract. The employment of camera in low-cost navigation and guidance of multirotor unmanned aerial vehicles (UAV)
has recently been the focus of many investigations. Nevertheless, in the previous works, camera measurements was
adopted either to aid in the position/velocity estimation or to directly provide feedback for guidance, but not specifically
for assisting in the attitude determination process. This work is concerned with the attitude determination of multirotor
UAVs using vector measurements taken from a camera. The vehicle is assumed to be equipped with an altimeter, a triad of
rate-gyros, and a downward-facing strapdown camera. It is assumed to fly in an indoor environment containing various
landmarks placed in known positions on the floor. The quantity and positions of the landmarks are chosen in such a way
that at least two of them always remain in the camera field of view. Therefore, at each time instant, two noncollinear unit
vectors directed from the camera to the center of area of the landmarks can be computed. In order to carry out attitude
determination, two quaternion estimation methods are adopted: the multiplicative extended Kalman filter (MEKF) and
the quaternion extended Kalman filter (QEKF). The proposed multirotor attitude determination scheme is evaluated by
computational simulations.
Keywords: aerial robotics, attitude determination, Kalman filtering, computer vision
1. INTRODUCTION
The attitude determination (AD) is a fundamental part of any control system for unmanned aerial vehicles (UAV). In
general, it is concerned with the estimation of the vehicle’s attitude and angular velocity with respect to a given reference
coordinate system. The estimates computed by the AD function is then used to provide the attitude control laws with
feedback information.
The literature on AD is very extensive and has mainly been developed in the aerospace field (Wertz, 1978), (Yang,
2012). The AD methods stems from the Wabba Problem (Markley, 1988), which defined a framework to estimate attitude
from vector measurements. (Cheng et al., 2008) uses the extended Kalman filter (EKF) to estimate pitch and roll angles
of a Micro Aerial Vehicle (MAV). The third column of the Direction Cosine Matrix (DCM) and the rate gyro bias are used
as state variables. Gravity is used as the observation vector in the measurement model. The yaw angle is obtained from
geomagnetic field vector. Gebre-Egziabher and Elkaim (2008) use both gravity and geomagnetic field as observation
vectors in two different approaches to estimate the attitude quaternion. The first approach is an iterated least-square
estimator (LSE) and the second is an EKF. The LSE executes a global search of the attitude at each time step. On the
contrary, the EKF algorithm accounts for a priori information, resulting in a better performance. The above two methods
were designed to be gyro-free and GPS assisted. (Bar-Itzhack and Oshman, 1985) proposes a quaternion extended Kalman
filter (QEKF), which, to ensure estimates with unit norm, realizes an Euclidian normalization step after each measurement
update. (Idan, 1996) proposes a minimum-variance filter to estimate attitude parameterized by Rodrigues parameters. Due
to simpler algebraic expressions, this approach has a relative computational advantage over the quaternion estimators.
(Markley and Crassidis, 1996) presents a multiplicative extended Kalman filter (MEKF) that estimates an attitude error in
MRP and updates the total attitude represented by quaternion by means of quaternion multiplication.
In satellite AD methods, vector measurements are typically taken from solar sensors (Sun direction), magnetometers
(local geomagnetic field vector), horizon sensors (direction of nadir), star sensors (direction of stars) (Wertz, 1978). On
the other hand, the multirotor UAV literature usually relies only on two vector measurements taken, respectively, from
accelerometers (local vertical) and magnetometers.
This work presents a multirotor UAV attitude determination method using vector measurements taken from images. It
is assumed that the vehicle is equipped with three strapdown sensors: a downward-facing camera, a triad of rate-gyros and
an altimeter. The vehicle is assumed to fly indoors over a flat ground with various landmarks. Both vehicle and landmarks
have known positions with respect to the adopted reference coordinate system. The landmarks are disposed, in quantity
and positions, in such a way that at least two of them always remain in the camera field of view (FOV). Using measure-
ments taken from the camera and the altimeter, two noncollinear vector measurements pointing from the camera to the
landmarks’ centers can be computed. In order to obtain a scheme for attitude determination of multirotor UAVs, these
Gonçalves, Brusnicki and Santos
Attitude Determination of Multirotors Using Camera Vector Measurements
vector measurements as well as rate-gyro data are considered in two attitude estimation methods: the QEKF (Bar-Itzhack
and Oshman, 1985) and the MEKF (Markley and Crassidis, 1996). The proposed scheme is evaluated by computational
simulation. The remaining text is organized in the following manner. Section II defines the paper problem. Section III
reformulates the attitude estimation methods. Section IV presents some simulation results. Finally, Section V presents
the paper’s conclusions.
Notation. IN is the N × N identity matrix, [•×] denotes the cross product matrix and • defines the matrix transpose.
2. PROBLEM STATEMENT
Consider the multirotor helicopter and the three Cartesian coordinate systems (CCS) illustrated in Fig. 1. The body
CCS SB = {XB, YB, ZB} is attached to the vehicle at its center of mass (CM). The ground CCS SG = {XG, YG, ZG} is
fixed on the ground at point O. The reference CCS SR = {XR, YR, ZR} is parallel to SG but is centered at CM.
𝑋B
𝑌B
𝑍B
𝑂
𝑍G
𝑋G
𝑌G
𝑋R
𝑌R
𝑍R
𝑓3
𝑓2
𝑓1
𝑓4
𝐬(1)
𝐬(2)
𝑀(1) 𝑀(2)
𝑀(𝑙)
𝐶𝑀
𝑀(3)
Figure 1: The Cartesian coordinate systems and the flight environment.
Assume that the camera is positioned at the CM and the triad of rate-gyros is aligned with SB. Define the set of
landmark indexes to be I {1, 2, ..., l}. Denote the center of the i-th landmark by M(i)
. Define s
(i)
to be the unit
geometric vector pointing from CM to M(i)
. Denote the representations of s
(i)
in SB and SR by b(i)
∈ R3
and r(i)
∈ R3
,
respectively. The representations b(i)
and r(i)
are interrelated by b(i)
= Dr(i)
, where D ∈ SO(3) is the attitude matrix of
SB with respect to SR. In order to measure two noncollinear pairs (b(i)
, r(i)
), one assumes that both CM and landmarks
have known positions and, moreover, at least two landmarks are measured by the camera at each sample instant. This
yields the following two pairs of vector measurements:
Vk
ˆb
(i1)
k ,ˆr
(i1)
k , ˆb
(i2)
k ,ˆr
(i2)
k , i1 ∈ I, i2 ∈ I, i1 = i2, (1)
where k denotes the discrete-time instant and, for i = i1, i2,
ˆb
(i)
k = D(ak)r
(i)
k + δb
(i)
k , (2)
r
(i)
k = ˆr
(i)
k + δr
(i)
k , (3)
where ˆr
(i)
k is a sample of r(i)
at instant k, δb
(i)
k and δr
(i)
k are zero-mean Gaussian white sequences with covariances R
(i)
b,k
22nd International Congress of Mechanical Engineering (COBEM 2013)
November 3-7, 2013, Ribeirão Preto, SP, Brazil
and R
(i)
r,k, respectively, and ak ∈ Rn
is a discrete-time attitude representation vector which parameterizes the attitude
matrix D(ak).
Let the attitude kinematics be modeled by the differential equation (Wertz, 1978)
˙a(t) = f(a(t), ω(t)), (4)
where a(t) is a continuous-time version of ak, ω(t) ∈ R3
is the true angular velocity. Since the rate-gyros are not perfect,
the true angular velocity is given by the following stochastic model:
ω(t) = ˆω(t) + δω(t), (5)
where ˆω(t) ∈ R3
is the measured angular velocity and δω(t) ∈ R3
is the rate-gyro measurement noise, which is
assumed to be a zero-mean Gaussian white sequence with covariance Q. A discrete-time version of Eq.(5) is given by
ωk = ˆωk + δωk, where δωk has the same characteristics of δω(t).
The main problem of the paper is to recursively compute the minimum-variance (MV) estimate ˆak|k of the true attitude
vector ak using the dynamic equation (4), the sequence of angular velocity measurements ˆω1:k, and sequence of vector
measurements V1:k.
3. PROBLEM SOLUTION
This section presents two estimation methods to face the afore-defined problem: The Quaternion Extended Kalman
Filter (QEKF) (Bar-Itzhack and Oshman, 1985) and the Multiplicative Extended Kalman Filter (MEKF) (Markley and
Crassidis, 1996).
3.1 Quaternion extended Kalman filter - QEKF
Bar-Itzhack and Oshman (1985) proposed a discrete-time extended Kalman filter to estimate the attitude quaternion.
This method is described in the sequel. Let the vector a(t) assumes the form of the attitude quaternion
q(t)
q1,t
et
, (6)
subject to the unit norm constrain
q(t) = q2
1,t + et = 1, (7)
where q1,t and et are, respectively, the scalar and the complex part of the attitude quaternion. This gives rise to the
following attitude kinematic equation (Wertz, 1978):
˙q(t) = Ω(t)q(t), (8)
where
Ω(t) =
1
2
0 −ω(t)
ω(t) −[ω(t)×]
. (9)
Integrating Eq.(9) from tk to tk+1, yields
qk+1 = Φ(tk+1, tk)qk, (10)
where Φ(tk+1, tk) ∈ R4×4
is the state transition matrix. Define Ts (tk+1 − tk) as the sampling time. Assuming
constant angular velocity ω(t) during the interval Ts, the state transition matrix can be written as
Φ(tk+1, tk) = eΩkTs
, (11)
where Ωk has the same form of Eq.(9), however it is computed using ωk. Rewriting Eq.(11) by using the discrete-time
version of Eq.(5), results
Gonçalves, Brusnicki and Santos
Attitude Determination of Multirotors Using Camera Vector Measurements
Φ(tk+1, tk) = e
ˆΩkTs
eδΩkTs
, (12)
where ˆΩk and δΩk are given by Eq.(9), but computed using ˆωk and δωk, respectively. The second factor at the right-hand
side of Eq.(12) is expanded in power series, yielding
Φ(tk+1, tk) = e
ˆΩkTs
(I4 + δΩkTs + ...). (13)
By truncating the series in Eq.(13) after the first order term, it is possible to approximate Eq.(10) by
qk+1 ≈ e
ˆΩkTs
qk + e
ˆΩkTs
δΩkTsqk. (14)
Manipulating the second term in the right-hand side of Eq.(14), which is the state noise, one can obtain the discrete-
time state model as follows:
qk+1 = e
ˆΩkTs
qk +
Ts
2
e
ˆΩkTs
Ξkδωk, (15)
where
Ξk
−ek
[ek×] + q1,kI3
. (16)
Let Γk be defined by
Γk =
Ts
2
e
ˆΩkTs ˆΞk, (17)
where ˆΞk is given by Eq.(16), but computed using ˆqk|k. The state noise covariance is approximated as follows:
Qq
k = ΓkQΓk. (18)
The discrete-time nonlinear measurement model is now described in quaternion as follows:
ˆb
(i)
k = D(qk)r
(i)
k + δb
(i)
k , (19)
where
D(qk) = (q2
1,k − |ek|2
)I3 + 2ekek − 2q1,k[ek×]. (20)
The QEKF requires the Jacobian matrix of the nonlinear measurement model of Eq.(20), which is defined as
H
(i)
q,k+1
∂D(q)r
(i)
k+1
∂q
|q=ˆqk+1|k
= ∂D(q)
∂q1
r
(i)
k+1
∂D(q)
∂q2
r
(i)
k+1
∂D(q)
∂q3
r
(i)
k+1
∂D(q)
∂q4
r
(i)
k+1
q=ˆqk+1|k
, (21)
where the partial derivatives are given by
∂D(q)
∂q1
|q=ˆqk+1|k
= 2


ˆq1 ˆq4 −ˆq3
−ˆq4 ˆq1 ˆq2
ˆq3 −ˆq2 ˆq1


k+1|k
, (22)
∂D(q)
∂q2
|q=ˆqk+1|k
= 2


ˆq2 ˆq3 ˆq4
ˆq3 −ˆq2 ˆq1
ˆq4 −ˆq1 −ˆq2


k+1|k
, (23)
22nd International Congress of Mechanical Engineering (COBEM 2013)
November 3-7, 2013, Ribeirão Preto, SP, Brazil
∂D(q)
∂q3
|q=ˆqk+1|k
= 2


−ˆq3 ˆq2 −ˆq1
ˆq2 ˆq3 ˆq4
ˆq1 ˆq4 −ˆq3


k+1|k
, (24)
∂D(q)
∂q4
|q=ˆqk+1|k
= 2


−ˆq4 ˆq1 ˆq2
−ˆq1 −ˆq4 ˆq3
ˆq2 ˆq3 ˆq4


k+1|k
. (25)
The QEKF consists of a discrete-time formulation of the extended Kalman filter (Bar-Shalom and Li, 1993) applied to
the system modeled by the state equation (15) and the measurement equation (19). In order to force an unit norm property,
an Euclidean normalization is carried out at each filter iteration, after the measurement update. For simplicity, the error
covariance of the normalized estimate is approximated by P∗
k+1|k+1 = Pk+1|k+1.
The QEKF algorithm is summarized as follows:
Initial conditions
ˆq∗
0|0 = ˆq0
P∗
0|0 = Pq
0
State propagation
ˆqk+1|k = e
ˆΩkTs ˆq∗
k|k
Pk+1|k = (e
ˆΩkTs
)P∗
k|k(e
ˆΩkTs
) + Qq
k
Measurement prediction
(ˆb
(i1)
k+1|k) (ˆb
(i2)
k+1|k) = D(ˆqk+1|k)(r
(i1)
k+1) D(ˆqk+1|k)(r
(i2)
k+1)
Pb
k+1|k = Hq,k+1Pk+1|kHq,k+1 + Rk+1
Update
Kk+1 = Pk+1|kHq,k+1(Pb
k+1|k)−1
ˆqk+1|k+1 = ˆqk+1|k + Kk+1 (b
(i1)
k+1) (b
(i2)
k+1) − (ˆb
(i1)
k+1|k) (ˆb
(i2)
k+1|k)
Pk+1|k+1 = Pk+1|k − Kk+1Pb
k+1|kKk+1
Normalization
ˆq∗
k+1|k+1 =
ˆqk+1|k+1
||ˆqk+1|k+1||
P∗
k+1|k+1 = Pk+1|k+1
Note that
Hq,k+1 = H
(i1)
q,k+1 H
(i2)
q,k+1
, (26)
and
Rk+1 =
R
(i1)
b,k+1 03×3
03×3 R
(i2)
b,k+1
. (27)
The estate transition matrix is solved by [(Wertz, 1978), pp.567]
e
ˆΩkTs
= cos( ˆωk
Ts
2
)I4 +
1
ˆωk
sin( ˆωk
Ts
2
)ˆΩk. (28)
Gonçalves, Brusnicki and Santos
Attitude Determination of Multirotors Using Camera Vector Measurements
3.2 Multiplicative extended Kalman filter - MEKF
Markley and Crassidis (1996) proposed a continuous/discrete-time filter which represents the true attitude quaternion
by
q(t) = δq(p(t)) ⊗ ˆq(t), (29)
where ˆq(t) is a reference quaternion, δq(p(t)) is the multiplicative error quaternion parameterized by modified Rodrigues
parameters p(t), and ⊗ denotes the quaternion product (Shuster, 1993). The reference quaternion ˆq(t) is considered
the best estimate of the true quaternion q(t) between the interval [tk, tk+1). Thus, the MRP assumes p(t) = 0 for
t ∈ [tk, tk+1), which eliminates the redundancy of two paramerizations use.
Let Eq.(4) be redefined by the MRP p(t) p1,t p2,t p3,t as follows:
˙p(t) = f(p(t), ω(t)). (30)
The nonlinear function f(p(t), ω(t)) is defined by
f(p(t), ω(t)) = G(p(t))ω(t), (31)
where (Schaub, 1998)
G(p(t)) =
1
4
(1 − ||p(t)||2
)I3 + 2[p(t)×] + 2p(t)p(t) . (32)
Applying Eq.(5) in Eq.(31), and the result in Eq.(30), yields in the state model as follows:
˙p(t) = G(p(t))ˆω(t) + G(p(t))δω(t), (33)
where the second term in the right-hand side of Eq.(33) is the state noise. Its covariance is approximated as:
Qp
(t) = Γ(t)QΓ(t) , (34)
where Γ(t) = G(ˆp(t)), ∀t ∈ [tk, tk+1).
The MEKF requires the Jacobian matrix of Eq.(33), as follows:
F(ˆp(t), ˆω(t))
∂G(p(t))ω(t)
∂p
|p=ˆp(t). (35)
Assuming null MRP for [tk, tk+1), Eq.(35) results in
F(ˆp(t), ˆω(t)) =
1
2
(−[ˆω×]). (36)
Let discrete-time measurement model be defined in MRP by
ˆb
(i)
k = D(pk)r
(i)
k + δb
(i)
k , (37)
where (Shuster, 1993)
D(pk) = I3 +
8[pk×]2
− 4(1 − pk
2
)[pk×]
(1 + pk
2)2
, (38)
is the attitude matrix in MRP. In order to obtain a linear model of Eq.(37), first order Taylor expansion is applied. The
Jacobian of this operation is given by
22nd International Congress of Mechanical Engineering (COBEM 2013)
November 3-7, 2013, Ribeirão Preto, SP, Brazil
H
(i)
p,k+1
∂D(p)r
(i)
k+1
∂p
|p=ˆpk+1|k
= ∂D(p)
∂p1
r
(i)
k+1
∂D(p)
∂p2
r
(i)
k+1
∂D(p)
∂p3
r
(i)
k+1
p=ˆpk+1|k
, (39)
where the partial derivatives, assuming null MRP ∀t ∈ [tk, tk+1), yield
∂D(p)
∂p1
|p=ˆpk+1|k
=


0 0 0
0 0 4
0 −4 0

 , (40)
∂D(p)
∂p2
|p=ˆpk+1|k
=


0 0 −4
0 0 0
4 0 0

 , (41)
∂D(p)
∂p3
|p=ˆpk+1|k
=


0 4 0
−4 0 0
0 0 0

 . (42)
By means of the continuous-discrete EKF, both state model given by Eq.(33) and measurement model given by Eq.(37)
are fused in order to estimate the attitude error in MRP. The global nonsingular attitude is propagated in quaternion by
Eq.(15) in the interval [tk, tk+1). The update of the global attitude is given by the discrete-time version of Eq.(29), where
δq(pk+1|k+1) =







1−||pk+1|k+1||2
1+||pk+1|k+1||2
2p1,k+1|k+1
1+||pk+1|k+1||2
2p2,k+1|k+1
1+||pk+1|k+1||2
2p3,k+1|k+1
1+||pk+1|k+1||2







. (43)
The MEKF algorithm is summarized as follows:
Initial conditions
ˆq0|0 = ˆq0
Pp
0|0 = Pp
0
ˆm0|0 = 0
State propagation
ˆp(t) = 0, t ∈ [tk, tk+1)
˙P
p
(t) = F(ˆp(t), ˆω(t))Pp
(t) + Pp
(t)F(ˆp(t), ˆω(t)) + Qp
(t)
ˆqk+1|k = e
ˆΩkTs ˆq∗
k|k
Measurement prediction
(ˆb
(i1)
k+1|k) (ˆb
(i2)
k+1|k) = D(ˆqk+1|k)(r
(i1)
k+1) D(ˆqk+1|k)(r
(i2)
k+1)
Pb
k+1|k = Hp,k+1Pp
k+1|kHp,k+1 + Rk+1
Update
Kk+1 = Pp
k+1|kHp,k+1(Pb
k+1|k)−1
ˆpk+1|k+1 = Kk+1 (b
(i1)
k+1) (b
(i2)
k+1) − (ˆb
(i1)
k+1|k) (ˆb
(i2)
k+1|k)
Pp
k+1|k+1 = Pp
k+1|k − Kk+1Pb
k+1|kKk+1
Gonçalves, Brusnicki and Santos
Attitude Determination of Multirotors Using Camera Vector Measurements
ˆqk+1|k+1 = δq(pk+1|k+1) ⊗ ˆqk+1|k
Note that
Hq,k+1 = H
(i1)
q,k+1 H
(i2)
q,k+1
, (44)
and
Rk+1 =
R
(i1)
b,k+1 03×3
03×3 R
(i2)
b,k+1
. (45)
4. SIMULATION AND RESULTS
The performance of both presented estimators will be compared using simulated data. The multirotor true attitude qk
is propagated by Eq.(15) with the following angular velocity:
ωk =


0.1 sin(kTs)
0.1 cos(kTs)
−0.1 sin(kTs) cos(kTs)

 , (46)
where Ts = 0.1s. The camera vector measurements were generated using Eq.(2), where
r
(1)
k =


0
5
13
−12
13

 , (47)
r
(2)
k =


0
− 5
13
−12
13

 . (48)
Both rate-gyro and camera noise covariances were tuned in order to not diverge the filter estimate. Table 1 shows the
assumed measurement noise covariances.
Table 1: Measurement noise covariances.
Sensor Covariance
Rate-gyro Qk = (0.005)2
I3 (rad/s)2
Camera R
(i)
b,k = (0.01)2
I3
Using the simulated measurements, both QEKF and MEKF were submitted to one hundred Monte-Carlo runs with
1000s of duration each. The integration for MEKF is given by fourth order Runge-Kutta. The initial conditions assumed
are shown in Tab.2.
Table 2: Initial conditions.
Parameter QEKF MEKF
True attitude q0 ∼ N 1 0 0 0 , P0|0 q0 ∼ N 1 0 0 0 , P0|0
State ˆq0|0 = 1 0 0 0 ˆq0|0 = 1 0 0 0 , ˆp0|0 = 0 0 0
Covariance P0|0 = 0.01I4 Pp
0|0 = 0.01I3
Accuracy, orthogonality and relative computational burden are the parameters to be examined. The accuracy is mea-
sured as follows (Wertz, 1978):
Ik = |acos
1
2
tr D(ˆqk|k) D(qk) − 1 |, (49)
22nd International Congress of Mechanical Engineering (COBEM 2013)
November 3-7, 2013, Ribeirão Preto, SP, Brazil
(a) Zoom in 0-200 seconds (b) 0-1000 seconds
Figure 2: MEKF angular error in degree
(a) Zoom in 0-200 seconds (b) 0-1000 seconds
Figure 3: QEKF angular error in degree
where the index Ik corresponds to the error angle between the true and the estimate attitudes in the Euler principal angle
notation. The orthogonality is given by
Jk = tr D(ˆqk|k) D(ˆqk|k) − I3 D(ˆqk|k) D(ˆqk|k) − I3 (50)
where the index Jk describes how close the estimate attitude matrix is to the orthogonal matrix, as it gets closer to zero.
Since the CPU performs tasks parallel to the simulation, it is not possible to use the cycle time for measure an absolute
computational burden of each filter. Rather, the cycle time is used to measure how fast is one algorithm relative to the
other.
Defined the simulation conditions, the mean and the standard deviation values of both indexes Ik and Jk are calculated.
Figure (2) shows the MEKF mean accuracy index between 0.4 and 0.6 degrees, while Fig.(3) presents same index for
QEKF approximately equal to 0.2 degrees. For this simulation conditions, the QEKF shows better accuracy than the
MEKF. From Figs. (5) and (4), one can conclude that the QEKF attitude matrix is closer to the orthogonal matrix than
the MEKF one. The QEKF spent an average of 0.115ms per cycle while the MEKF spent 0.152ms, resulting in 24.34%
more time consumption for the MEKF over the QEKF.
Gonçalves, Brusnicki and Santos
Attitude Determination of Multirotors Using Camera Vector Measurements
Figure 4: QEKF orthogonality Index Figure 5: MEKF orthogonality Index
5. CONCLUSION
Two attitude determination methods based on camera vector measurements were presented. The quaternion extended
Kalman filter performed better than the multiplicative extended Kalman filter for the proposed simulation scheme. How-
ever, MEKF does not need a normalization step after the state update. These methods are suitable for indoor environments,
since they do not use GPS. An alternative upgrade for outdoor flight is to use gravity direction and geomagnetic field vector
along with camera vector measurements. An experimental flight test is being prepared in order to evaluate the embedded
computational burden.
6. ACKNOWLEDGEMENTS
The first author is plenty thankful to be supported by Fundação de Amparo à Pesquisa do Estado do Amazonas -
FAPEAM.
7. REFERENCES
Bar-Itzhack, I.Y. and Oshman, Y., 1985. “Attitude determination from vector observations: Quaternion estimation”. IEEE
Transactions on Aerospace and Electronic Systems, Vol. 21, pp. 128–136.
Bar-Shalom, Y. and Li, X.L., 1993. Estimation and Tracking – Principles, Techniques and Software. Artech House,
Norwood.
Cheng, L., Zhaoying, Z. and Xu, F., 2008. “Attitude determination for MAVs using a kalman filter”. Tsinghua Science
and Technology, Vol. 13, pp. 593–597.
Gebre-Egziabher, D. and Elkaim, G.H., 2008. “MAV attitude determination by vector matching”. IEEE Transactions on
Aerospace and Electronic Systems, Vol. 44, pp. 1012–1028.
Idan, M., 1996. “Estimation of rodrigues parameters from vector observations”. IEEE Transactions on Aerospace and
Electronic Systems, Vol. 32, pp. 578–586.
Markley, F.L., 1988. “Attitude determination using vector observations and the singular value decomposition”. Journal
of the Astronautical Sciences, Vol. 38(3), pp. 245–258.
Markley, F.L. and Crassidis, J.L., 1996. “Attitude estimation using modified rodrigues parameters”. In Proceedings of the
Flight Mechanics/Estimation Theory Symposium. Greenbelt, USA.
Schaub, H., 1998. Novel coordinates for nonlinear multibody motion with applications to spacecraft dynamics and
control. Ph.D. thesis, Texas A M University, Texas.
Shuster, M.D., 1993. “A survey of attitude representations”. Journal of the Astronautical Sciences, Vol. 41, pp. 439–517.
Wertz, J.R., 1978. Spacecraft Attitude Determination and control. Kluwer Academic Publishers, The Netherlands.
Yang, Y., 2012. “Spacecraft attitude determination and control: Quaternion based method”. Annual Reviews in Control,
Vol. 36, pp. 198–219.
The authors are the only responsible for the printed material included in this paper.

More Related Content

PDF
A Study of Non-Gaussian Error Volumes and Nonlinear Uncertainty Propagation f...
PDF
Inversão com sigmoides
PDF
Three-dimensional structure from motion recovery of a moving object with nois...
PDF
4 hydrology geostatistics-part_2
PDF
2006 Green Incorporating Pulse to Pulse Motion Effects anto Side Looking Arra...
PDF
GPS cycle slips detection and repair through various signal combinations
PDF
SLAM of Multi-Robot System Considering Its Network Topology
PDF
response spectra
A Study of Non-Gaussian Error Volumes and Nonlinear Uncertainty Propagation f...
Inversão com sigmoides
Three-dimensional structure from motion recovery of a moving object with nois...
4 hydrology geostatistics-part_2
2006 Green Incorporating Pulse to Pulse Motion Effects anto Side Looking Arra...
GPS cycle slips detection and repair through various signal combinations
SLAM of Multi-Robot System Considering Its Network Topology
response spectra

What's hot (18)

PDF
Action Trajectory Reconstruction for Controlling of Vehicle Using Sensors
PDF
Photogrammetry - Space Resection by Collinearity Equations
PDF
Gaussian Orbital Determination of 1943 Anteros
PDF
Smear correction of highly variable,
PDF
Verification of gravimetric geoidal models by a combination of gps and orthom...
PDF
Mapping spiral structure on the far side of the Milky Way
PDF
COMPLEMENTARY VISION BASED DATA FUSION FOR ROBUST POSITIONING AND DIRECTED FL...
PDF
Precise Attitude Determination Using a Hexagonal GPS Platform
PDF
E043036045
PPTX
Indoor Heading Estimation
PDF
Basics1variogram
PDF
GLOBAL TRAJECTORY OPTIMISATION OF A SPACE-BASED VERY-LONG-BASELINE INTERFEROM...
PDF
Mixed signal approach
PDF
20320130406015 2-3-4
PDF
Assessment of the Usefulness of Egnos Differential Corrections in Conducting ...
PDF
Electrical Engineering Sample Assignment
PDF
Spectroscopic confirmation of_the_existence_of_large_diffuse_galaxies_in_the_...
PDF
Conference poster : Morphometry of galaxies: Asymmetry
Action Trajectory Reconstruction for Controlling of Vehicle Using Sensors
Photogrammetry - Space Resection by Collinearity Equations
Gaussian Orbital Determination of 1943 Anteros
Smear correction of highly variable,
Verification of gravimetric geoidal models by a combination of gps and orthom...
Mapping spiral structure on the far side of the Milky Way
COMPLEMENTARY VISION BASED DATA FUSION FOR ROBUST POSITIONING AND DIRECTED FL...
Precise Attitude Determination Using a Hexagonal GPS Platform
E043036045
Indoor Heading Estimation
Basics1variogram
GLOBAL TRAJECTORY OPTIMISATION OF A SPACE-BASED VERY-LONG-BASELINE INTERFEROM...
Mixed signal approach
20320130406015 2-3-4
Assessment of the Usefulness of Egnos Differential Corrections in Conducting ...
Electrical Engineering Sample Assignment
Spectroscopic confirmation of_the_existence_of_large_diffuse_galaxies_in_the_...
Conference poster : Morphometry of galaxies: Asymmetry
Ad

Similar to Attitude determination of multirotors using camera (15)

PDF
Development of an Integrated Attitude Determination System for Small Unmanned...
PDF
dinamica_e_controle_cap_4_hall_attde.pdf
PDF
FinalReport
PDF
Sensor Fusion Algorithm by Complementary Filter for Attitude Estimation of Qu...
PDF
(Research Note) Model-Aided Monocular Visual-Inertial State Estimation and De...
PDF
Detection of Sensor Faults in Small Helicopter UAVs Using Observer/Kalman Fil...
PDF
UAV como controlar cin PID
PDF
NASA_OSSI_NandaS_ShockleyL_Final
PDF
Robot Pose Estimation: A Vertical Stereo Pair Versus a Horizontal One
PDF
Attitude Control of Quadrotor Using PD Plus Feedforward Controller on SO(3)
PPT
PDF
Jung.Rapport
PDF
Visual Odomtery(2)
PDF
Inertial Sensor Array Calibration Made Easy !
Development of an Integrated Attitude Determination System for Small Unmanned...
dinamica_e_controle_cap_4_hall_attde.pdf
FinalReport
Sensor Fusion Algorithm by Complementary Filter for Attitude Estimation of Qu...
(Research Note) Model-Aided Monocular Visual-Inertial State Estimation and De...
Detection of Sensor Faults in Small Helicopter UAVs Using Observer/Kalman Fil...
UAV como controlar cin PID
NASA_OSSI_NandaS_ShockleyL_Final
Robot Pose Estimation: A Vertical Stereo Pair Versus a Horizontal One
Attitude Control of Quadrotor Using PD Plus Feedforward Controller on SO(3)
Jung.Rapport
Visual Odomtery(2)
Inertial Sensor Array Calibration Made Easy !
Ad

Recently uploaded (20)

PDF
August Patch Tuesday
PDF
A comparative analysis of optical character recognition models for extracting...
PDF
Getting Started with Data Integration: FME Form 101
PPTX
Digital-Transformation-Roadmap-for-Companies.pptx
PDF
DASA ADMISSION 2024_FirstRound_FirstRank_LastRank.pdf
PDF
Univ-Connecticut-ChatGPT-Presentaion.pdf
PPTX
Group 1 Presentation -Planning and Decision Making .pptx
PPTX
OMC Textile Division Presentation 2021.pptx
PDF
Assigned Numbers - 2025 - Bluetooth® Document
PDF
Approach and Philosophy of On baking technology
PPTX
A Presentation on Touch Screen Technology
PDF
NewMind AI Weekly Chronicles - August'25-Week II
PDF
Hybrid model detection and classification of lung cancer
PDF
Building Integrated photovoltaic BIPV_UPV.pdf
PPTX
cloud_computing_Infrastucture_as_cloud_p
PDF
DP Operators-handbook-extract for the Mautical Institute
PDF
From MVP to Full-Scale Product A Startup’s Software Journey.pdf
PPTX
Tartificialntelligence_presentation.pptx
PPTX
Chapter 5: Probability Theory and Statistics
PDF
Encapsulation theory and applications.pdf
August Patch Tuesday
A comparative analysis of optical character recognition models for extracting...
Getting Started with Data Integration: FME Form 101
Digital-Transformation-Roadmap-for-Companies.pptx
DASA ADMISSION 2024_FirstRound_FirstRank_LastRank.pdf
Univ-Connecticut-ChatGPT-Presentaion.pdf
Group 1 Presentation -Planning and Decision Making .pptx
OMC Textile Division Presentation 2021.pptx
Assigned Numbers - 2025 - Bluetooth® Document
Approach and Philosophy of On baking technology
A Presentation on Touch Screen Technology
NewMind AI Weekly Chronicles - August'25-Week II
Hybrid model detection and classification of lung cancer
Building Integrated photovoltaic BIPV_UPV.pdf
cloud_computing_Infrastucture_as_cloud_p
DP Operators-handbook-extract for the Mautical Institute
From MVP to Full-Scale Product A Startup’s Software Journey.pdf
Tartificialntelligence_presentation.pptx
Chapter 5: Probability Theory and Statistics
Encapsulation theory and applications.pdf

Attitude determination of multirotors using camera

  • 1. 22nd International Congress of Mechanical Engineering (COBEM 2013) November 3-7, 2013, Ribeirão Preto, SP, Brazil Copyright c 2013 by ABCM ATTITUDE DETERMINATION OF MULTIROTORS USING CAMERA VECTOR MEASUREMENTS Pedro Filizola Sousa Maia Gonçalves Roberto Brusnicki Davi Antônio dos Santos Instituto Tecnológico de Aeronáutica, Praça Marechal Eduardo Gomes, 50, Vila das Acácias, 12228-900, São José dos Campos, SP, Brazil pedrofsm@ita.br, rbrusnicki@gmail.com, davists@ita.br Abstract. The employment of camera in low-cost navigation and guidance of multirotor unmanned aerial vehicles (UAV) has recently been the focus of many investigations. Nevertheless, in the previous works, camera measurements was adopted either to aid in the position/velocity estimation or to directly provide feedback for guidance, but not specifically for assisting in the attitude determination process. This work is concerned with the attitude determination of multirotor UAVs using vector measurements taken from a camera. The vehicle is assumed to be equipped with an altimeter, a triad of rate-gyros, and a downward-facing strapdown camera. It is assumed to fly in an indoor environment containing various landmarks placed in known positions on the floor. The quantity and positions of the landmarks are chosen in such a way that at least two of them always remain in the camera field of view. Therefore, at each time instant, two noncollinear unit vectors directed from the camera to the center of area of the landmarks can be computed. In order to carry out attitude determination, two quaternion estimation methods are adopted: the multiplicative extended Kalman filter (MEKF) and the quaternion extended Kalman filter (QEKF). The proposed multirotor attitude determination scheme is evaluated by computational simulations. Keywords: aerial robotics, attitude determination, Kalman filtering, computer vision 1. INTRODUCTION The attitude determination (AD) is a fundamental part of any control system for unmanned aerial vehicles (UAV). In general, it is concerned with the estimation of the vehicle’s attitude and angular velocity with respect to a given reference coordinate system. The estimates computed by the AD function is then used to provide the attitude control laws with feedback information. The literature on AD is very extensive and has mainly been developed in the aerospace field (Wertz, 1978), (Yang, 2012). The AD methods stems from the Wabba Problem (Markley, 1988), which defined a framework to estimate attitude from vector measurements. (Cheng et al., 2008) uses the extended Kalman filter (EKF) to estimate pitch and roll angles of a Micro Aerial Vehicle (MAV). The third column of the Direction Cosine Matrix (DCM) and the rate gyro bias are used as state variables. Gravity is used as the observation vector in the measurement model. The yaw angle is obtained from geomagnetic field vector. Gebre-Egziabher and Elkaim (2008) use both gravity and geomagnetic field as observation vectors in two different approaches to estimate the attitude quaternion. The first approach is an iterated least-square estimator (LSE) and the second is an EKF. The LSE executes a global search of the attitude at each time step. On the contrary, the EKF algorithm accounts for a priori information, resulting in a better performance. The above two methods were designed to be gyro-free and GPS assisted. (Bar-Itzhack and Oshman, 1985) proposes a quaternion extended Kalman filter (QEKF), which, to ensure estimates with unit norm, realizes an Euclidian normalization step after each measurement update. (Idan, 1996) proposes a minimum-variance filter to estimate attitude parameterized by Rodrigues parameters. Due to simpler algebraic expressions, this approach has a relative computational advantage over the quaternion estimators. (Markley and Crassidis, 1996) presents a multiplicative extended Kalman filter (MEKF) that estimates an attitude error in MRP and updates the total attitude represented by quaternion by means of quaternion multiplication. In satellite AD methods, vector measurements are typically taken from solar sensors (Sun direction), magnetometers (local geomagnetic field vector), horizon sensors (direction of nadir), star sensors (direction of stars) (Wertz, 1978). On the other hand, the multirotor UAV literature usually relies only on two vector measurements taken, respectively, from accelerometers (local vertical) and magnetometers. This work presents a multirotor UAV attitude determination method using vector measurements taken from images. It is assumed that the vehicle is equipped with three strapdown sensors: a downward-facing camera, a triad of rate-gyros and an altimeter. The vehicle is assumed to fly indoors over a flat ground with various landmarks. Both vehicle and landmarks have known positions with respect to the adopted reference coordinate system. The landmarks are disposed, in quantity and positions, in such a way that at least two of them always remain in the camera field of view (FOV). Using measure- ments taken from the camera and the altimeter, two noncollinear vector measurements pointing from the camera to the landmarks’ centers can be computed. In order to obtain a scheme for attitude determination of multirotor UAVs, these
  • 2. Gonçalves, Brusnicki and Santos Attitude Determination of Multirotors Using Camera Vector Measurements vector measurements as well as rate-gyro data are considered in two attitude estimation methods: the QEKF (Bar-Itzhack and Oshman, 1985) and the MEKF (Markley and Crassidis, 1996). The proposed scheme is evaluated by computational simulation. The remaining text is organized in the following manner. Section II defines the paper problem. Section III reformulates the attitude estimation methods. Section IV presents some simulation results. Finally, Section V presents the paper’s conclusions. Notation. IN is the N × N identity matrix, [•×] denotes the cross product matrix and • defines the matrix transpose. 2. PROBLEM STATEMENT Consider the multirotor helicopter and the three Cartesian coordinate systems (CCS) illustrated in Fig. 1. The body CCS SB = {XB, YB, ZB} is attached to the vehicle at its center of mass (CM). The ground CCS SG = {XG, YG, ZG} is fixed on the ground at point O. The reference CCS SR = {XR, YR, ZR} is parallel to SG but is centered at CM. 𝑋B 𝑌B 𝑍B 𝑂 𝑍G 𝑋G 𝑌G 𝑋R 𝑌R 𝑍R 𝑓3 𝑓2 𝑓1 𝑓4 𝐬(1) 𝐬(2) 𝑀(1) 𝑀(2) 𝑀(𝑙) 𝐶𝑀 𝑀(3) Figure 1: The Cartesian coordinate systems and the flight environment. Assume that the camera is positioned at the CM and the triad of rate-gyros is aligned with SB. Define the set of landmark indexes to be I {1, 2, ..., l}. Denote the center of the i-th landmark by M(i) . Define s (i) to be the unit geometric vector pointing from CM to M(i) . Denote the representations of s (i) in SB and SR by b(i) ∈ R3 and r(i) ∈ R3 , respectively. The representations b(i) and r(i) are interrelated by b(i) = Dr(i) , where D ∈ SO(3) is the attitude matrix of SB with respect to SR. In order to measure two noncollinear pairs (b(i) , r(i) ), one assumes that both CM and landmarks have known positions and, moreover, at least two landmarks are measured by the camera at each sample instant. This yields the following two pairs of vector measurements: Vk ˆb (i1) k ,ˆr (i1) k , ˆb (i2) k ,ˆr (i2) k , i1 ∈ I, i2 ∈ I, i1 = i2, (1) where k denotes the discrete-time instant and, for i = i1, i2, ˆb (i) k = D(ak)r (i) k + δb (i) k , (2) r (i) k = ˆr (i) k + δr (i) k , (3) where ˆr (i) k is a sample of r(i) at instant k, δb (i) k and δr (i) k are zero-mean Gaussian white sequences with covariances R (i) b,k
  • 3. 22nd International Congress of Mechanical Engineering (COBEM 2013) November 3-7, 2013, Ribeirão Preto, SP, Brazil and R (i) r,k, respectively, and ak ∈ Rn is a discrete-time attitude representation vector which parameterizes the attitude matrix D(ak). Let the attitude kinematics be modeled by the differential equation (Wertz, 1978) ˙a(t) = f(a(t), ω(t)), (4) where a(t) is a continuous-time version of ak, ω(t) ∈ R3 is the true angular velocity. Since the rate-gyros are not perfect, the true angular velocity is given by the following stochastic model: ω(t) = ˆω(t) + δω(t), (5) where ˆω(t) ∈ R3 is the measured angular velocity and δω(t) ∈ R3 is the rate-gyro measurement noise, which is assumed to be a zero-mean Gaussian white sequence with covariance Q. A discrete-time version of Eq.(5) is given by ωk = ˆωk + δωk, where δωk has the same characteristics of δω(t). The main problem of the paper is to recursively compute the minimum-variance (MV) estimate ˆak|k of the true attitude vector ak using the dynamic equation (4), the sequence of angular velocity measurements ˆω1:k, and sequence of vector measurements V1:k. 3. PROBLEM SOLUTION This section presents two estimation methods to face the afore-defined problem: The Quaternion Extended Kalman Filter (QEKF) (Bar-Itzhack and Oshman, 1985) and the Multiplicative Extended Kalman Filter (MEKF) (Markley and Crassidis, 1996). 3.1 Quaternion extended Kalman filter - QEKF Bar-Itzhack and Oshman (1985) proposed a discrete-time extended Kalman filter to estimate the attitude quaternion. This method is described in the sequel. Let the vector a(t) assumes the form of the attitude quaternion q(t) q1,t et , (6) subject to the unit norm constrain q(t) = q2 1,t + et = 1, (7) where q1,t and et are, respectively, the scalar and the complex part of the attitude quaternion. This gives rise to the following attitude kinematic equation (Wertz, 1978): ˙q(t) = Ω(t)q(t), (8) where Ω(t) = 1 2 0 −ω(t) ω(t) −[ω(t)×] . (9) Integrating Eq.(9) from tk to tk+1, yields qk+1 = Φ(tk+1, tk)qk, (10) where Φ(tk+1, tk) ∈ R4×4 is the state transition matrix. Define Ts (tk+1 − tk) as the sampling time. Assuming constant angular velocity ω(t) during the interval Ts, the state transition matrix can be written as Φ(tk+1, tk) = eΩkTs , (11) where Ωk has the same form of Eq.(9), however it is computed using ωk. Rewriting Eq.(11) by using the discrete-time version of Eq.(5), results
  • 4. Gonçalves, Brusnicki and Santos Attitude Determination of Multirotors Using Camera Vector Measurements Φ(tk+1, tk) = e ˆΩkTs eδΩkTs , (12) where ˆΩk and δΩk are given by Eq.(9), but computed using ˆωk and δωk, respectively. The second factor at the right-hand side of Eq.(12) is expanded in power series, yielding Φ(tk+1, tk) = e ˆΩkTs (I4 + δΩkTs + ...). (13) By truncating the series in Eq.(13) after the first order term, it is possible to approximate Eq.(10) by qk+1 ≈ e ˆΩkTs qk + e ˆΩkTs δΩkTsqk. (14) Manipulating the second term in the right-hand side of Eq.(14), which is the state noise, one can obtain the discrete- time state model as follows: qk+1 = e ˆΩkTs qk + Ts 2 e ˆΩkTs Ξkδωk, (15) where Ξk −ek [ek×] + q1,kI3 . (16) Let Γk be defined by Γk = Ts 2 e ˆΩkTs ˆΞk, (17) where ˆΞk is given by Eq.(16), but computed using ˆqk|k. The state noise covariance is approximated as follows: Qq k = ΓkQΓk. (18) The discrete-time nonlinear measurement model is now described in quaternion as follows: ˆb (i) k = D(qk)r (i) k + δb (i) k , (19) where D(qk) = (q2 1,k − |ek|2 )I3 + 2ekek − 2q1,k[ek×]. (20) The QEKF requires the Jacobian matrix of the nonlinear measurement model of Eq.(20), which is defined as H (i) q,k+1 ∂D(q)r (i) k+1 ∂q |q=ˆqk+1|k = ∂D(q) ∂q1 r (i) k+1 ∂D(q) ∂q2 r (i) k+1 ∂D(q) ∂q3 r (i) k+1 ∂D(q) ∂q4 r (i) k+1 q=ˆqk+1|k , (21) where the partial derivatives are given by ∂D(q) ∂q1 |q=ˆqk+1|k = 2   ˆq1 ˆq4 −ˆq3 −ˆq4 ˆq1 ˆq2 ˆq3 −ˆq2 ˆq1   k+1|k , (22) ∂D(q) ∂q2 |q=ˆqk+1|k = 2   ˆq2 ˆq3 ˆq4 ˆq3 −ˆq2 ˆq1 ˆq4 −ˆq1 −ˆq2   k+1|k , (23)
  • 5. 22nd International Congress of Mechanical Engineering (COBEM 2013) November 3-7, 2013, Ribeirão Preto, SP, Brazil ∂D(q) ∂q3 |q=ˆqk+1|k = 2   −ˆq3 ˆq2 −ˆq1 ˆq2 ˆq3 ˆq4 ˆq1 ˆq4 −ˆq3   k+1|k , (24) ∂D(q) ∂q4 |q=ˆqk+1|k = 2   −ˆq4 ˆq1 ˆq2 −ˆq1 −ˆq4 ˆq3 ˆq2 ˆq3 ˆq4   k+1|k . (25) The QEKF consists of a discrete-time formulation of the extended Kalman filter (Bar-Shalom and Li, 1993) applied to the system modeled by the state equation (15) and the measurement equation (19). In order to force an unit norm property, an Euclidean normalization is carried out at each filter iteration, after the measurement update. For simplicity, the error covariance of the normalized estimate is approximated by P∗ k+1|k+1 = Pk+1|k+1. The QEKF algorithm is summarized as follows: Initial conditions ˆq∗ 0|0 = ˆq0 P∗ 0|0 = Pq 0 State propagation ˆqk+1|k = e ˆΩkTs ˆq∗ k|k Pk+1|k = (e ˆΩkTs )P∗ k|k(e ˆΩkTs ) + Qq k Measurement prediction (ˆb (i1) k+1|k) (ˆb (i2) k+1|k) = D(ˆqk+1|k)(r (i1) k+1) D(ˆqk+1|k)(r (i2) k+1) Pb k+1|k = Hq,k+1Pk+1|kHq,k+1 + Rk+1 Update Kk+1 = Pk+1|kHq,k+1(Pb k+1|k)−1 ˆqk+1|k+1 = ˆqk+1|k + Kk+1 (b (i1) k+1) (b (i2) k+1) − (ˆb (i1) k+1|k) (ˆb (i2) k+1|k) Pk+1|k+1 = Pk+1|k − Kk+1Pb k+1|kKk+1 Normalization ˆq∗ k+1|k+1 = ˆqk+1|k+1 ||ˆqk+1|k+1|| P∗ k+1|k+1 = Pk+1|k+1 Note that Hq,k+1 = H (i1) q,k+1 H (i2) q,k+1 , (26) and Rk+1 = R (i1) b,k+1 03×3 03×3 R (i2) b,k+1 . (27) The estate transition matrix is solved by [(Wertz, 1978), pp.567] e ˆΩkTs = cos( ˆωk Ts 2 )I4 + 1 ˆωk sin( ˆωk Ts 2 )ˆΩk. (28)
  • 6. Gonçalves, Brusnicki and Santos Attitude Determination of Multirotors Using Camera Vector Measurements 3.2 Multiplicative extended Kalman filter - MEKF Markley and Crassidis (1996) proposed a continuous/discrete-time filter which represents the true attitude quaternion by q(t) = δq(p(t)) ⊗ ˆq(t), (29) where ˆq(t) is a reference quaternion, δq(p(t)) is the multiplicative error quaternion parameterized by modified Rodrigues parameters p(t), and ⊗ denotes the quaternion product (Shuster, 1993). The reference quaternion ˆq(t) is considered the best estimate of the true quaternion q(t) between the interval [tk, tk+1). Thus, the MRP assumes p(t) = 0 for t ∈ [tk, tk+1), which eliminates the redundancy of two paramerizations use. Let Eq.(4) be redefined by the MRP p(t) p1,t p2,t p3,t as follows: ˙p(t) = f(p(t), ω(t)). (30) The nonlinear function f(p(t), ω(t)) is defined by f(p(t), ω(t)) = G(p(t))ω(t), (31) where (Schaub, 1998) G(p(t)) = 1 4 (1 − ||p(t)||2 )I3 + 2[p(t)×] + 2p(t)p(t) . (32) Applying Eq.(5) in Eq.(31), and the result in Eq.(30), yields in the state model as follows: ˙p(t) = G(p(t))ˆω(t) + G(p(t))δω(t), (33) where the second term in the right-hand side of Eq.(33) is the state noise. Its covariance is approximated as: Qp (t) = Γ(t)QΓ(t) , (34) where Γ(t) = G(ˆp(t)), ∀t ∈ [tk, tk+1). The MEKF requires the Jacobian matrix of Eq.(33), as follows: F(ˆp(t), ˆω(t)) ∂G(p(t))ω(t) ∂p |p=ˆp(t). (35) Assuming null MRP for [tk, tk+1), Eq.(35) results in F(ˆp(t), ˆω(t)) = 1 2 (−[ˆω×]). (36) Let discrete-time measurement model be defined in MRP by ˆb (i) k = D(pk)r (i) k + δb (i) k , (37) where (Shuster, 1993) D(pk) = I3 + 8[pk×]2 − 4(1 − pk 2 )[pk×] (1 + pk 2)2 , (38) is the attitude matrix in MRP. In order to obtain a linear model of Eq.(37), first order Taylor expansion is applied. The Jacobian of this operation is given by
  • 7. 22nd International Congress of Mechanical Engineering (COBEM 2013) November 3-7, 2013, Ribeirão Preto, SP, Brazil H (i) p,k+1 ∂D(p)r (i) k+1 ∂p |p=ˆpk+1|k = ∂D(p) ∂p1 r (i) k+1 ∂D(p) ∂p2 r (i) k+1 ∂D(p) ∂p3 r (i) k+1 p=ˆpk+1|k , (39) where the partial derivatives, assuming null MRP ∀t ∈ [tk, tk+1), yield ∂D(p) ∂p1 |p=ˆpk+1|k =   0 0 0 0 0 4 0 −4 0   , (40) ∂D(p) ∂p2 |p=ˆpk+1|k =   0 0 −4 0 0 0 4 0 0   , (41) ∂D(p) ∂p3 |p=ˆpk+1|k =   0 4 0 −4 0 0 0 0 0   . (42) By means of the continuous-discrete EKF, both state model given by Eq.(33) and measurement model given by Eq.(37) are fused in order to estimate the attitude error in MRP. The global nonsingular attitude is propagated in quaternion by Eq.(15) in the interval [tk, tk+1). The update of the global attitude is given by the discrete-time version of Eq.(29), where δq(pk+1|k+1) =        1−||pk+1|k+1||2 1+||pk+1|k+1||2 2p1,k+1|k+1 1+||pk+1|k+1||2 2p2,k+1|k+1 1+||pk+1|k+1||2 2p3,k+1|k+1 1+||pk+1|k+1||2        . (43) The MEKF algorithm is summarized as follows: Initial conditions ˆq0|0 = ˆq0 Pp 0|0 = Pp 0 ˆm0|0 = 0 State propagation ˆp(t) = 0, t ∈ [tk, tk+1) ˙P p (t) = F(ˆp(t), ˆω(t))Pp (t) + Pp (t)F(ˆp(t), ˆω(t)) + Qp (t) ˆqk+1|k = e ˆΩkTs ˆq∗ k|k Measurement prediction (ˆb (i1) k+1|k) (ˆb (i2) k+1|k) = D(ˆqk+1|k)(r (i1) k+1) D(ˆqk+1|k)(r (i2) k+1) Pb k+1|k = Hp,k+1Pp k+1|kHp,k+1 + Rk+1 Update Kk+1 = Pp k+1|kHp,k+1(Pb k+1|k)−1 ˆpk+1|k+1 = Kk+1 (b (i1) k+1) (b (i2) k+1) − (ˆb (i1) k+1|k) (ˆb (i2) k+1|k) Pp k+1|k+1 = Pp k+1|k − Kk+1Pb k+1|kKk+1
  • 8. Gonçalves, Brusnicki and Santos Attitude Determination of Multirotors Using Camera Vector Measurements ˆqk+1|k+1 = δq(pk+1|k+1) ⊗ ˆqk+1|k Note that Hq,k+1 = H (i1) q,k+1 H (i2) q,k+1 , (44) and Rk+1 = R (i1) b,k+1 03×3 03×3 R (i2) b,k+1 . (45) 4. SIMULATION AND RESULTS The performance of both presented estimators will be compared using simulated data. The multirotor true attitude qk is propagated by Eq.(15) with the following angular velocity: ωk =   0.1 sin(kTs) 0.1 cos(kTs) −0.1 sin(kTs) cos(kTs)   , (46) where Ts = 0.1s. The camera vector measurements were generated using Eq.(2), where r (1) k =   0 5 13 −12 13   , (47) r (2) k =   0 − 5 13 −12 13   . (48) Both rate-gyro and camera noise covariances were tuned in order to not diverge the filter estimate. Table 1 shows the assumed measurement noise covariances. Table 1: Measurement noise covariances. Sensor Covariance Rate-gyro Qk = (0.005)2 I3 (rad/s)2 Camera R (i) b,k = (0.01)2 I3 Using the simulated measurements, both QEKF and MEKF were submitted to one hundred Monte-Carlo runs with 1000s of duration each. The integration for MEKF is given by fourth order Runge-Kutta. The initial conditions assumed are shown in Tab.2. Table 2: Initial conditions. Parameter QEKF MEKF True attitude q0 ∼ N 1 0 0 0 , P0|0 q0 ∼ N 1 0 0 0 , P0|0 State ˆq0|0 = 1 0 0 0 ˆq0|0 = 1 0 0 0 , ˆp0|0 = 0 0 0 Covariance P0|0 = 0.01I4 Pp 0|0 = 0.01I3 Accuracy, orthogonality and relative computational burden are the parameters to be examined. The accuracy is mea- sured as follows (Wertz, 1978): Ik = |acos 1 2 tr D(ˆqk|k) D(qk) − 1 |, (49)
  • 9. 22nd International Congress of Mechanical Engineering (COBEM 2013) November 3-7, 2013, Ribeirão Preto, SP, Brazil (a) Zoom in 0-200 seconds (b) 0-1000 seconds Figure 2: MEKF angular error in degree (a) Zoom in 0-200 seconds (b) 0-1000 seconds Figure 3: QEKF angular error in degree where the index Ik corresponds to the error angle between the true and the estimate attitudes in the Euler principal angle notation. The orthogonality is given by Jk = tr D(ˆqk|k) D(ˆqk|k) − I3 D(ˆqk|k) D(ˆqk|k) − I3 (50) where the index Jk describes how close the estimate attitude matrix is to the orthogonal matrix, as it gets closer to zero. Since the CPU performs tasks parallel to the simulation, it is not possible to use the cycle time for measure an absolute computational burden of each filter. Rather, the cycle time is used to measure how fast is one algorithm relative to the other. Defined the simulation conditions, the mean and the standard deviation values of both indexes Ik and Jk are calculated. Figure (2) shows the MEKF mean accuracy index between 0.4 and 0.6 degrees, while Fig.(3) presents same index for QEKF approximately equal to 0.2 degrees. For this simulation conditions, the QEKF shows better accuracy than the MEKF. From Figs. (5) and (4), one can conclude that the QEKF attitude matrix is closer to the orthogonal matrix than the MEKF one. The QEKF spent an average of 0.115ms per cycle while the MEKF spent 0.152ms, resulting in 24.34% more time consumption for the MEKF over the QEKF.
  • 10. Gonçalves, Brusnicki and Santos Attitude Determination of Multirotors Using Camera Vector Measurements Figure 4: QEKF orthogonality Index Figure 5: MEKF orthogonality Index 5. CONCLUSION Two attitude determination methods based on camera vector measurements were presented. The quaternion extended Kalman filter performed better than the multiplicative extended Kalman filter for the proposed simulation scheme. How- ever, MEKF does not need a normalization step after the state update. These methods are suitable for indoor environments, since they do not use GPS. An alternative upgrade for outdoor flight is to use gravity direction and geomagnetic field vector along with camera vector measurements. An experimental flight test is being prepared in order to evaluate the embedded computational burden. 6. ACKNOWLEDGEMENTS The first author is plenty thankful to be supported by Fundação de Amparo à Pesquisa do Estado do Amazonas - FAPEAM. 7. REFERENCES Bar-Itzhack, I.Y. and Oshman, Y., 1985. “Attitude determination from vector observations: Quaternion estimation”. IEEE Transactions on Aerospace and Electronic Systems, Vol. 21, pp. 128–136. Bar-Shalom, Y. and Li, X.L., 1993. Estimation and Tracking – Principles, Techniques and Software. Artech House, Norwood. Cheng, L., Zhaoying, Z. and Xu, F., 2008. “Attitude determination for MAVs using a kalman filter”. Tsinghua Science and Technology, Vol. 13, pp. 593–597. Gebre-Egziabher, D. and Elkaim, G.H., 2008. “MAV attitude determination by vector matching”. IEEE Transactions on Aerospace and Electronic Systems, Vol. 44, pp. 1012–1028. Idan, M., 1996. “Estimation of rodrigues parameters from vector observations”. IEEE Transactions on Aerospace and Electronic Systems, Vol. 32, pp. 578–586. Markley, F.L., 1988. “Attitude determination using vector observations and the singular value decomposition”. Journal of the Astronautical Sciences, Vol. 38(3), pp. 245–258. Markley, F.L. and Crassidis, J.L., 1996. “Attitude estimation using modified rodrigues parameters”. In Proceedings of the Flight Mechanics/Estimation Theory Symposium. Greenbelt, USA. Schaub, H., 1998. Novel coordinates for nonlinear multibody motion with applications to spacecraft dynamics and control. Ph.D. thesis, Texas A M University, Texas. Shuster, M.D., 1993. “A survey of attitude representations”. Journal of the Astronautical Sciences, Vol. 41, pp. 439–517. Wertz, J.R., 1978. Spacecraft Attitude Determination and control. Kluwer Academic Publishers, The Netherlands. Yang, Y., 2012. “Spacecraft attitude determination and control: Quaternion based method”. Annual Reviews in Control, Vol. 36, pp. 198–219. The authors are the only responsible for the printed material included in this paper.