A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle
Abstract
The problem of state estimation for vehicles when an initial fix is highly uncertain and/or the number of sensors is not sufficient (and changes with time) is very relevant for both aircraft and spacecraft navigation. This work proposes a Locally Linearized Particle Filter based on a quaternion-adapted Unscented Kalman Filter to estimate the state of a vehicle with minimal sensors and uncertain initial conditions, exploiting geometrical symmetries. The algorithm is applied to a ballistic vehicle navigating towards a laser-illuminated target using on-board sensors, including a triad of accelerometers and gyroscopes, a barometric altimeter and a laser receiver. A symmetry around the vertical axis is identified; based on it, the algorithm becomes capable of solving the navigation problem, even with highly uncertain initial conditions and without enough sensor information; this second condition is particularly severe when the laser receiver is not yet obtaining data. The proposed navigation algorithm offers promising results in simulation, rapidly converging to an accurate estimate of the real trajectory when the laser receiver becomes active.
Full text
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.