IFAC Pape sOnLine 56-2 (2023) 4508–4513
ScienceDi ec
A ailable online a www.sciencedi ec .com
2405-8963 Copy igh © 2023 The Au ho s. This is an open access a icle unde he CC BY-NC-ND license
.
Pee e iew unde esponsibili y o In e na ional Fede a ion o Au oma ic Con ol.
10.1016/j.i acol.2023.10.942
10.1016/j.i acol.2023.10.942 2405-8963
Copy igh ©
2023 The Au ho s. This is an open access a icle unde he CC BY-NC-ND license
(
h ps://c ea i ecommons.o g/licenses/by-nc-nd/4.0/
)
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗
Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos [email p o ec ed], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain (
{
jgco de o,jjimenez
}
@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o
∗∗
Ja ie Jimenez
∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
Jose A. Rebollo e al. / IFAC Pape sOnLine 56-2 (2023) 4508–4513 4509
Copy igh ©
2023 The Au ho s. This is an open access a icle unde he CC BY-NC-ND license
(
h ps://c ea i ecommons.o g/licenses/by-nc-nd/4.0/
)
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
A Symme y-Based Unscen ed Pa icle
Fil e o S a e Es ima ion o a Ballis ic
Vehicle
Jose A. Rebollo ∗Ra ael Vazquez ∗F ancisco Ga ilan ∗
Jo ge Co de o ∗∗ Ja ie Jimenez ∗∗
∗Dp o. de Ingenie ´ıa Ae oespacial, Uni e sidad de Se illa, Camino de
los Descub imien os s/n, 41092, Se illa, Spain
(jos eb e [email protected], { azquez1, ga ilan}@us.es)
∗∗ AERTEC Solu ions S.L., C/ Wilbu y O ille W igh , 31, 41309 -
La Rinconada, Spain ({jgco de o,jjimenez}@ae ecsolu ions.com)
Abs ac : The p oblem o s a e es ima ion o ehicles when an ini ial ix is highly unce ain
and/o he numbe o senso s is no su icien (and changes wi h ime) is e y ele an o
bo h ai c a and spacec a na iga ion. This wo k p oposes a Locally Linea ized Pa icle Fil e
based on a qua e nion-adap ed Unscen ed Kalman Fil e o es ima e he s a e o a ehicle
wi h minimal senso s and unce ain ini ial condi ions, exploi ing geome ical symme ies. The
algo i hm is applied o a ballis ic ehicle na iga ing owa ds a lase -illumina ed a ge using
on-boa d senso s, including a iad o accele ome e s and gy oscopes, a ba ome ic al ime e and
a lase ecei e . A symme y a ound he e ical axis is iden i ied; based on i , he algo i hm
becomes capable o sol ing he na iga ion p oblem, e en wi h highly unce ain ini ial condi ions
and wi hou enough senso in o ma ion; his second condi ion is pa icula ly se e e when he
lase ecei e is no ye ob aining da a. The p oposed na iga ion algo i hm o e s p omising
esul s in simula ion, apidly con e ging o an accu a e es ima e o he eal ajec o y when he
lase ecei e becomes ac i e.
Keywo ds: Ballis ic ehicles, pa icle il e , unscen ed Kalman il e , symme y-based obse e ,
na iga ion p oblem, a i ude es ima ion.
1. INTRODUCTION
F equen ly, one needs o sol e he p oblem o s a e es-
ima ion o ehicles (bo h ai c a and spacec a ) in
si ua ions whe e an ini ial ix, i a ailable, con ains la ge
unce ain ies, and/o he numbe and quali y o senso s
is no su icien o is changing wi h ime. Some exam-
ples include GPS-denied ai c a na iga ion (see, e.g., Wu
e al. (2013)), spacec a endez ous wi h non-coope a i e
umbling a ge s such as space deb is (see, e.g., Ma
e al. (2020)) o ballis ic ehicles a eling owa ds non-
maneu e ing g ound a ge s (see, e.g., Wei e al. (2017)),
being his las example he one conside ed in his wo k.
In pa icula , his pape conside s he p oblem o online
s a e econs uc ion o a ballis ic ehicle, a eling owa ds
a a ge illumina ed by lase ; in his o mula ion o a
Na iga ion P oblem, one needs o econs uc wi h on
boa d da a and in eal ime he ela i e posi ion o
he a ge , and he ehicle’s eloci y and a i ude. This
in o ma ion can hen be used o implemen , o ins ance,
a p edic i e guidance sys em. The a ailable senso s a e
a iad o accele ome e s and gy oscopes, a ba ome ic
al ime e , and a lase ecei e ha only ac i a es when he
a ge is close enough, gi ing hen he Line o Sigh (LOS)
angles ( his is ypically known as a s apdown seeke in he
li e a u e; hey a e conside ed supe io o pla o m seeke s
o hei simple s uc u e, highe eliabili y, smalle size,
and ligh e weigh , see, e.g. Wei e al. (2017)). The ini ial
ix is only e y app oxima ely known.
To sol e he p oblem, he au ho s p opose a Locally Lin-
ea ized Pa icle Fil e (LLPF), based on a qua e nion-
adap ed Unscen ed Kalman Fil e (UKF) o es ima e he
s a e o a ehicle wi h a minimal numbe o senso s and
unce ain ini ial condi ions, by exploi ing he geome ical
symme ies o he p oblem. Pa icle il e s (see, o in-
s ance, Ris ic e al. (2003)) ha e es ablished hemsel es as
a easible op ion o acking and es ima ion in ae ospace
p oblems, and ha e become easible wi h oday’s compu-
a ional means. They can deal wi h la ge unce ain ies
and nonlinea i ies, and can be combined wi h local il e s
inhe i ing hei p ope ies, such as he UKF in his case.
While, o he au ho s’ knowledge, he e a e no p e ious
wo k conside ing his pa icula p oblem, he e a e o he
con ibu ions ela ed o s apdown seeke s. In pa icula ,
he p oblem o line-o -sigh (LOS) a e econs uc ion
has been widely s udied. Since s apdown seeke s ixed
o a ehicle bodies canno di ec ly p o ide his a e in-
o ma ion, which is essen ial o p opo ional na iga ion
guidance laws, i is o g ea signi icance o es ablish an
app op ia e es ima ion model and design he co espond-
ing il e , so as o ob ain mo e accu a e a es. Nex , a
e y b ie e iew o some signi ican esul s in he a ea
is gi en. Fo ins ance, Wei e al. (2017) conside ed his
p oblem, p oposing a i h-deg ee cuba u e Kalman Fil e
es ima ion me hod based on an augmen ed-dimensional
s a e model o es ima e he LOS a es. Lin e al. (2005)
p oposed a LOS econs uc ion il e based on an exac
LOS dynamic model o s ap-down seeke , o which he
heo y o unscen ed ans o ma ion (UT) and unscen ed
Kalman il e was applied o ea he nonlinea i ies. Ma-
ley (2015) add essed he p oblem h ough he de elopmen
o a line o sigh a e ex ended Kalman il e , which was
able o accommoda e signi ican ime delays due o he
p ocessing equi emen s o compu e ision algo i hms o
he seeke . Finally, one can also ci e he esul s o Tae-Hun
e al. (2017), which conside a pa asi ic ins abili y e ec
due o he seeke ’s la ency; by in oducing a new s a e
ec o ep esen a ion along wi h he Pade app oxima ion
o compensa ing he ime-delay o he seeke , his wo k
p oposed a new guidance il e s uc u e based on an
ex ended Kalman il e .
The main con ibu ions o he p esen wo k a e wo old.
Fi s , a LLPF ha exploi s he symme ies o he p oblem
wi h a minimal and easonable numbe o senso s, unde
ealis ic condi ions o he ac i a ion o he lase ecei e ,
is o mula ed. Secondly, his LLPF is based on an UKF
which is ailo ed o he speci ic p oblem, in pa icula
espec ing he in a ian s a ising due o he use o qua e -
nions, e en hough o he such il e s al eady exis (see,
o ins ance, K a (2003)). Simula ions show an excel-
len pe o mance o he algo i hm unde easonable senso
noises and ini ial unce ain ies, wi h he es ima ion apidly
con e ging o an accu a e es ima e o he eal ajec o y
when he lase ecei e becomes ac i e.
2. PROBLEM STATEMENT
2.1 Kinema ic and ideal measu emen s desc ip ion
Rega ding no a ion, ec o s a e deno ed by bold a iables.
A ec o ae alua ed in a e e ence ame (A) is w i en
as aA, while i s componen s a e gi en by aA
j,j =1,2,3.
In his s udy, we conside a ee- all ballis ic ehicle (BV),
launched om a Remo ely Pilo ed Ai c a (RPAS); he
BV’s ajec o y could also con ain some guided segmen s,
bu his guidance is no aken in o conside a ion in his
wo k. The ehicle is modeled as ixed mass igid body
subjec o ee all mo ion in a eal a mosphe e. OBis
de ined as he cen e o mass o he BV, while OSis he
illumina ed a ge . Th ee e e ence ames a e conside ed
o con enience. Fi s ly, he Su ace (S) e e ence ame is
cen e ed in OSand mo es along wi h he Ea h’s su ace.
The zSaxis poin s owa ds he cen e o he Ea h while
xSand ySa e o ien ed a bi a ily ollowing a igh -hand
s uc u e. Secondly, he Body (B) ame is cen e ed in
OB, and o a es wi h he BV. Conside ing a cylind ical
shaped BV wi h a e ical plane o symme y, xBis aligned
wi h he longi udinal axis, on wa ds; zBlies in he
plane o symme y, downwa ds; and yBpoin s igh wa ds,
ul illing a igh -handed ame. Finally, a gene ic Ine ial
(I) e e ence ame is conside ed, wi h a bi a y cen e and
o ien a ion.
Le XSbe he posi ion o he cen e o mass o he BV
e e ed o (S). Le VS=˙
XS, and VBi s componen s
in B. Le S
IωSand B
IωBbe he angula eloci ies o
he Ea h and he BV, espec i ely. AB
Gand AB
NG a e
de ined as he g a i a ional and non-g a i a ional ine ial
accele a ions in OB. Fo he a i ude ep esen a ion, he
a i ude qua e nion B
Sqo he B ame wi h espec o S
is used (see, o ins ance, Wie (2015)).
Fo he cu en p oblem, se e al simpli ying hypo heses
can be made, wi hou in oducing app eciable e o s. On
one hand, he e ec o he Ea h’s o a ion is minimal, so
ha S
IωBcan be app oxima ed o ze o. Fo his pa icula
p oblem, he expec ed ligh ime o he BV is o less han
one minu e, hus he e ec i e o a ion o he Ea h wi h
espec o an ine ial ame du ing ha ime in e al is
clea ly impe cep ible. On he o he hand, i he Ea h’s
cu a u e is neglec ed, by app oxima ing he Ea h’s su -
ace by i s locally angen plane, he g a i a ional accele -
a ion is always aligned wi h kS, his is, he hi d ec o o
he Scanonical basis. By doing so, he equa ions o mo ion
o he BV a e educed o
˙
XS=VS,(1)
˙
VB=−B
IωB×VB+AGkB
S+AB
NG,(2)
B
I˙q=1
2
B
Iq⊗0
B
IωB.(3)
No e ha kB
Sis he kS ec o e alua ed in he B e e ence
ame. AGis he s anda d mean g a i a ional accele a ion
in he Ea h’s su ace. The ⊗ope a o deno es he qua e -
nion p oduc . Ro a ions be ween e e ence ames can be
pe o med wi h qua e nions (see Wie (2015)).
In o de o compu e he s a e o he BV, se e al on-
boa d senso s a e a ailable: a se o 3 accele ome e s and 3
gy oscopes, a ba ome ic al ime e and a lase ecei e . Fo
ideal senso s, no including measu emen e o s, which a e
cha ac e ized la e , he co esponding ideal measu emen
models can be w i en as
AB
Acc =˙
VB+B
IωB×VB−AB
G,(4)
B
IωB
Gy =B
IωB,(5)
hBa o =−XS
3,(6)
γ1= a c an 2(XB
3,XB
1),(7)
γ2= a c an 2(XB
2,XB
1),(8)
whe e AB
Gis he g a i a ional accele a ion exp essed in
he Bbasis. I he ajec o y s a ing poin was known,
wi hou conside ing e o accumula ion and wi h an ex-
ac model o g a i y, he accele ome e and gy oscope
measu emen s su ice o sol e he Na iga ion P oblem.
Fo ha eason, hese 2 ec o s de ine he p opaga ion
measu emen s, Zp, so ha
Zp=AB
Acc
B
IωB
Gy .(9)
As o he lase measu emen s, he lase beam de ia ion
om he ehicle’s main axis is measu ed in e ms o wo
angles, which a e e e ed o as γ1and γ2, co esponding
o he de ia ion o he lase di ec ion om he xBaxis
p ojec ed in he xBzBand xByBplanes, espec i ely. The
LOS angles measu emen s a e no always a ailable du ing
he BV’s ope a ion. In pa icula , wo condi ions mus
be sa is ied. Fi s ly, he lase pa h mus be easonably
aligned wi h he BV longi udinal axis, so ha he LOS
measu emen s a e inside he so called Field o View
(FOV) o he op ical senso used (Rudin (1993)). In his
applica ion, he FOV is o 15 deg ees o each angle.
Secondly, he dis ance om he poin sou ce o he senso
mus no be la ge han a h eshold which, in his case, is
conside ed o 2500 m. Thus, only i −15◦<γ
1,γ
2<15◦
and ∥XS∥<2500 m, he LOS angles can be conside ed.
Fo easons ha a e de ailed la e , he ime de i a i es
o hand γia e o in e es , despi e ha hey a e no
di ec ly measu ed. Conside ing he kinema ic e olu ion,
hese alues a e gi en by
4510 Jose A. Rebollo e al. / IFAC Pape sOnLine 56-2 (2023) 4508–4513
h=˙
h=−VS
3,(10)
˙γ1=XB
1d
d XB
3−XB
3d
d XB
1
�XB
12+�XB
32,(11)
˙γ2=XB
1d
d XB
2−XB
2d
d XB
1
�XB
12+�XB
22,(12)
o d
d XB=−�B
IωB×XB+VB.(13)
As he angula eloci y is con inuously measu ed and
he posi ion, eloci y and a i ude a e s a e a iables,
˙γ1and ˙γ2a e comple ely de ined p o ided he s a e is
known. No e ha , as (h, γ1,γ
2) only depend on geome ic
pa ame e s, while hei de i a i es also depend on bo h
linea and angula eloci ies, his second se o pa ame e s
can no be ob ained by algeb aic combina ion o he i s
one and ice e sa, consequen ly gua an eeing ha hese
6 measu emen s a e independen , o wha is he same,
hey p o ide in o ma ion ha is no c oss co ela ed. This
esul is p o ed o be o g ea in e es la e on.
2.2 S a e de e mina ion om cons ain s and symme ies
Le Xbe he s a e 10-uple o be compu ed, de ined as
X=
XS
VB
B
Sq
.(14)
No e ha Xdoes no belong o a ec o space, as i
con ains he componen s o an a i ude qua e nion, which
belongs o he space o a 4-D hype sphe e (Wie (2015)).
The s a e o he BV has 9 deg ees o eedom. Thus,
since no ini ial ix is a ailable, a se o 9 independen
equa ions gi en by measu able magni udes a e needed o
ully cha ac e ize X.
The i s majo di icul y o he es ima ion p oblem is o
de e mine o wha ex en can he BV’s s a e be compu ed
only om he a ailable measu emen s. In his sec ion, in
o de o simpli y he analysis, measu emen e o s a e
no conside ed. The senso s’ noise sou ces a e aken in o
conside a ion du ing he il e ing app oach la e on.
P o ided ha he lase is isible o he BV’s op ical
senso , 3 equa ions can be w i en o he sys em’s s a e
a a gi en ime,
Za=hBa o
γ1
γ2=
−XS
3
a c an 2(XB
3,XB
1)
a c an 2(XB
2,XB
1)
=h1(X).(15)
These equa ions a e insu icien o compu e Xin e ing h.
As long as he sampling equency o Zais adequa e, i s
de i a i e can be ob ained compu a ionally, leading o
˙
Za=
−VS
3
XB
1V′B
3−XB
3V′B
1
�XB
12+�XB
32
XB
1V′B
2−XB
2V′B
1
�XB
12+�XB
22
=h2(X,Zp).(16)
No e ha , besides compu a ional o measu emen e o s,
a highe o de de i a i e o Zacan no be conside ed as
he e appea e ms om ˙
Zpwhich a e no a ailable and
can no be de e mined. Wi h 3 addi ional measu emen s o
cons ain s, so ha h(X, Zp) is in e ible o X, he BV’s
s a e would be comple ely de e mined om he a ailable
in o ma ion on-boa d.
One in e es ing p ope y o his es ima ion p oblem, which
can be applied o educe he numbe o unknown a iables,
is ha he e is a spa ial symme y. Indeed, on one hand,
Zpdepends only on measu emen s on he B ame. On
he o he hand, he only ec o componen s w i en in he
Saxes in (15)–(16) a e XS
3and VS
3. Thus, i is use ul o
conside a o a ion o he S e e ence ame a ound he zS
axis, so ha he Bcomponen s a e unchanged.
A e his ans o ma ion (15)–(16) s ays in a ian and in
consequence has a cylind ical symme y. This means ha ,
wi h he on-boa d measu emen s only, XS
1and XS
2can
no be compu ed, as any pai (XS
1,XS
2) ha sa is ies
(XS
1)2+(XS
2)2+(hBa o)2=∥XB∥2(17)
can be a solu ion o (15)–(16) o X. This could be
expec ed, as he e is no way o dis inguish he xSand yS
axes. The zSdi ec ion, howe e , is explici in (15)–(16) as a
consequence o he al ime e ’s measu emen s. 1No e ha ,
o he cu en p oblem, i is o no in e es o compu e
XS
1and XS
2independen ly bu he ho izon al dis ance
om he BV o he a ge , Xh=(XS
1)2+(XS
2)2, and
i s ela i e o ien a ion. Thus, wi hou losing any use ul
in o ma ion o guidance, he cylind ical symme y can be
b oken by o a ing Sso ha , a any gi en ime,
XS
1=Xh,X
S
2=0.(18)
This conside a ion educes in 1 he numbe o deg ees
o eedom o he sys em’s s a e wi hou educing he
in o ma ion a ailable o he guidance sys em. In o de o
de e mine X, as he e a e no addi ional measu emen s o
symme ies, 2 cons ain s a e needed. One use ul app oach
is o bene i om he BV’s ae odynamic geome y. Wi h-
ou any con ol ac ion, he ae odynamic momen s end o
align he longi udinal axis o he ehicle wi h he eloci y
ec o . A e a ansi o y egime, he eloci y ec o in
he Baxes can be simpli ied o VB=(
U00
)T. This
condi ion educes in wo he numbe o unknown a iables.
Le he addi ional measu emen ec o be
Za=
hBa o
γ1
γ2
˙
hBa o
˙γ1
˙γ2
0
0
0
=
−XS
3
a c an 2(XB
3,XB
1)
a c an 2(XB
2,XB
1)
−VS
3
XB
1V′B
3−XB
3V′B
1
�XB
12+�XB
32
XB
1V′B
2−XB
2V′B
1
�XB
12+�XB
22
XS
2
VB
2
VB
3
=h(X,Zp).
(19)
I can be p o ed ha he unc ion his locally in e ible
o Xinside a signi ican domain (see, e.g. Cla ke (1976)).
The e o e, i can be conside ed ha i s in e se exis s in
his egion, so ha he a ailable measu emen s allow o
compu e he ehicle’s s a e i he s a ing poin used o
sol e he nonlinea p oblem is close enough o he eal
s a e. This shows ha a na iga ion sys em should be
easible, as long as he noise e ms a e small enough.
1In p ac ice, a se o magne ome e s o magne ic compass is enough
o b eak his symme y so ha he e is a measu able ho izon al
e e ence. This is no he case o he conside ed p oblem.
Jose A. Rebollo e al. / IFAC Pape sOnLine 56-2 (2023) 4508–4513 4511
h=˙
h=−VS
3,(10)
˙γ1=XB
1d
d XB
3−XB
3d
d XB
1
�XB
12+�XB
32,(11)
˙γ2=XB
1d
d XB
2−XB
2d
d XB
1
�XB
12+�XB
22,(12)
o d
d XB=−�B
IωB×XB+VB.(13)
As he angula eloci y is con inuously measu ed and
he posi ion, eloci y and a i ude a e s a e a iables,
˙γ1and ˙γ2a e comple ely de ined p o ided he s a e is
known. No e ha , as (h, γ1,γ
2) only depend on geome ic
pa ame e s, while hei de i a i es also depend on bo h
linea and angula eloci ies, his second se o pa ame e s
can no be ob ained by algeb aic combina ion o he i s
one and ice e sa, consequen ly gua an eeing ha hese
6 measu emen s a e independen , o wha is he same,
hey p o ide in o ma ion ha is no c oss co ela ed. This
esul is p o ed o be o g ea in e es la e on.
2.2 S a e de e mina ion om cons ain s and symme ies
Le Xbe he s a e 10-uple o be compu ed, de ined as
X=
XS
VB
B
Sq
.(14)
No e ha Xdoes no belong o a ec o space, as i
con ains he componen s o an a i ude qua e nion, which
belongs o he space o a 4-D hype sphe e (Wie (2015)).
The s a e o he BV has 9 deg ees o eedom. Thus,
since no ini ial ix is a ailable, a se o 9 independen
equa ions gi en by measu able magni udes a e needed o
ully cha ac e ize X.
The i s majo di icul y o he es ima ion p oblem is o
de e mine o wha ex en can he BV’s s a e be compu ed
only om he a ailable measu emen s. In his sec ion, in
o de o simpli y he analysis, measu emen e o s a e
no conside ed. The senso s’ noise sou ces a e aken in o
conside a ion du ing he il e ing app oach la e on.
P o ided ha he lase is isible o he BV’s op ical
senso , 3 equa ions can be w i en o he sys em’s s a e
a a gi en ime,
Za=hBa o
γ1
γ2=
−XS
3
a c an 2(XB
3,XB
1)
a c an 2(XB
2,XB
1)
=h1(X).(15)
These equa ions a e insu icien o compu e Xin e ing h.
As long as he sampling equency o Zais adequa e, i s
de i a i e can be ob ained compu a ionally, leading o
˙
Za=
−VS
3
XB
1V′B
3−XB
3V′B
1
�XB
12+�XB
32
XB
1V′B
2−XB
2V′B
1
�XB
12+�XB
22
=h2(X,Zp).(16)
No e ha , besides compu a ional o measu emen e o s,
a highe o de de i a i e o Zacan no be conside ed as
he e appea e ms om ˙
Zpwhich a e no a ailable and
can no be de e mined. Wi h 3 addi ional measu emen s o
cons ain s, so ha h(X, Zp) is in e ible o X, he BV’s
s a e would be comple ely de e mined om he a ailable
in o ma ion on-boa d.
One in e es ing p ope y o his es ima ion p oblem, which
can be applied o educe he numbe o unknown a iables,
is ha he e is a spa ial symme y. Indeed, on one hand,
Zpdepends only on measu emen s on he B ame. On
he o he hand, he only ec o componen s w i en in he
Saxes in (15)–(16) a e XS
3and VS
3. Thus, i is use ul o
conside a o a ion o he S e e ence ame a ound he zS
axis, so ha he Bcomponen s a e unchanged.
A e his ans o ma ion (15)–(16) s ays in a ian and in
consequence has a cylind ical symme y. This means ha ,
wi h he on-boa d measu emen s only, XS
1and XS
2can
no be compu ed, as any pai (XS
1,XS
2) ha sa is ies
(XS
1)2+(XS
2)2+(hBa o)2=∥XB∥2(17)
can be a solu ion o (15)–(16) o X. This could be
expec ed, as he e is no way o dis inguish he xSand yS
axes. The zSdi ec ion, howe e , is explici in (15)–(16) as a
consequence o he al ime e ’s measu emen s. 1No e ha ,
o he cu en p oblem, i is o no in e es o compu e
XS
1and XS
2independen ly bu he ho izon al dis ance
om he BV o he a ge , Xh=(XS
1)2+(XS
2)2, and
i s ela i e o ien a ion. Thus, wi hou losing any use ul
in o ma ion o guidance, he cylind ical symme y can be
b oken by o a ing Sso ha , a any gi en ime,
XS
1=Xh,X
S
2=0.(18)
This conside a ion educes in 1 he numbe o deg ees
o eedom o he sys em’s s a e wi hou educing he
in o ma ion a ailable o he guidance sys em. In o de o
de e mine X, as he e a e no addi ional measu emen s o
symme ies, 2 cons ain s a e needed. One use ul app oach
is o bene i om he BV’s ae odynamic geome y. Wi h-
ou any con ol ac ion, he ae odynamic momen s end o
align he longi udinal axis o he ehicle wi h he eloci y
ec o . A e a ansi o y egime, he eloci y ec o in
he Baxes can be simpli ied o VB=(
U00
)T. This
condi ion educes in wo he numbe o unknown a iables.
Le he addi ional measu emen ec o be
Za=
hBa o
γ1
γ2
˙
hBa o
˙γ1
˙γ2
0
0
0
=
−XS
3
a c an 2(XB
3,XB
1)
a c an 2(XB
2,XB
1)
−VS
3
XB
1V′B
3−XB
3V′B
1
�XB
12+�XB
32
XB
1V′B
2−XB
2V′B
1
�XB
12+�XB
22
XS
2
VB
2
VB
3
=h(X,Zp).
(19)
I can be p o ed ha he unc ion his locally in e ible
o Xinside a signi ican domain (see, e.g. Cla ke (1976)).
The e o e, i can be conside ed ha i s in e se exis s in
his egion, so ha he a ailable measu emen s allow o
compu e he ehicle’s s a e i he s a ing poin used o
sol e he nonlinea p oblem is close enough o he eal
s a e. This shows ha a na iga ion sys em should be
easible, as long as he noise e ms a e small enough.
1In p ac ice, a se o magne ome e s o magne ic compass is enough
o b eak his symme y so ha he e is a measu able ho izon al
e e ence. This is no he case o he conside ed p oblem.
2.3 Comple e p oblem o mula ion
The es ima ion p oblem s uc u e is as ollows. I he s a e
is known a a gi en ime, i s u u e alue can be ob ained
om he p opaga ion equa ion,
˙
X=
VS
−�B
IωB×VB+AGkB
S+AB
NG
1
2
B
Iq⊗0
B
IωB
= (X,Zp).
(20)
The eal p opaga ion measu emen s ˆ
Zpa e co up ed wi h
noise, his is, ˆ
Zp=Zp+δZp, whe e δZpa e modeled as
samples om whi e noise Gaussian independen p ocesses
(see, e.g. Johnson (2022))
δZp=δAB
Acc
δB
IωB
Gy ∼N
6(0,Σp).(21)
The e 9 a e addi ional measu emen s, Za, which con ain
in o ma ion abou he ajec o y. These measu emen s
can be compu ed om he s a e, wi h an addi i e noise
sampled om a whi e Gaussian noise
ˆ
Za=h(X,Zp)+δZa, δZa∼N
9(0,Σa),(22)
whe e and ha e nonlinea , and he ini ial condi ions a e
no a speci ic poin bu a wide p obabili y dis ibu ion.
The Fil e ing P oblem can be s a ed as ollows: F om
he a ailable measu emen s, unce ain ini ial condi ions
and hei expec ed s a is ical cha ac e is ics, a il e ing
algo i hm mus pe iodically compu e he sys em’s s a e.
3. PARTICLE FILTER FORMULATION
Fo his es ima ion p oblem, he ini ial s a e p obabili y
dis ibu ion is widesp ead . The p opaga ion and measu e-
men unc ions ,g a e mani es ly nonlinea wi hin his
domain, and he e o e a linea ized Kalman il e is no
adequa e o sol e he na iga ion p oblem.
The Pa icle Fil e (PF) makes use o he Mon e Ca lo
in eg a ion heo y and Bayes’ Theo em in o de o ob ain
an op imized es ima ion o nonlinea sys ems wi hou
nei he a linea app oxima ion no he Gaussian dis ibu-
ion hypo hesis (see e.g. Go don e al. (1993)). Ins ead,
i app oxima es a p obabili y dis ibu ion by a se o
pa icles ha a e p opaga ed and il e ed in pa allel. The
comple e p obabili y dis ibu ion is ob ained by means o
a Bayesian app oach. This amily o algo i hms is ex en-
si ely implemen ed in applica ions whe e i is needed o
deal wi h la ge unce ain ies and nonlinea i ies, such as
acking om ada da a. A Locally Linea ized Pa icle
Fil e (LLPF) is used as he na iga ion algo i hm o he
BV (see Ris ic e al. (2003)). As i is shown la e , he LLPF
s uc u e allows conside ing symme ies and cons ain s
na u ally as addi ional ixed measu emen s.
Le ˆ
Xi
kbe one es ima ion o he s a e, oge he wi h a
co a iance ma ix Pi
k, e e ed o as he pa icle i, a he
ime k. The algo i hm p opaga es a se o Nppa icles
ha cha ac e ize he s a e p obabili y dis ibu ion by
using a linea ized Kalman il e (EKF, UKF) (see, o
ins ance, Gelb (1974)). The PF assigns o each pa icle
ia posi i e weigh wi
k, compu ed by means o a Bayesian
app oach, ha is used o measu e he alue o a single
s a e es ima ion wi hin he se o pa icles. These weigh s
a e no malized and used as he p obabili y o each pa icle
in a esampling p ocess, o imp o e he quali y o he se
o pa icles o he nex i e a ion. This whole p ocess is
summa ized as ollows,
-Ini ial condi ions o k:
Nppa icles and weigh s, {ˆ
Xi
k,Pi
k,wi
k}
1Locally linea ized KF o each pa icle:
EKF,UKF →{ˆ
Xi+
k+1,Pi+
k+1,Pi−
k+1,Pi
νν}
2Compu e he new pa icles and weigh s:
ˆ
Xi
k+1 ∼N(ˆ
Xi+
k+1,Pi+
k+1)
˜wi
k+1 =
N(h(ˆ
Xi
k+1),P i
νν)(ˆ
Zk) N(ˆ
Xi−
k+1,P i−
k+1)(ˆ
Xi
k+1)
N(ˆ
Xi+
k+1,P i+
k+1)(ˆ
Xi
k+1)
3Weigh no maliza ion and esampling:
wi
k+1 =˜wi
k+1
Np
j=1 ˜wj
k+1
{ˆ
Xi
k+1,Pi
k+1}= Resample( ˆ
Xi
k+1,Pi
k+1,wi
k+1)
4Es ima ed s a e:
p(ˆ
Xk+1|ˆ
Xk,ˆ
Zk)≈
Np
i=1
1
Np
δ(ˆ
Xk+1 −ˆ
Xi
k+1)
ˆ
Xk+1 =
Np
i=1
1
Np
ˆ
Xi
k+1,
whe e δ(·) is he Di ac del a dis ibu ion. In he algo i hm,
N(ˆ
Xi−
k+1,P i−
k+1) e e s o he a p io i s a e Gaussian mul i-
a ia e p obabili y dis ibu ion es ima ed locally o each
pa icle iinside he UKF o EKF, N(ˆ
Xi+
k+1,P i+
k+1)is he
co esponding a pos e io i Gaussian mul i a ia e p obabil-
i y densi y and N(ˆ
Xi−
k+1,P i−
k+1)is he Gaussian mul i a ia e
densi y unc ion o he measu emen s.
The selec ed locally linea ized Kalman il e o his appli-
ca ion, due o i s obus ness o nonlinea unc ions, is he
Unscen ed Kalman Fil e (UKF) ex ended o qua e nion
a i ude ep esen a ion (K a (2003)).
-Ini ial condi ions o k:
ˆ
X( k)= ˆ
X+
k,P( k)=P+
k
1Compu e and p opaga e he Sigma Poin s:
Υk,i =ˆ
Xk±cols(NpChol(P′
k))
˙
Υi= (Υk,i,ˆ
Zp)→Υk+1,i,Z
i=h(Υk+1,i)
2UKF applica ion:
ˆ
X−
k+1 =1
2Np
2Np
i=1
Υk+1,i,¯
Z=1
2Np
2Np
i=1
Zi
P−
k+1 =1
2Np
2Np
i=1
2Np
j=1
(Υk+1,i −ˆ
X−
k+1)(Υk+1,i −ˆ
X−
k+1)′
Pxz =1
2Np
2Np
i=1
2Np
j=1
(Υk+1,i −ˆ
X−
k+1)(Zi−¯
Z)′
Pνν =R+1
2Np
2Np
i=1
2Np
j=1
(Zi−¯
Z)(Zi−¯
Z)′
K=PxzP−1
νν
ˆ
X+
k+1 =ˆ
X−
k+1 +K(ˆ
Za−¯
Z),P
+
k+1 =P−
k+1 −KPννK′
4512 Jose A. Rebollo e al. / IFAC Pape sOnLine 56-2 (2023) 4508–4513
whe e Chol(P) deno es he Cholesky decomposi ion o P.
In o de o implemen he qua e nion a i ude ep esen a-
ion in he s a e ˆ
Xk, se e al conside a ions mus be made
o bo h he UKF and he PF algo i hms. The main idea
ha allows o ex end he UKF and he PF o include
qua e nions is o use as an auxilia y ep esen a ion sys em
he a i ude ec o , which is minimal and does beha e
like a ec o . The s a e’s co a iance ma ix is compu ed
conside ing ha he a i ude is gi en by a o a ion ec o
θ(see, e.g. Wie (2015)). A e he Sma ix is ob ained,
wo sepa a e se s o Sigma Poin s a e s o ed; posi ion and
eloci y a e included in he ec o Sigma Poin VΥ, while
he 3 componen s associa ed o a i ude om each column
o S,θχ, a e used o compu e he qua e nion Sigma Poin s,
qΥ=B
Sˆq⊗
cos θχ
2
θχ
θχ
sin θχ
2
.(23)
Thus, a e he Sigma Poin s a e compu ed, he pai o
se s {VΥ,i,q
Υ,i},i∈{1,...,18}a e ob ained. These Sigma
Poin s can be used o de e mine he ime e olu ion and he
es ima ed addi ional measu emen s using he nonlinea
unc ions and hwi hou any addi ional modi ica ion.
Compu ing he co a iance ma ices in ol ing qΥis no
immedia e, since he de ini ion compu ing he di e ence
be ween ec o s does no hold o qua e nions. In ac , i
is necessa y o ob ain an al e na i e algo i hm o compu e
he mean o a se o qua e nions. Le {qi},i ∈{1,...,n}
be a se o a i ude qua e nions. Le ⟨q⟩be he mean
qua e nion. The o a ion qua e nion i om he mean o
any o he elemen s o he se e i ies he gene al o a ion
composi ion ela ion om he Hamil on p oduc qi=
i⊗⟨q⟩. In consequence, he se o o a ion qua e nions
be ween qiand ⟨q⟩a e gi en by i=qi⊗¯
⟨q⟩. Each o a ion
iis equi alen o a o a ion ec o θ ,i, so ha
θ ,i =2 i
∥ i∥a ccos i,0.(24)
Thus, he angle be ween any e e ence ame ep esen ed
by qiand he mean o ien a ion gi en by ⟨q⟩is θ ,i. I ⟨q⟩is
he mean qua e nion, he mean o a ion ec o ⟨θ ⟩mus
be ze o. I ⟨q⟩is no he mean qua e nion, ⟨θ ⟩is nonze o
and o ien ed owa ds he eal mean di ec ion. Using his
p ope y, he mean qua e nion o a se can be ob ained by
using an i e a i e algo i hm. In pa icula , he p oposed
in insic g adien descen desc ibed in Pennec (1998) is
implemen ed. This echnique is e y in e es ing o he
cu en applica ion since he inal se o o a ion ec o s
θ ,i is equi alen o he di e ence xi−⟨x⟩when compu ing
he co a iance ma ix o a se o ec o s xi. Hence, inside
he UKF, he e m Υk+1,i −ˆ
X−
k+1 is subs i u ed by
Υk+1,i −ˆ
X−
k+1 ≡
XS
Υ,k+1,i −ˆ
XS,−
k+1
VB
Υ,k+1,i −ˆ
VB,−
k+1
θ ,i
(25)
whe e XS
Υ,k+1,i and VB
Υ,k+1,i a e he posi ion and eloci y
Sigma Poin s. A e he s a e change is compu ed in
he Kalman Fil e , he h ee componen s desc ibing he
change in a i ude a e con e ed o a o a ion qua e nion
and applied o he unco ec ed a i ude qua e nion o
compu e he il e ed a i ude.
The LLPF was gene alized o include qua e nions ollow-
ing a simila easoning. In his case, he no mal mul i-
a ia e p obabili y dis ibu ions ha e o be ex ended o
use qua e nions as an a i ude ep esen a ion sys em. This
si ua ion appea s when e alua ing o each pa icle
N(ˆ
Xi±
k+1,P i±
k+1)=e−1
2(ˆ
Xi
k+1−ˆ
Xi±
k+1)′(Pi±
k+1)−1(ˆ
Xi
k+1−ˆ
Xi±
k+1)
(2π)9|Pi±
k+1|
,
o bo h he a p io i and a pos e io i dis ibu ions. No e
ha ( ˆ
Xi
k+1 −ˆ
Xi−
k+1) and ( ˆ
Xi
k+1 −ˆ
Xi+
k+1) a e no de ined
o he qua e nion pa . The LLPF can be ex ended by
conside ing he ollowing equi alences
ˆ
Xi
k+1 −ˆ
Xi±
k+1 ≡
ˆ
XS,i
k+1 −ˆ
XS,i±
k+1
ˆ
VB,i
k+1 −ˆ
VB,i±
k+1
θ±
k+1
,(26)
whe e
±
k+1 =B
Sˆqi
k+1 ⊗B
S¯
ˆqi±
k+1,θ±
k+1 =2
±
k+1
∥ ±
k+1∥a ccos ±
k+1,0.
A co ec ion is needed o he imposed symme y condi-
ion, as s a ed in (18), which can o he wise be p oblem-
a ic. I XS
2≪XS
1, se ing XS
2 o ze o does no change
signi ican ly he ho izon al dis ance om he BV o he
a ge , nei he he BV’s o ien a ion wi h espec o he
S e e ence sys em. I , on he con a y, XS
2is ep esen a-
i e agains XS
1, he conside ed equa ion can lead o an
undesi able educ ion o he dis ance o he a ge and
an unexpec ed o a ion o he Baxes ela i e o he S
axes, hus educing he algo i hm’s o e all pe o mance.
To gua an ee ha his equa ion beha es as o a ion, he
ollowing co ec ions can be applied. Le XS
0be he posi-
ion be o e applying he il e , and XS
i s alue a e he
il e ing p ocess. Thei ho izon al angle is
cos ψ=XS
0,1XS
,1+XS
0,2XS
,2
�XS
0,12+�XS
0,22XS
,12+XS
,22,(27)
whe e only he ho izon al componen s we e conside ed.
Thus, he co ec ed dis ance a e il e ing is gi en by
X′S
,1=XS
,1
cos ψ.(28)
As o he needed a i ude co ec ion o B
Sq, i can
be ob ained by in oducing a o a ion gi en by q′=
(cos ψ/200 sinψ/2)T. Unde his algo i hm, he pa i-
cles dis ibu ion can be p opaga ed eely om he a ail-
able in o ma ion wi hou cons ain s, and co ec ed when
he lase ecei e is ac i e. This allows o main ain all
he ini ially a ailable in o ma ion by p opaga ing he un-
cons ained pa icles. When LOS measu emen s can be
used, he conside ed equa ions a e included in he il e ing
algo i hm.
4. RESULTS
The p oposed na iga ion algo i hm is implemen ed and
es ed in simula ion. The ini ial s a e is se o
XS
0,1=(
−3670 0 −2000)Tm,VB
0,1=(
200 0 0)Tm/s,
B
Sq0,1=(
1000
)T,B
IωB
0,1=0 ad/s.
The a iances o he addi i e Gaussian whi e noise o
measu emen s a e se o σ2
h= 100 m2,σ2
a=2·10−2m2/s4,
σ2
ω=5·10−6 ad2/s2and σ2
γ=5·10−5 ad2. The
ini ial pa icles dis ibu ion is sampled om a Gaussian
mul i a ia e s a e p obabili y densi y,
ˆ
Xi
0∼N(µX,0,ΣX,0),i∈[1,N
p].(29)
Jose A. Rebollo e al. / IFAC Pape sOnLine 56-2 (2023) 4508–4513 4513
whe e Chol(P) deno es he Cholesky decomposi ion o P.
In o de o implemen he qua e nion a i ude ep esen a-
ion in he s a e ˆ
Xk, se e al conside a ions mus be made
o bo h he UKF and he PF algo i hms. The main idea
ha allows o ex end he UKF and he PF o include
qua e nions is o use as an auxilia y ep esen a ion sys em
he a i ude ec o , which is minimal and does beha e
like a ec o . The s a e’s co a iance ma ix is compu ed
conside ing ha he a i ude is gi en by a o a ion ec o
θ(see, e.g. Wie (2015)). A e he Sma ix is ob ained,
wo sepa a e se s o Sigma Poin s a e s o ed; posi ion and
eloci y a e included in he ec o Sigma Poin VΥ, while
he 3 componen s associa ed o a i ude om each column
o S,θχ, a e used o compu e he qua e nion Sigma Poin s,
qΥ=B
Sˆq⊗
cos θχ
2
θχ
θχ
sin θχ
2
.(23)
Thus, a e he Sigma Poin s a e compu ed, he pai o
se s {VΥ,i,q
Υ,i},i∈{1,...,18}a e ob ained. These Sigma
Poin s can be used o de e mine he ime e olu ion and he
es ima ed addi ional measu emen s using he nonlinea
unc ions and hwi hou any addi ional modi ica ion.
Compu ing he co a iance ma ices in ol ing qΥis no
immedia e, since he de ini ion compu ing he di e ence
be ween ec o s does no hold o qua e nions. In ac , i
is necessa y o ob ain an al e na i e algo i hm o compu e
he mean o a se o qua e nions. Le {qi},i ∈{1,...,n}
be a se o a i ude qua e nions. Le ⟨q⟩be he mean
qua e nion. The o a ion qua e nion i om he mean o
any o he elemen s o he se e i ies he gene al o a ion
composi ion ela ion om he Hamil on p oduc qi=
i⊗⟨q⟩. In consequence, he se o o a ion qua e nions
be ween qiand ⟨q⟩a e gi en by i=qi⊗¯
⟨q⟩. Each o a ion
iis equi alen o a o a ion ec o θ ,i, so ha
θ ,i =2 i
∥ i∥a ccos i,0.(24)
Thus, he angle be ween any e e ence ame ep esen ed
by qiand he mean o ien a ion gi en by ⟨q⟩is θ ,i. I ⟨q⟩is
he mean qua e nion, he mean o a ion ec o ⟨θ ⟩mus
be ze o. I ⟨q⟩is no he mean qua e nion, ⟨θ ⟩is nonze o
and o ien ed owa ds he eal mean di ec ion. Using his
p ope y, he mean qua e nion o a se can be ob ained by
using an i e a i e algo i hm. In pa icula , he p oposed
in insic g adien descen desc ibed in Pennec (1998) is
implemen ed. This echnique is e y in e es ing o he
cu en applica ion since he inal se o o a ion ec o s
θ ,i is equi alen o he di e ence xi−⟨x⟩when compu ing
he co a iance ma ix o a se o ec o s xi. Hence, inside
he UKF, he e m Υk+1,i −ˆ
X−
k+1 is subs i u ed by
Υk+1,i −ˆ
X−
k+1 ≡
XS
Υ,k+1,i −ˆ
XS,−
k+1
VB
Υ,k+1,i −ˆ
VB,−
k+1
θ ,i
(25)
whe e XS
Υ,k+1,i and VB
Υ,k+1,i a e he posi ion and eloci y
Sigma Poin s. A e he s a e change is compu ed in
he Kalman Fil e , he h ee componen s desc ibing he
change in a i ude a e con e ed o a o a ion qua e nion
and applied o he unco ec ed a i ude qua e nion o
compu e he il e ed a i ude.
The LLPF was gene alized o include qua e nions ollow-
ing a simila easoning. In his case, he no mal mul i-
a ia e p obabili y dis ibu ions ha e o be ex ended o
use qua e nions as an a i ude ep esen a ion sys em. This
si ua ion appea s when e alua ing o each pa icle
N(ˆ
Xi±
k+1,P i±
k+1)=e−1
2(ˆ
Xi
k+1−ˆ
Xi±
k+1)′(Pi±
k+1)−1(ˆ
Xi
k+1−ˆ
Xi±
k+1)
(2π)9|Pi±
k+1|
,
o bo h he a p io i and a pos e io i dis ibu ions. No e
ha ( ˆ
Xi
k+1 −ˆ
Xi−
k+1) and ( ˆ
Xi
k+1 −ˆ
Xi+
k+1) a e no de ined
o he qua e nion pa . The LLPF can be ex ended by
conside ing he ollowing equi alences
ˆ
Xi
k+1 −ˆ
Xi±
k+1 ≡
ˆ
XS,i
k+1 −ˆ
XS,i±
k+1
ˆ
VB,i
k+1 −ˆ
VB,i±
k+1
θ±
k+1
,(26)
whe e
±
k+1 =B
Sˆqi
k+1 ⊗B
S¯
ˆqi±
k+1,θ±
k+1 =2
±
k+1
∥ ±
k+1∥a ccos ±
k+1,0.
A co ec ion is needed o he imposed symme y condi-
ion, as s a ed in (18), which can o he wise be p oblem-
a ic. I XS
2≪XS
1, se ing XS
2 o ze o does no change
signi ican ly he ho izon al dis ance om he BV o he
a ge , nei he he BV’s o ien a ion wi h espec o he
S e e ence sys em. I , on he con a y, XS
2is ep esen a-
i e agains XS
1, he conside ed equa ion can lead o an
undesi able educ ion o he dis ance o he a ge and
an unexpec ed o a ion o he Baxes ela i e o he S
axes, hus educing he algo i hm’s o e all pe o mance.
To gua an ee ha his equa ion beha es as o a ion, he
ollowing co ec ions can be applied. Le XS
0be he posi-
ion be o e applying he il e , and XS
i s alue a e he
il e ing p ocess. Thei ho izon al angle is
cos ψ=XS
0,1XS
,1+XS
0,2XS
,2
�XS
0,12+�XS
0,22XS
,12+XS
,22,(27)
whe e only he ho izon al componen s we e conside ed.
Thus, he co ec ed dis ance a e il e ing is gi en by
X′S
,1=XS
,1
cos ψ.(28)
As o he needed a i ude co ec ion o B
Sq, i can
be ob ained by in oducing a o a ion gi en by q′=
(cos ψ/200 sinψ/2)T. Unde his algo i hm, he pa i-
cles dis ibu ion can be p opaga ed eely om he a ail-
able in o ma ion wi hou cons ain s, and co ec ed when
he lase ecei e is ac i e. This allows o main ain all
he ini ially a ailable in o ma ion by p opaga ing he un-
cons ained pa icles. When LOS measu emen s can be
used, he conside ed equa ions a e included in he il e ing
algo i hm.
4. RESULTS
The p oposed na iga ion algo i hm is implemen ed and
es ed in simula ion. The ini ial s a e is se o
XS
0,1=(
−3670 0 −2000)Tm,VB
0,1=(
200 0 0)Tm/s,
B
Sq0,1=(
1000
)T,B
IωB
0,1=0 ad/s.
The a iances o he addi i e Gaussian whi e noise o
measu emen s a e se o σ2
h= 100 m2,σ2
a=2·10−2m2/s4,
σ2
ω=5·10−6 ad2/s2and σ2
γ=5·10−5 ad2. The
ini ial pa icles dis ibu ion is sampled om a Gaussian
mul i a ia e s a e p obabili y densi y,
ˆ
Xi
0∼N(µX,0,ΣX,0),i∈[1,N
p].(29)
The ini ial bias µX,0o he s a e es ima ion is o 80 m
o each posi ion coo dina e, 15 m/s o each eloci y com-
ponen and 0.3 ad o each Eule angle. The co a iance
ma ix desc ibing he ini ial unce ain y is se o
ΣX,0= Diag (8000 8000 8000 200 200 200 0.05 0.05 0.05)T
.
A se o Np= 200 pa icles is conside ed, wi h an
upda e equency o 40 Hz. The Fil e is p og ammed using
C++ on a In el(R) Co e(TM) i7-3537U wi h no GPU
accele a ion. Wi h his limi ed capaci y, he simula ions
including he 40 Hz na iga ion sys em a e execu ed wi h
a a io o a ound 1 second o compu a ion pe simula ion
second, hus he algo i hm ope a es in eal ime.
Figu e 1 shows he pa icle ajec o ies. The algo i hm
p opaga es and il e s he pa icles dis ibu ion consid-
e ing only he in o ma ion accessible on-boa d. No e he
signi ican unce ain y p io o he con e gence o he na -
iga ion sys em. as a consequence o he lack o in o ma ion
on he sys em’s s a e. When he LOS measu emen is
a ailable, a apid con e gence is ob ained.
Fig. 1. Pa h o each pa icle in he na iga ion algo i hm
The p oposed na iga ion algo i hm co ec ly con e ges o
he desi ed s a e. Mo eo e , he ime be ween he LOS
ini ial measu emen s and con e gence is, in mos o he
simula ions, e y sho , as a consequence o he esam-
pling p ocess. Each ime a pa icle ob ains a e y good
app oxima ion o he BV’s s a e, all he o he pa icles
a e esampled o ha s a e, as i s weigh is nea ly 1.
Thus, he hype olume illed by he pa icles dis ibu ion
wi hin he s a e space is signi ican ly educed. A e he
esampling s ep, each pa icle andomly p opaga es o a
di e en s a e, sligh ly inc easing he size o he pa icles
dis ibu ion and he e o e a oiding degene acy. This ex-
cellen beha io is ob ained in a a ie y o ini ial condi-
ions, unce ain y and measu emen noise. The pa ame e s
wi hin he qua e nion UKF and he numbe o pa icles
can be uned o imp o e pe o mance o a gi en se up.
5. CONCLUSIONS
This wo k in oduced a Locally Linea ized Pa icle Fil e
based on UKF and symme y exploi a ion. The p oposed
na iga ion algo i hm con e ges apidly o an accu a e
es ima e when he lase ecei e ac i a es. Al hough he
numbe o pa icles used in he PF was small, he LLPF
pe o med well in he simula ions. To imp o e he LLPF’s
pe o mance in scena ios wi h mo e unce ain ies o mea-
su ing noise, he numbe o pa icles could be inc eased,
bu his would inc ease compu a ional cos s. The pa icles
used we e sampled om an a bi a y p obabili y dis ibu-
ion, bu i he ini ial s a e en elope is known, he pa icles
can be ini ialized acco dingly, esul ing in mo e accu a e
es ima es. Thus, he algo i hm can be easily modi ied by
changing he ini ial p obabili y dis ibu ion o include any
a ailable in o ma ion abou he ope a ion. The s a ing se
o pa icles can be ailo ed o any ope a ional condi ions
wi hou any knowledge o il e ing heo y.
These esul s could be ex ended o o he ehicle s a e es-
ima ion p oblems such as GPS-denied ai c a na iga ion
o spacec a endez ous wi h non-coope a i e umbling
a ge s (e.g. space deb is).
ACKNOWLEDGEMENTS
We acknowledge suppo by g an TED2021-132099B-C33
unded by MCIN/ AEI/ 10.13039 /501100011033 and by
“Eu opean Union Nex Gene a ionEU/PRTR.”
REFERENCES
Cla ke, F. (1976). On he in e se unc ion heo em. Paci ic
Jou nal o Ma hema ics, 64(1), 97–102.
Gelb, A. (1974). Applied Op imal Es ima ion. The MIT
P ess.
Go don, N.J., Salmond, D.J., and Smi h, A.F. (1993).
No el app oach o nonlinea /non-Gaussian Bayesian
s a e es ima ion. IEE p oceedings F, 140(2), 107–113.
Johnson, R.A. (2022). Applied Mul i a ia e S a is ical
Analysis. Pea son College Di , subsequen edi ion.
K a , E. (2003). A qua e nion-based unscen ed Kalman
il e o o ien a ion acking. In P oceedings o FUSION
2003, olume 1, 47–54. IEEE.
Lin, Z., Yao, Y., and Ma, K.M. (2005). The design o LOS
econs uc ion il e o s ap-down imaging seeke . In
2005 ICMLC, olume 4, 2272–2277.
Ma, C., Zheng, Z., Chen, J., and Yuan, J. (2020). Je
anspo pa icle il e o a i ude es ima ion o um-
bling space objec s. Ae ospace Science and Technology,
107, 106330.
Maley, J.M. (2015). Line o sigh a e es ima ion o
guided p ojec iles wi h s apdown seeke s. In AIAA
GNC Con e ence, 0344.
Pennec, X. (1998). Compu ing he mean o geome ic
ea u es applica ion o he mean o a ion. Ph.D. hesis,
INRIA.
Ris ic, B., A ulampalam, S., and Go don, N. (2003).
Beyond he Kalman Fil e : Pa icle Fil e s o T acking
Applica ions. A ech House.
Rudin, R. (1993). S apdown s abiliza ion o imaging
seeke s. In Annual In e cep o Technology Con e ence,
2660.
Tae-Hun, K., Jong-Han, K., and Philsung, K. (2017). New
guidance il e s uc u e o homing missiles wi h s ap-
down IIR seeke . In e na ional Jou nal o Ae onau ical
and Space Sciences, 18(4), 757–766.
Wei, C., Han, Y., Cui, N., and Xu, H. (2017). Fi h-deg ee
cuba u e Kalman il e es ima ion o seeke line-o -sigh
a e using augmen ed-dimensional model. Jou nal o
Guidance, Con ol, and Dynamics, 40(9), 2355–2362.
Wie, B. (2015). Space Vehicle Dynamics and Con ol.
AIAA, 3 d edi ion.
Wu, A.D., Johnson, E.N., Kaess, M., Dellae , F., and
Chowdha y, G. (2013). Au onomous ligh in GPS-
denied en i onmen s using monocula ision and ine ial
senso s. Jou nal o Ae ospace In o ma ion Sys ems,
10(4), 172–186.
