CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
Model-Based A i ude Es ima ion o Mul icop e s
Radek BARANEK, F an isek SOLC
Depa men o Con ol and Ins umen a ion, Facul y o Elec ical Enginee ing and Communica ion,
B no Uni e si y o Technology, Technicka 12, 616 00 B no, Czech Republic
[email p o ec ed], [email p o ec ed]
Abs ac . The pape deals wi h model-based a i ude
es ima ion o mul icop e s and is mainly ocused on
in es iga ion o accu acy deg ada ion due o wind and
inaccu a e model pa ame e s which a e condi ions al-
ways p esen when using in eal wo ld. A i s he need
o model-base es ima ion is mo i a ed. Then he mul-
icop e model is desc ibed. Based on he ma hema ical
model o mul icop e , he es ima ion algo i hm u ilizing
he ex ended Kalman il e is cons uc ed. The main
con ibu ion o he pape is he in es iga ion o he neg-
a i e impac o he wind and o inaccu a e knowledge
o he model pa ame e s.
Keywo ds
A i ude es ima ion, ex ended Kalman il e ,
mul icop e .
1. In oduc ion
Mul icop e s a e VTOL (Ve ical Take-O and Land-
ing) ae ial ehicles (AVs) included wi hin he class o
mul i o o helicop e s. These ehicles di e om s an-
da d helicop e s in ha hey use o o s wi h ixed-pi ch
blades, and hus hei o o pi ch does no a y as he
blades o a e. Cu en ly exis ing e sions comp ise 4
o mo e o o s (x-cop e s); he hexacop e uses 6 o o s
(Fig. 1).
The e also exis e sions wi h ewe o o s, bu hey
equi e addi ional mo ing componen s o s abiliza ion,
hus hey do no possess he mechanical simplici y ea-
u e.
A p esen , hese ypes o AVs a e used as a s an-
da d pla o m o obo ics esea ch. The i s objec-
i e in he cons uc ion o au oma ic con ol sys ems
o hese AVs is o ensu e s able ligh a low eloci ies,
pa icula ly du ing he ho e ing phase. The design o
x cop e con ol elemen s has hi he o been discussed
by a la ge numbe o au ho s. Gene ally, he con-
Fig. 1: The expe imen al mul icop e : hexacop e .
olle s a e designed as linea SISO sys ems [1], PID
sys ems [2], special nonlinea con olle s [3] o [4], and
e en neu al ne con olle s [5]. The mos impo an
componen o he con ol loop is a good eedback sig-
nal p o iding co ec in o ma ion abou he AV’s a i-
ude. Al hough he majo i y o he abo e-men ioned
sys ems use on-boa d senso s, he necessa y eedback
can be also p o ided by o -boa d sensing elemen s [6].
The mos widely used de ices o a i ude es ima ion
a e ine ial senso s and magne ic ield senso s based
on MEMS echnology, namely MEMS accele ome e s,
gy oscopes and magne ome e s. In ac , hese de ices
o m a s ap-down ine ial measu emen uni (IMU),
and hey ha e become he main a i ude senso s due
o hei low cos . O he senso s, such as sona ange
inde s, came as, lase s, o GPS, a e used especially o
posi ion eedback.
A signi ican ac o in he design o a con ol sys-
em is a good ma hema ical model which desc ibes
bo h dynamics o he AV i sel and dynamics o he
IMU as well. In mos pape s, he p esen ed models o
con olled AVs conside only he o ces and momen s
caused by p opelle s in ho e ing and neglec all o he
ae odynamic e ec s because o he low linea eloci ies.
Such models do no p oduce accele ome e da a usable
o good eedback ensu ing accu a e in o ma ion on he
a i ude o an AV [7].
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 501
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
The basic senso s o IMU do no p o ide di ec in-
o ma ion abou he a i ude o he AV. To p o ide
he igh in o ma ion abou he a i ude o he AV,
he senso signals mus be app op ia ely handled. A
p esen , he p ocessing o such signals is pe o med by
he algo i hm o Kalman il e ing. This algo i hm is
used in he calcula ion o he a i ude in o m o he
combina ion o p edic ion and co ec ion. Fo he p e-
dic ion a sui able dynamic model is used, he co ec ion
is ca ied ou by di ec measu emen . Thus well cho-
sen model o p edic ion can imp o e in o ma ion abou
he a i ude o he AV. The ollowing a icle shows and
discusses cha ac e is ics o such a model and i s use o
es ima ion.
2. Model Based A i ude
Es ima ion
The ypical a i ude es ima ion algo i hms [10] use
no assump ions abou dynamics o he examined ob-
jec . They ely only on he used senso s ( ypically ac-
cele ome e s, gy oscopes, magne ome e s, GPS e c.).
Wi h good model including he knowledge o he dy-
namic p ope ies can p o ide mo e accu a e esul s.
Bu any unmodeled e ec s o inaccu a e model pa am-
e e s can deg ade he accu acy and e en make he e-
sul s wo se han wi hou he dynamic model.
Mul icop e s a e ob iously good adep s (inpu s
o he sys em a e known, se e al dynamics models
we e cons uc ed) whe e including he dynamic model
should imp o e he accu acy. The analysis o inaccu-
a e model pa ame e s and unmodeled e ec s (namely
wind) is subjec o his pape .
In he ollowing sec ion mul icop e dynamic model
based on he ecen pape s is desc ibed. In he ou h
sec ion he dynamic model based a i ude es ima ion
algo i hm is cons uc ed. The esul s o simula ions,
whe e mainly he e ec s o wind and inaccu a e model
pa ame e s a e s udied, a e in sec ion 5.
Finally in he sec ion 6 he wind e ec mi iga ion is
desc ibed.
3. Mul icop e Model
In his chap e he ma hema ical model o mul icop e
is men ioned. This model was used o gene a ion o
inpu da a o simula ion o a i ude es ima ion algo-
i hm. Addi ionally he model was also used o de-
signing he es ima ion algo i hm i sel . The bigges
ad an age o using ma hema ical model o gene a ion
o es ing da a is he knowledge o he ue alues o all
s a es. This ea u e enables di ec compa ison o he
es ima ed s a e wi h he ue s a e.
Ma hema ical model o mul icop e is s udied in
many ecen publica ion ocused mainly on a i ude
and posi ion con ol [1], [2], [3], [4], [5] bu also o
a i ude es ima ion [10]. They di e in le el o p eci-
sion and in numbe o modeled e ec s. He e men ioned
model is based on all ci ed wo ks, and he main e ec s
o in e es a e ae odynamic d ag, wind and p ecise sen-
so models.
This pa icula model assumes he mul icop e has
6 mo o s wi h p opelle s a anged equidis an ly on a
ci cle wi h adius L. The scheme o he mul icop e is
in Fig. 2.
Fig. 2: Scheme o mul icop e wi h 6 mo o s ( op iew).
Two coo dina e ames a e used h oughou he de-
sc ip ion o he mul icop e model. Body ame is he
coo dina e sys em igidly linked wi h he mul icop e .
The o ien a ion o body ame axes wi h espec o
mul icop e is depic ed in Fig. 2. Re e ence ame is
a coo dina e sys em wi h espec o which he a i-
ude o body ame is in es iga ed. Usually his ame
is linked o local e ical di ec ion and o ue no h
di ec ion. In his wo k his ame is assumed o be
ine ial ame. I means ha Co iolis o ce, ea h o a-
ion and anspo a e, which a e common o ames
linked wi h ea h su ace, a e neglec ed. This app ox-
ima ion is alid when he usage o low-cos senso s is
expec ed since hese e ec s a e beyond he esolu ion
o he senso s.
3.1. Fo ces and To ques
Fo ces and o ques a e d i ing he mo ion o he mul-
icop e . The o al o ce and o que a e composed o
di e en sou ces: h us o indi idual p opelle s, ae o-
dynamic d ag and g a i y.
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 502
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
1) Mo o s wi h P opelle s
Th us s o indi idual p opelle s a e ela ed o he con-
ol signal. This ela ion is modeled by his equa ion.
Ti=cTS2
i,(1)
whe e Tiis h us o i- h mo o , cTis a posi i e con-
s an pa ame e and Siis con ol alue o i- h mo o .
Each mo o also gene a es eac i e o que, which has
opposi e di ec ion han he angula a e ec o o he
mo o sha . The magni ude o he eac i e o que is
assumed o be p opo ional o mo o h us .
~mi=−cRTi
~ωi
k~ωik,(2)
whe e ~miis he eac i e o que ec o o i- h mo o , cR
is a posi i e cons an pa ame e and ~ωiis he angula
a e ec o o i- h mo o sha . The esul ing o ce
and o que ec o gene a ed by h us s o mo o s a e
compu ed using.
Fz
Mx
My
Mz
=A[T1T2T3T4T5T6]T,
A=
−1−1−1−1−1−1
0−L√3
2−L√3
2
0L√3
2L√3
2
LL
2−L
2−L−L
2
L
2
cR−cRcR−cRcR−cR
,(3)
whe e Fzis he z-componen o o ce ec o , ~
FBT ex-
p essed in body ame espec ing he coo dina e sys-
em de ined in Fig. 2, Mx,My,Mza e componen s o
o que ~
MBT exp essed again in body ame, Lis leng h
o he mul icop e a m and cRis cons an pa ame e
de ined in Eq. (2). The ma ix Acan be de i ed using
simple mechanics and Fig. 2.
2) Ae odynamic D ag
Ae odynamic d ag is e y o en neglec ed in mul i-
cop e models. Howe e , his phenomenon enables use
o he accele ome e measu emen s in he a i ude es-
ima ion. This is he main eason why his d ag is
implemen ed in his mul icop e model.
The ae odynamic d ag o ce is modeled as linea ly
dependen on he ela i e eloci y wi h espec o wind.
~
FBA =
−kx w
xb
−ky w
yb
−kz w
zb
,(4)
whe e ~
FBA is d ag o ce ec o exp essed in body
ame, kia e posi i e cons an pa ame e s and w
ib a e
componen s o ela i e eloci y wi h espec o wind
exp essed in body ame. This is a simpli ied e sion
o p opelle induced ae odynamic d ag om [7]. The
eason why he pa ame e s kia e cons an in body
ame comes om he cons an o a ion axis o p o-
pelle s wi h espec o his ame. The pa ame e kzis
assumed o be o o de smalle han he kxand ky, be-
cause he zbody axis is pa allel o he axis o o a ion
o he p opelle s and he main con ibu o o he ae o-
dynamic d ag is he blade- lapping phenomenon which
occu s only in he axis pe pendicula o he p opelle
o a ion [7].
The ae odynamic d ag o que is simila ly modeled
as linea ly dependen on angula a e.
~
MBA =−kM~ωB,(5)
whe e ~
MBA is d ag o que ec o exp essed in body
ame, kMis posi i e cons an pa ame e and ~ωBis
angula a e ec o exp essed in body ame. The ae o-
dynamic d ag o que is based on he expe ience ha
angula a e se les on some ini e alue when cons an
o que is applied. In many ecen mul icop e models
his e ec is omi ed.
3) To al Accele a ion and Angula
Accele a ion
All he men ioned o ces and o ques a e summed and
di ided by mechanical p ope ies o o m he inal ac-
cele a ion ~aIand angula accele a ion ~εB.
~aI=~gI+RBI(~
FBT +~
FBA)
m,(6)
whe e ~aIis accele a ion exp essed in ine ial ame, ~gI
is g a i a ional accele a ion exp essed in ine ial ame,
RBI is ans o ma ion ma ix om body o ine ial
ame and mis he mass o he mul icop e .
~εB= [( ~
MBT +~
MBA)−(~ωB×IB~ωB)] ·I−1
B,(7)
whe e ~εBis angula accele a ion, ~ωBis angula a e
and IBis ine ia ma ix o he mul icop e , all a iables
exp essed in body ame.
3.2. Mul icop e Mo ion
The mo ion o mul icop e is modeled as he 6–DoF
igid body mo ion d i en by o ces and o ques de-
sc ibed abo e. The mo ion s a e is desc ibed by eloc-
i y and posi ion o ansla ional mo ion and by angu-
la a e and a i ude qua e nion o o a ional mo ion.
This pa is alid o any ae ial ehicle and is exp essed
by he ollowing di e en ial equa ions.
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 503
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
1) Posi ion and Veloci y
~
˙p=~ I,~
˙ I=~aI,(8)
whe e ~p is posi ion ec o , ~ Iis eloci y ec o ex-
p essed in ine ial ame and ~aIis accele a ion ec o
exp essed in ine ial ame.
2) A i ude and Angula Ra e
~
˙q=
˙q1
˙q2
˙q3
˙q4
=
= 0.5
q1−q2−q3−q4
q2q1−q4q3
q3q4q1−q2
q4−q3q2q1
0
ωx
ωy
ωz
,(9)
whe e ~q is a i ude qua e nion and ωx,ωy,ωza e com-
ponen s o angula a e ec o exp essed in body ame.
~
˙ωB=~εB,(10)
whe e ~ωBis angula a e ec o exp essed in body
ame and ~εBis angula accele a ion ec o exp essed
in body ame.
3.3. Wind
Since he ae odynamic d ag o ce is dependen on e-
loci y wi h espec o wind and he accele a ion mea-
su ed by accele ome e s (used o a i ude es ima ion)
is he ime de i a i e o he ine ial eloci y, i is ea-
sonable o model he wind o be able o e eal possible
nega i e impac o his ac on he accu acy o a i ude
es ima ion.
The wind is speci ied by ins an eloci y ec o ex-
p essed in ine ial ame and in his wo k is assumed
o be only unc ion o ime. The o al wind eloci y
consis s o s a ic and dynamic pa . S a ic pa is a
cons an eloci y ec o exp essed in ine ial ame and
co esponds o dominan cons an wind expe ienced in
eal condi ion. Dynamic pa is ime dependen and
each componen o he dynamic eloci y is modeled as
a i s o de Gauss Ma ko p ocess [11]. The dynamic
componen co esponds o sho ime a ia ion o he
wind (wind gus ). The beha io o he dynamic pa
can be adjus ed by s eady s a e a iance and by ime
cons an o Gauss Ma ko p ocess.
In Fig. 3 he e is he ou pu o he wind model sim-
ula ion wi h ime cons an τ= 7 s, cons an speed
ec o ~
b= [2,0,−2] m·s−1and s eady s a e de ia ion
o Gauss-Ma ko p ocess σ= 0.3m·s−1.
Fig. 3: Wind speed simula ion.
3.4. Senso s
Wi h espec o a i ude es ima ion hese senso s a e
o in e es :
•Gy oscope – measu ing angula a e.
•Accele ome e – measu ing speci ic o ce.
•Magne ome e – measu ing magne ic ield.
Each o his senso senses he physical quan i y along
h ee pe pendicula axes pa allel o body ame axes.
T ue alues o hese senso s can be easily compu ed
om ue s a e a ailable o mul icop e model. T ue
alues a e hen delibe a ely co up ed o ha e simila
cha ac e is ics like he eal senso s o his ype.
The same senso model bu wi h di e en pa ame e s
is used o each o hese senso s.
xOUT =xT RUE +bS+bD+n, (11)
whe e xT RUE is ue alue, xOUT is modeled seno
ou pu , bSis cons an bias, bDis dynamic bias mod-
eled as i s o de Gauss-Ma ko p ocess and nis a
whi e noise. The quali a i e cha ac e is ics o he sen-
so model can be seen in Fig. 4, whe e ze o ue signal
is co up ed by all e ms o he senso model.
4. Implemen a ion o
Model-Based A i ude
Es ima ion
The model-based a i ude es ima ion is no new. Is
discussed in some pape s including [12], [13]. The new
con ibu ion is he iden i ica ion and in es iga ion o
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 504
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
Fig. 4: Ou pu o he senso model.
main causes which could make he es ima ion less ac-
cu a e in eal wo ld. The model based a i ude es i-
ma ion o mul icop e s assumes ha his in o ma ion
is a ailable:
•Da a om gy oscope, accele ome e and magne-
ome e .
•Con ol alues o mo o s.
The adi ional algo i hms which p ocess he sen-
so s om he i s bulle use so-called ec o ma ching
me hod. This me hod assumes ha he accele ome-
e measu es g a i a ional ield only (which holds some
in o ma ion abou a i ude). Bu his is gene ally no
ue du ing he whole ligh and i is especially no ue
du ing agg essi e maneu e s and changes o a i ude.
On he o he hand he model based a i ude es i-
ma ion use ma hema ical model wi h modeled ae ody-
namic d ag o p edic he ue accele ome e measu e-
men . The accu acy o his app oach is hen heo e i-
cally independen o he maneu e s lown by he ae ial
ehicle.
The model based a i ude es ima o is based on he
ma hema ical model o mul icop e men ioned in he
p e ious chap e . In Fig. 5 he e is a scheme o he ull
mul icop e model.
The me hod used o es ima ion is ex ended Kalman
il e . I p o ides he bes scalable amewo k o im-
plemen a ion o such a model-based es ima ion p ob-
lem. Ex ended Kalman il e is an i e a i e algo i hm.
Each o i s i e a ion consis s o wo s eps. In p edic ion
s ep, he nex alue o he s a e is p edic ed based on
he inpu s and he p e ious s a e. In upda e s ep he
co ec ions a e compu ed using he measu emen s and
applied o he p edic ed s a e.
Acco ding o Fig. 5 he ull model could be used as
a co e o a Kalman il e es ima o . I means use mo-
o signals and wind speed o p edic ion o he s a e
Fig. 5: Scheme o he ull mul icop e model.
and hen use all h ee senso s (gy oscope, accele ome-
e and magne ome e ) o co ec ion o he p edic ed
s a e.
Howe e he ull model is sligh ly simpli ied o
model based a i ude es ima ion o a ious easons.
A i s i is assumed ha he mul icop e is no
equipped wi h any kind o senso capable o measu -
ing wind speed. Fo his eason he wind speed inpu
has o be neglec ed. Nex , he mo o signals a e no
used o p edic ion o angula a e (measu ed di ec ly
by gy oscopes), despi e i is possible. This p edic ion
is no used in he p esen ed es ima ion algo i hm, as
he accu acy o he p edic ion is e y low compa ed o
he accu acy e en o low cos gy oscope. This op i-
miza ion leads o lowe numbe o s a es o he il e
and in lowe compu a ion complexi y. Addi ionally he
knowledge o ine ia ma ix o he mul icop e is no
equi ed.
The e o e he Kalman il e based on he simpli ied
model uses he gy oscope and mo o signal o p edic-
ion s ep and he accele ome e and magne ome e o
upda e s ep. The scheme o he simpli ied model is in
Fig. 6. The es ima ed wind is assumed o be ze o in he
i s e sion. Chap e 6 discuss he e ec o es ima ion
o he wind speed.
The gene al equa ions o disc e e ex ended Kalman
il e a e:
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 505
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
Fig. 6: Scheme o he simpli ied model used o a i ude es i-
ma ion.
1) P edic ion S ep
~x(k+ 1|k) = (~x(k|k), ~u(k)),
P(k+ 1|k) = F·P·FT+Q.(12)
2) Upda e S ep
K=P(k+ 1|k)·HT
H·P(k+ 1|k)·H+R,
δ~z(k+ 1) = ~z(k+ 1) −h(~x(k+ 1|k)),
~x(k+ 1|k+ 1) = ~x(k+ 1|k) + K(δ~z(k+ 1)),
P(k+ 1|k+ 1) = (I−K·H)·P(k+ 1|k),
F=∂
∂~x |~x(k|k),~u(k),(13)
H=∂h
∂~x |~x(k+1|k),
whe e ~x,~u,~z a e he s a e, inpu and measu emen
ec o s espec i ely, P,Q,Ra e he co a iance ma i-
ces o s a e, inpu s and measu emen s espec i ely, F
and Ha e Jacobian ma ices o unc ions and hand
inally kis he sample ime index.
The es ima ion algo i hm u ilizing ex ended Kalman
il e is uniquely de ined by he unc ions and hand
by co a iance ma ices Qand R. The s a e, inpu and
measu emen s ec o s o he e p esen ed model based
a i ude es ima ion a e de ined as ollows.
~x =h~q ~ I~
bi,
~u =~ωg~
S,(14)
~z =~aB~mB,
whe e ~q is he ou elemen a i ude qua e nion, ~ Iis
he eloci y ec o exp essed in ine ial ame, ~
bis he
gy oscope bias ec o , ~ωgis he angula a e ec o
sensed by gy oscope, ~
Sis he ec o o mo o con ol
inpu s, ~aBis he speci ic o ce ec o sensed by ac-
cele ome e and ~mBis he magne ic ield sensed by
magne ome e . In his case, o model based a i ude
es ima ion algo i hm, he unc ion consis s o he ol-
lowing subpa s:
•Disc e e qua e nion in eg a ion based on Eq. (9)
whe e he angula a e ec o has he ollowing
o ma o inco po a e es ima ed gy oscope bias.
~ω =~ωg−~
b. (15)
•Ine ial accele a ion compu a ion and disc e e in-
eg a ion o ob ain ine ial eloci y which a e
based on Eq. (1), Eq. (3), Eq. (4) and Eq. (6).
•Bias p opaga ion de ined by Gauss-Ma ko p o-
cess [11].
~
bk+1 =βD~
bk.(16)
The unc ion hwhich p edic s he measu emen al-
ues ha e he ollowing pa s.
•Speci ic- o ce p edic ion based on Eq. (1), Eq. (3),
Eq. (4) and Eq. (6) whe e g a i y is excluded.
•The p edic ion o magne ic ield in body ame.
~mb=RIB ~mI=RT
BI ~mI,(17)
whe e RIB is ans o ma ion ma ix om ine ial
o body ame and ~mIis supposed o be known
Ea h magne ic ield ec o exp essed in ine ial
ame. The measu emen model o magne ome-
e assumes ha magne ome e is measu ing he
Ea h magne ic ield only which is gene ally no
ue and ca e mus be aken when using especially
in indoo en i onmen s.
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 506
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
The co a iance ma ices Qand Ra e de ined as ol-
lows [13].
Q=
Qq0 0
0QV0
0 0 QB
,
Qq=BQgBT,
B=∂
∂~ω |~x,~ω,
Qg=diag(~σ2
g),(18)
QV=diag(~σ2
V),
QB=diag(~σ2
B),
R=diag(~σ2
A, ~σ2
M),
whe e ~σgis s anda d de ia ion ec o o gy oscope
whi e noise, ~σVis s anda d de ia ion ec o ep e-
sen ing he accu acy in compu a ion o he eloci y
inc emen , ~σBis s anda d de ia ion ec o o whi e
noise d i ing he Gauss-Ma ko p ocess o gy oscope
biases, ~σAis s anda d de ia ion o accele ome e which
o e bounds all e o sou ces o his senso and ~σMis
s anda d de ia ion ec o o magne ome e which o e -
bounds all e o sou ces o his senso .
5. Simula ions
The compu e simula ions o model-based a i ude es-
ima ion algo i hm, desc ibed in he p e ious chap e ,
we e a ge ed o ind he limi s o use o such algo-
i hm in eal condi ions. Wi h espec o his aim, he
e ec s o wind and he inaccu a e model pa ame e s
on accu acy o a i ude es ima es we e in es iga ed.
A i s he pa ame e s o he ull mul icop e model
was de ined. The summa y o he model pa ame e s
a e in he Tab. 1. This model was hen used o gene -
a ion o wo, 60 s long, es ing ajec o ies. Fi s is low
speed no mal ajec o y wi h il s less han 45 deg ees.
The second one is high speed agg essi e ajec o y wi h
il s up o 85 deg ees. The es ima ion e o s o a i-
ude ecompu ed o Eule angles o bo h ajec o ies
a e in Fig. 7 and Fig. 8. The e o is compu ed as a
di e ence be ween es ima ed alue and he ue alue.
Red lines in he igu es a e 1−σaccu acy o he es i-
ma ed s a e di ec ly compu ed om he Kalman il e
co a iance ma ix. The model pa ame e s used in es-
ima ion algo i hm ma ch he pa ame e s used in he
gene a ion o es ing da a. Wind is no p esen in hese
cases.
5.1. Wind E ec
The eason ha one can assume ha wind should ha e
nega i e impac on accu acy o a i ude es ima ion
Fig. 7: Es ima ion e o o no mal ajec o y.
Fig. 8: Es ima ion e o o agg essi e ajec o y.
comes om he ac , ha in eal wo ld he ae ody-
namic d ag is p opo ional o ela i e eloci y wi h e-
spec o wind while he es ima ion algo i hm assumes
i is p opo ional o ine ial speed.
I is app op ia e o assume ha cons an wind will
ha e no e ec on he accu acy o es ima ion because
wi h espec o a i ude he cons an wind will cause no
change o measu emen s in any used senso . Howe e
he changes o wind can cause signi ican change in ac-
cele ome e measu emen s which a e no p edic ed by
he model. This can ob iously lead o dec eased accu-
acy. The ollowing simula ions a e pe o med o e -
i y abo e hypo hesis. On bo h es ing ajec o ies, ou
di e en se ings o wind we e simula ed. The measu e
o nega i e impac is he RMSE e o o es ima ed a -
i ude exp essed in Eule angles. The di e en se ings
o wind pa ame e s a e in Tab. 2. The esul ing RMSE
alues a e in Tab. 3.
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 507
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
Tab. 1: Full mul icop e model pa ame e s.
Model pa ame e s
Pa ame e Value Pa ame e Value
Mass m= 1 kg Fo ce d ag z kz= 0.8Ns·m−1
Ine ia enso I/103kg·m2Mo o cons an cT= 6.9·10−5N
To que d ag kM= 0.2Nm To que cons an cR= 0.05 Nm
Fo ce d ag x kx= 2 Ns·m−1A m leng h L= 0.2m
Fo ce d ag y ky= 2 Ns·m−1
Senso Pa ame e s
Pa ame e Gy oscope Accele ome e Magne ome e
S a ic bias s d 0.05 ad·s−10m·s−20mT
GM ime cons an 300 s300 s300 s
GM s d 0.013 ad·s−10.041 m·s−20.041 mT
Whi e noise s d 0.014 ad·s−10.14 m·s−20.022 mT
Tab. 2: Wind pa ame e s o di e en scena ios.
S a ic wind pa eloci y [m·s−1] Dynamic wind pa
x y z Time
cons an [s]
S anda d de ia ion
o GM p ocess [m·s−1]
Wind 1 0.5 -0.5 0.5 7.0 0.2
Wind 2 5.0 -5.0 5.0 7.0 0.2
Wind 3 0.5 -0.5 0.5 7.0 2.0
Wind 4 5.0 -5.0 5.0 7.0 2.0
Tab. 3: Resul ing RMSE alues o wind impac simula ions.
No mal ajec o y
RMSE (deg)
Agg essi e ajec o y
RMSE (deg)
Roll Pi ch Yaw Roll Pi ch Yaw
No wind 0.33 0.46 0.91 0.50 0.46 0.64
Wind 1 0.47 0.56 0.90 0.73 0.52 0.64
Wind 2 0.74 0.59 0.89 1.06 0.54 0.71
Wind 3 4.09 2.81 1.68 4.31 1.94 2.05
Wind 4 3.89 2.82 1.62 4.17 1.92 2.00
Tab. 4: Lis o pa ame e s and hei co up ions.
Mass Fo ce
d ag x
Fo ce
d ag y
Fo ce
d ag z
Mo o
cons an
A m
leng h
Pa ame e co up ion ac o (%)
Scena io 1 - 70 - - - -
Scena io 2 - 130 - - - -
Scena io 3 - - - 70 - -
Scena io 4 - - - 130 - -
Scena io 5 - - - - 70 -
Scena io 6 - - - - 130 -
Tab. 5: Resul ing RMSE alues o inaccu a e model pa ame e s impac simula ions.
No mal ajec o y
RMSE (deg)
Agg essi e ajec o y
RMSE (deg)
Roll Pi ch Yaw Roll Pi ch Yaw
No wind 0.33 0.46 0.91 0.50 0.46 0.64
Scen. 1 0.88 0.75 1.02 2.11 1.25 1.46
Scen. 2 0.58 0.56 0.93 1.18 0.82 0.85
Scen. 3 1.13 0.65 0.93 3.42 2.33 1.27
Scen. 4 0.68 0.46 0.92 1.90 1.47 1.09
Scen. 5 3.27 1.55 1.23 6.70 5.51 2.76
Scen. 6 3.99 1.62 1.28 5.27 4.80 3.02
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 508
CONTROL ENGINEERING VOLUME: 12 |NUMBER: 5 |2014 |DECEMBER
Tab. 6: Resul ing RMSE alues o algo i hm wi h o wi hou he wind ec o s a e.
Agg essi e
Algo i hm
Wi h wind in
s a e RMSE [deg]
Wi hou wind in
s a e RMSE [deg]
Roll Pi ch Yaw Roll Pi ch Yaw
Wind 1 0.80 0.63 0.67 0.73 0.52 0.64
Wind 2 0.91 0.62 0.69 1.06 0.54 0.71
Wind 3 2.11 0.74 1.38 4.31 1.94 2.05
Wind 4 0.94 0.62 0.76 4.17 1.92 2.00
The alues in Tab. 3 indica e how he wind a ec s
accu acy o he a i ude es ima es. I can be concluded
ha s a ic pa o he wind has almos no e ec on es-
ima ion accu acy while dynamic pa ( he wind speed
changes) causes signi ican dec ease o he accu acy.
Ano he conclusion is ha he ype o ajec o y has a
li le in luence on accu acy when wind dis u bance is
in oduced.
5.2. Model Pa ame e s
The inaccu a e model pa ame e s can also ha e nega-
i e impac on accu acy o a i ude es ima es. In o de
o de e mine he quan i a i e in o ma ion abou his
e ec , ano he se o simula ions was pe o med.
The model pa ame e s equi ed in es ima ion algo-
i hm a e lis ed in Tab. 4, whe e also he co up ion
scena ios o selec ed pa ame e s a e de ined. Fo each
scena io he co esponding pa ame e was co up ed in
es ima ion algo i hm om i s ue alue by he ac o
shown in Tab. 4 and hen he RMSE alue o bo h
ajec o ies was compu ed which a e lis ed in Tab. 5.
Mass and a m leng h a e excluded om simula ions,
since hese quan i ies can be wi h ease e y accu a ely
measu ed. In o he wo ds i is no expec ed ha hese
pa ame e s would be inaccu a e. The o ce d ag y pa-
ame e is excluded because i is assumed ha he be-
ha iou in xand yaxis will be e y simila .
F om Tab. 5 many in e es ing conclusions can be
made. A i s he accu acy o es ima ion s ongly de-
pends on he ype o ajec o y when some o he pa-
ame e is known inaccu a ely. Fu he he accu acy
o he o ce d ag z pa ame e is mo e c i ical han he
ones o xand yaxes. Finally he mos c i ical pa-
ame e is cons an ela ing he mo o con ol signal
o mo o h us .
The esul s o his chap e show he accu acy deg a-
da ion e ec s which can be expec ed when using he
model-based es ima ion algo i hm in eal applica ion.
The wind and model pa ame e s a e only a pa o all
e ec which can deg ade he accu acy. E en he model
equa ions could be jus app oxima ions o eal wo ld
beha io (which is ob iously ue) bu he e men ioned
esul s can help sugges ing on which pa he deep e-
sea ch should be pe o med o gain be e accu acy.
6. Wind E ec Mi iga ion
As shown in sec ion 5.1 he wind has nega i e impac
on a i ude es ima ion accu acy using dynamic model
app oach. One possible way how o inhibi his e ec
is o include he wind speed ec o in o he es ima ed
s a es. In he p e ious case he model assumed ha
eloci y wi h espec o wind equals o ine ial eloci y.
In his case hey di e by amoun o he es ima ed
wind speed. The es ima ed wind speed is exp essed in
ine ial ame. By ew i ing Eq. (4) one ge s.
~
FBA =~ i
B−RIB~ w
IT
−kx
−ky
−kz
,(19)
whe e ~ iB is eloci y ec o wi h espec o ine ial
ame exp essed in body ame, RIB is ans o ma ion
ma ix om ine ial o body ame and ~ w
Iis es ima ed
wind speed ec o exp essed in ine ial ame. Ad-
di ionally he indi idual componen s o wind eloci y
ec o a e modeled as Gauss Ma ko p ocess. In he
ollowing simula ions he pa ame e s o Gauss-Ma ko
p ocess o es ima ed s a e is he same as he one used
o gene a ing he wind speed ec o . Resul s o simu-
la ions a e in Tab. 6. The esul s show ha including
he wind speed ec o in o es ima ed s a e space sig-
ni ican ly educe he RMSE alues o las wo ows
whe e dynamic wind is p esen .
7. Conclusion
This pape e eals some p ope ies o so-called model
based a i ude es ima ion o mul icop e s. The s an-
da d model o mul icop e wi h induced d ag o ces was
p esen ed as a baseline o designing he es ima ion al-
go i hm. In simula ions he pe o mance o he il e
was e i ied on da a gene a ed by he ull model wi h
ad anced senso models. Fu he he e ec o wind and
inaccu a e model pa ame e s we e in es iga ed. The
esul s can be e ec i ely used o ocusing u u e e-
sea ch on he key pa s which ha e he bigges in lu-
ence on es ima ion accu acy, namely he wind s a e
es ima ion and online pa ame e es ima ion.
c
2014 ADVANCES IN ELECTRICAL AND ELECTRONIC ENGINEERING 509
