A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle

IFAC PapersOnLine 56-2 (2023) 4508–4513 ScienceDirect Available online at www.sciencedirect.com 2405-8963 Copyright © 2023 The Authors. This is an open access article under the CC BY-NC-ND license . Peer review under responsibility of International Federation of Automatic Control. 10.1016/j.ifacol.2023.10.942 10.1016/j.ifacol.2023.10.942 2405-8963 Copyright © 2023 The Authors. This is an open access article under the CC BY-NC-ND license ( https://creativecommons.org/licenses/by-nc-nd/4.0/ ) A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗ Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain (josr[email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ( { jgcordero,jjimenez } @aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the Jose A. Rebollo et al. / IFAC PapersOnLine 56-2 (2023) 4508–4513 4509 Copyright © 2023 The Authors. This is an open access article under the CC BY-NC-ND license ( https://creativecommons.org/licenses/by-nc-nd/4.0/ ) A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the A Symmetry-Based Unscented Particle Filter for State Estimation of a Ballistic Vehicle Jose A. Rebollo ∗Rafael Vazquez ∗Francisco Gavilan ∗ Jorge Cordero ∗∗ Javier Jimenez ∗∗ ∗Dpto. de Ingenier´ıa Aeroespacial, Universidad de Sevilla, Camino de los Descubrimientos s/n, 41092, Sevilla, Spain ([email protected], {rvazquez1,fgavilan}@us.es) ∗∗ AERTEC Solutions S.L., C/ Wilbur y Orville Wright, 31, 41309 - La Rinconada, Spain ({jgcordero,jjimenez}@aertecsolutions.com) 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. Keywords: Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation. 1. INTRODUCTION Frequently, one needs to solve the problem of state estimation for vehicles (both aircraft and spacecraft) in situations where an initial fix, if available, contains large uncertainties, and/or the number and quality of sensors is not sufficient or is changing with time. Some examples include GPS-denied aircraft navigation (see, e.g., Wu et al. (2013)), spacecraft rendezvous with non-cooperative tumbling targets such as space debris (see, e.g., Ma et al. (2020)) or ballistic vehicles traveling towards nonmaneuvering ground targets (see, e.g., Wei et al. (2017)), being this last example the one considered in this work. In particular, this paper considers the problem of online state reconstruction of a ballistic vehicle, traveling towards a target illuminated by laser; in this formulation of a Navigation Problem, one needs to reconstruct with on board data and in real time the relative position of the target, and the vehicle’s velocity and attitude. This information can then be used to implement, for instance, a predictive guidance system. The available sensors are a triad of accelerometers and gyroscopes, a barometric altimeter, and a laser receiver that only activates when the target is close enough, giving then the Line of Sight (LOS) angles (this is typically known as a strapdown seeker in the literature; they are considered superior to platform seekers for their simpler structure, higher reliability, smaller size, and lighter weight, see, e.g. Wei et al. (2017)). The initial fix is only very approximately known. To solve the problem, the authors propose a Locally Linearized Particle Filter (LLPF), based on a quaternionadapted Unscented Kalman Filter (UKF) to estimate the state of a vehicle with a minimal number of sensors and uncertain initial conditions, by exploiting the geometrical symmetries of the problem. Particle filters (see, for instance, Ristic et al. (2003)) have established themselves as a feasible option for tracking and estimation in aerospace problems, and have become feasible with today’s computational means. They can deal with large uncertainties and nonlinearities, and can be combined with local filters inheriting their properties, such as the UKF in this case. While, to the authors’ knowledge, there are no previous work considering this particular problem, there are other contributions related to strapdown seekers. In particular, the problem of line-of-sight (LOS) rate reconstruction has been widely studied. Since strapdown seekers fixed to a vehicle bodies cannot directly provide this rate information, which is essential for proportional navigation guidance laws, it is of great significance to establish an appropriate estimation model and design the corresponding filter, so as to obtain more accurate rates. Next, a very brief review of some significant results in the area is given. For instance, Wei et al. (2017) considered this problem, proposing a fifth-degree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates. Lin et al. (2005) proposed a LOS reconstruction filter based on an exact LOS dynamic model of strap-down seeker, to which the theory of unscented transformation (UT) and unscented Kalman filter was applied to treat the nonlinearities. Maley (2015) addressed the problem through the development of a line of sight rate extended Kalman filter, which was able to accommodate significant time delays due to the processing requirements of computer vision algorithms of the seeker. Finally, one can also cite the results of Tae-Hun et al. (2017), which consider a parasitic instability effect due to the seeker’s latency; by introducing a new state vector representation along with the Pade approximation for compensating the time-delay of the seeker, this work proposed a new guidance filter structure based on an extended Kalman filter. The main contributions of the present work are twofold. First, a LLPF that exploits the symmetries of the problem with a minimal and reasonable number of sensors, under realistic conditions for the activation of the laser receiver, is formulated. Secondly, this LLPF is based on an UKF which is tailored to the specific problem, in particular respecting the invariants arising due to the use of quaternions, even though other such filters already exist (see, for instance, Kraft (2003)). Simulations show an excellent performance of the algorithm under reasonable sensor noises and initial uncertainties, with the estimation rapidly converging to an accurate estimate of the real trajectory when the laser receiver becomes active. 2. PROBLEM STATEMENT 2.1 Kinematic and ideal measurements description Regarding notation, vectors are denoted by bold variables. A vector aevaluated in a reference frame (A) is written as aA, while its components are given by aA j,j =1,2,3. In this study, we consider a free-fall ballistic vehicle (BV), launched from a Remotely Piloted Aircraft (RPAS); the BV’s trajectory could also contain some guided segments, but this guidance is not taken into consideration in this work. The vehicle is modeled as fixed mass rigid body subject to free fall motion in a real atmosphere. OBis defined as the center of mass of the BV, while OSis the illuminated target. Three reference frames are considered for convenience. Firstly, the Surface (S) reference frame is centered in OSand moves along with the Earth’s surface. The zSaxis points towards the center of the Earth while xSand ySare oriented arbitrarily following a right-hand structure. Secondly, the Body (B) frame is centered in OB, and rotates with the BV. Considering a cylindrical shaped BV with a vertical plane of symmetry, xBis aligned with the longitudinal axis, frontwards; zBlies in the plane of symmetry, downwards; and yBpoints rightwards, fulfilling a right-handed frame. Finally, a generic Inertial (I) reference frame is considered, with arbitrary center and orientation. Let XSbe the position of the center of mass of the BV referred to (S). Let VS=˙ XS, and VBits components in B. Let S IωSand B IωBbe the angular velocities of the Earth and the BV, respectively. AB Gand AB NG are defined as the gravitational and non-gravitational inertial accelerations in OB. For the attitude representation, the attitude quaternion B Sqof the Bframe with respect to S is used (see, for instance, Wie (2015)). For the current problem, several simplifying hypotheses can be made, without introducing appreciable errors. On one hand, the effect of the Earth’s rotation is minimal, so that S IωBcan be approximated to zero. For this particular problem, the expected flight time of the BV is of less than one minute, thus the effective rotation of the Earth with respect to an inertial frame during that time interval is clearly imperceptible. On the other hand, if the Earth’s curvature is neglected, by approximating the Earth’s surface by its locally tangent plane, the gravitational acceleration is always aligned with kS, this is, the third vector of the Scanonical basis. By doing so, the equations of motion of the BV are reduced to ˙ 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) Note that kB Sis the kSvector evaluated in the Breference frame. AGis the standard mean gravitational acceleration in the Earth’s surface. The ⊗operator denotes the quaternion product. Rotations between reference frames can be performed with quaternions (see Wie (2015)). In order to compute the state of the BV, several onboard sensors are available: a set of 3 accelerometers and 3 gyroscopes, a barometric altimeter and a laser receiver. For ideal sensors, not including measurement errors, which are characterized later, the corresponding ideal measurement models can be written as AB Acc =˙ VB+B IωB×VB−AB G,(4) B IωB Gyr =B IωB,(5) hBaro =−XS 3,(6) γ1= arctan 2(XB 3,XB 1),(7) γ2= arctan 2(XB 2,XB 1),(8) where AB Gis the gravitational acceleration expressed in the Bbasis. If the trajectory starting point was known, without considering error accumulation and with an exact model for gravity, the accelerometer and gyroscope measurements suffice to solve the Navigation Problem. For that reason, these 2 vectors define the propagation measurements, Zp, so that Zp=AB Acc B IωB Gyr.(9) As for the laser measurements, the laser beam deviation from the vehicle’s main axis is measured in terms of two angles, which are referred to as γ1and γ2, corresponding to the deviation of the laser direction from the xBaxis projected in the xBzBand xByBplanes, respectively. The LOS angles measurements are not always available during the BV’s operation. In particular, two conditions must be satisfied. Firstly, the laser path must be reasonably aligned with the BV longitudinal axis, so that the LOS measurements are inside the so called Field of View (FOV) of the optical sensor used (Rudin (1993)). In this application, the FOV is of 15 degrees for each angle. Secondly, the distance from the point source to the sensor must not be larger than a threshold which, in this case, is considered of 2500 m. Thus, only if −15◦<γ 1,γ 2<15◦ and ∥XS∥<2500 m, the LOS angles can be considered. For reasons that are detailed later, the time derivatives of hand γiare of interest, despite that they are not directly measured. Considering the kinematic evolution, these values are given by 4510 Jose A. Rebollo et al. / IFAC PapersOnLine 56-2 (2023) 4508–4513 vh=˙ h=−VS 3,(10) ˙γ1=XB 1d dt XB 3−XB 3d dt XB 1 �XB 12+�XB 32,(11) ˙γ2=XB 1d dt XB 2−XB 2d dt XB 1 �XB 12+�XB 22,(12) for d dtXB=−�B IωB×XB+VB.(13) As the angular velocity is continuously measured and the position, velocity and attitude are state variables, ˙γ1and ˙γ2are completely defined provided the state is known. Note that, as (h, γ1,γ 2) only depend on geometric parameters, while their derivatives also depend on both linear and angular velocities, this second set of parameters can not be obtained by algebraic combination of the first one and vice versa, consequently guaranteeing that these 6 measurements are independent, or what is the same, they provide information that is not cross correlated. This result is proved to be of great interest later on. 2.2 State determination from constraints and symmetries Let Xbe the state 10-uple to be computed, defined as X=  XS VB B Sq .(14) Note that Xdoes not belong to a vector space, as it contains the components of an attitude quaternion, which belongs to the space of a 4-D hypersphere (Wie (2015)). The state of the BV has 9 degrees of freedom. Thus, since no initial fix is available, a set of 9 independent equations given by measurable magnitudes are needed to fully characterize X. The first major difficulty of the estimation problem is to determine to what extent can the BV’s state be computed only from the available measurements. In this section, in order to simplify the analysis, measurement errors are not considered. The sensors’ noise sources are taken into consideration during the filtering approach later on. Provided that the laser is visible for the BV’s optical sensor, 3 equations can be written for the system’s state at a given time, Za=hBaro γ1 γ2=  −XS 3 arctan 2(XB 3,XB 1) arctan 2(XB 2,XB 1) =h1(X).(15) These equations are insufficient to compute Xinverting h. As long as the sampling frequency for Zais adequate, its derivative can be obtained computationally, leading to ˙ Za=       −VS 3 XB 1V′B 3−XB 3V′B 1 �XB 12+�XB 32 XB 1V′B 2−XB 2V′B 1 �XB 12+�XB 22        =h2(X,Zp).(16) Note that, besides computational or measurement errors, a higher order derivative of Zacan not be considered as there appear terms from ˙ Zpwhich are not available and can not be determined. With 3 additional measurements or constraints, so that h(X, Zp) is invertible for X, the BV’s state would be completely determined from the available information on-board. One interesting property of this estimation problem, which can be applied to reduce the number of unknown variables, is that there is a spatial symmetry. Indeed, on one hand, Zpdepends only on measurements on the Bframe. On the other hand, the only vector components written in the Saxes in (15)–(16) are XS 3and VS 3. Thus, it is useful to consider a rotation of the Sreference frame around the zS axis, so that the Bcomponents are unchanged. After this transformation (15)–(16) stays invariant and in consequence has a cylindrical symmetry. This means that, with the on-board measurements only, XS 1and XS 2can not be computed, as any pair (XS 1,XS 2) that satisfies (XS 1)2+(XS 2)2+(hBaro)2=∥XB∥2(17) can be a solution to (15)–(16) for X. This could be expected, as there is no way to distinguish the xSand yS axes. The zSdirection, however, is explicit in (15)–(16) as a consequence of the altimeter’s measurements. 1Note that, for the current problem, it is of no interest to compute XS 1and XS 2independently but the horizontal distance from the BV to the target, Xh=(XS 1)2+(XS 2)2, and its relative orientation. Thus, without losing any useful information for guidance, the cylindrical symmetry can be broken by rotating Sso that, at any given time, XS 1=Xh,X S 2=0.(18) This consideration reduces in 1 the number of degrees of freedom of the system’s state without reducing the information available for the guidance system. In order to determine X, as there are no additional measurements or symmetries, 2 constraints are needed. One useful approach is to benefit from the BV’s aerodynamic geometry. Without any control action, the aerodynamic moments tend to align the longitudinal axis of the vehicle with the velocity vector. After a transitory regime, the velocity vector in the Baxes can be simplified to VB=( U00 )T. This condition reduces in two the number of unknown variables. Let the additional measurement vector be Za=             hBaro γ1 γ2 ˙ hBaro ˙γ1 ˙γ2 0 0 0             =                    −XS 3 arctan 2(XB 3,XB 1) arctan 2(XB 2,XB 1) −VS 3 XB 1V′B 3−XB 3V′B 1 �XB 12+�XB 32 XB 1V′B 2−XB 2V′B 1 �XB 12+�XB 22 XS 2 VB 2 VB 3                    =h(X,Zp). (19) It can be proved that the function his locally invertible for Xinside a significant domain (see, e.g. Clarke (1976)). Therefore, it can be considered that its inverse exists in this region, so that the available measurements allow to compute the vehicle’s state if the starting point used to solve the nonlinear problem is close enough to the real state. This shows that a navigation system should be feasible, as long as the noise terms are small enough. 1In practice, a set of magnetometers or magnetic compass is enough to break this symmetry so that there is a measurable horizontal reference. This is not the case for the considered problem. Jose A. Rebollo et al. / IFAC PapersOnLine 56-2 (2023) 4508–4513 4511 vh=˙ h=−VS 3,(10) ˙γ1=XB 1d dt XB 3−XB 3d dt XB 1 �XB 12+�XB 32,(11) ˙γ2=XB 1d dt XB 2−XB 2d dt XB 1 �XB 12+�XB 22,(12) for d dtXB=−�B IωB×XB+VB.(13) As the angular velocity is continuously measured and the position, velocity and attitude are state variables, ˙γ1and ˙γ2are completely defined provided the state is known. Note that, as (h, γ1,γ 2) only depend on geometric parameters, while their derivatives also depend on both linear and angular velocities, this second set of parameters can not be obtained by algebraic combination of the first one and vice versa, consequently guaranteeing that these 6 measurements are independent, or what is the same, they provide information that is not cross correlated. This result is proved to be of great interest later on. 2.2 State determination from constraints and symmetries Let Xbe the state 10-uple to be computed, defined as X=  XS VB B Sq .(14) Note that Xdoes not belong to a vector space, as it contains the components of an attitude quaternion, which belongs to the space of a 4-D hypersphere (Wie (2015)). The state of the BV has 9 degrees of freedom. Thus, since no initial fix is available, a set of 9 independent equations given by measurable magnitudes are needed to fully characterize X. The first major difficulty of the estimation problem is to determine to what extent can the BV’s state be computed only from the available measurements. In this section, in order to simplify the analysis, measurement errors are not considered. The sensors’ noise sources are taken into consideration during the filtering approach later on. Provided that the laser is visible for the BV’s optical sensor, 3 equations can be written for the system’s state at a given time, Za=hBaro γ1 γ2=  −XS 3 arctan 2(XB 3,XB 1) arctan 2(XB 2,XB 1) =h1(X).(15) These equations are insufficient to compute Xinverting h. As long as the sampling frequency for Zais adequate, its derivative can be obtained computationally, leading to ˙ Za=       −VS 3 XB 1V′B 3−XB 3V′B 1 �XB 12+�XB 32 XB 1V′B 2−XB 2V′B 1 �XB 12+�XB 22        =h2(X,Zp).(16) Note that, besides computational or measurement errors, a higher order derivative of Zacan not be considered as there appear terms from ˙ Zpwhich are not available and can not be determined. With 3 additional measurements or constraints, so that h(X, Zp) is invertible for X, the BV’s state would be completely determined from the available information on-board. One interesting property of this estimation problem, which can be applied to reduce the number of unknown variables, is that there is a spatial symmetry. Indeed, on one hand, Zpdepends only on measurements on the Bframe. On the other hand, the only vector components written in the Saxes in (15)–(16) are XS 3and VS 3. Thus, it is useful to consider a rotation of the Sreference frame around the zS axis, so that the Bcomponents are unchanged. After this transformation (15)–(16) stays invariant and in consequence has a cylindrical symmetry. This means that, with the on-board measurements only, XS 1and XS 2can not be computed, as any pair (XS 1,XS 2) that satisfies (XS 1)2+(XS 2)2+(hBaro)2=∥XB∥2(17) can be a solution to (15)–(16) for X. This could be expected, as there is no way to distinguish the xSand yS axes. The zSdirection, however, is explicit in (15)–(16) as a consequence of the altimeter’s measurements. 1Note that, for the current problem, it is of no interest to compute XS 1and XS 2independently but the horizontal distance from the BV to the target, Xh=(XS 1)2+(XS 2)2, and its relative orientation. Thus, without losing any useful information for guidance, the cylindrical symmetry can be broken by rotating Sso that, at any given time, XS 1=Xh,X S 2=0.(18) This consideration reduces in 1 the number of degrees of freedom of the system’s state without reducing the information available for the guidance system. In order to determine X, as there are no additional measurements or symmetries, 2 constraints are needed. One useful approach is to benefit from the BV’s aerodynamic geometry. Without any control action, the aerodynamic moments tend to align the longitudinal axis of the vehicle with the velocity vector. After a transitory regime, the velocity vector in the Baxes can be simplified to VB=( U00 )T. This condition reduces in two the number of unknown variables. Let the additional measurement vector be Za=             hBaro γ1 γ2 ˙ hBaro ˙γ1 ˙γ2 0 0 0             =                    −XS 3 arctan 2(XB 3,XB 1) arctan 2(XB 2,XB 1) −VS 3 XB 1V′B 3−XB 3V′B 1 �XB 12+�XB 32 XB 1V′B 2−XB 2V′B 1 �XB 12+�XB 22 XS 2 VB 2 VB 3                    =h(X,Zp). (19) It can be proved that the function his locally invertible for Xinside a significant domain (see, e.g. Clarke (1976)). Therefore, it can be considered that its inverse exists in this region, so that the available measurements allow to compute the vehicle’s state if the starting point used to solve the nonlinear problem is close enough to the real state. This shows that a navigation system should be feasible, as long as the noise terms are small enough. 1In practice, a set of magnetometers or magnetic compass is enough to break this symmetry so that there is a measurable horizontal reference. This is not the case for the considered problem. 2.3 Complete problem formulation The estimation problem structure is as follows. If the state is known at a given time, its future value can be obtained from the propagation equation, ˙ X=    VS −�B IωB×VB+AGkB S+AB NG 1 2 B Iq⊗0 B IωB    =f(X,Zp). (20) The real propagation measurements ˆ Zpare corrupted with noise, this is, ˆ Zp=Zp+δZp, where δZpare modeled as samples from white noise Gaussian independent processes (see, e.g. Johnson (2022)) δZp=δAB Acc δB IωB Gyr∼N 6(0,Σp).(21) There 9 are additional measurements, Za, which contain information about the trajectory. These measurements can be computed from the state, with an additive noise sampled from a white Gaussian noise ˆ Za=h(X,Zp)+δZa, δZa∼N 9(0,Σa),(22) where fand hare nonlinear, and the initial conditions are not a specific point but a wide probability distribution. The Filtering Problem can be stated as follows: From the available measurements, uncertain initial conditions and their expected statistical characteristics, a filtering algorithm must periodically compute the system’s state. 3. PARTICLE FILTER FORMULATION For this estimation problem, the initial state probability distribution is widespread . The propagation and measurement functions f,g are manifestly nonlinear within this domain, and therefore a linearized Kalman filter is not adequate to solve the navigation problem. The Particle Filter (PF) makes use of the Monte Carlo integration theory and Bayes’ Theorem in order to obtain an optimized estimation for nonlinear systems without neither a linear approximation nor the Gaussian distribution hypothesis (see e.g. Gordon et al. (1993)). Instead, it approximates a probability distribution by a set of particles that are propagated and filtered in parallel. The complete probability distribution is obtained by means of a Bayesian approach. This family of algorithms is extensively implemented in applications where it is needed to deal with large uncertainties and nonlinearities, such as tracking from radar data. A Locally Linearized Particle Filter (LLPF) is used as the navigation algorithm for the BV (see Ristic et al. (2003)). As it is shown later, the LLPF structure allows considering symmetries and constraints naturally as additional fixed measurements. Let ˆ Xi kbe one estimation of the state, together with a covariance matrix Pi k, referred to as the particle i, at the time tk. The algorithm propagates a set of Npparticles that characterize the state probability distribution by using a linearized Kalman filter (EKF, UKF) (see, for instance, Gelb (1974)). The PF assigns to each particle ia positive weight wi k, computed by means of a Bayesian approach, that is used to measure the value of a single state estimation within the set of particles. These weights are normalized and used as the probability of each particle in a resampling process, to improve the quality of the set of particles for the next iteration. This whole process is summarized as follows, -Initial conditions for tk: Npparticles and weights, {ˆ Xi k,Pi k,wi k} 1Locally linearized KF for each particle: EKF,UKF →{ˆ Xi+ k+1,Pi+ k+1,Pi− k+1,Pi νν} 2Compute the new particles and weights: ˆ Xi k+1 ∼N(ˆ Xi+ k+1,Pi+ k+1) ˜wi k+1 = fN(h(ˆ Xi k+1),P i νν)(ˆ Zk)fN(ˆ Xi− k+1,P i− k+1)(ˆ Xi k+1) fN(ˆ Xi+ k+1,P i+ k+1)(ˆ Xi k+1) 3Weight normalization and resampling: 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) 4Estimated state: 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, where δ(·) is the Dirac delta distribution. In the algorithm, fN(ˆ Xi− k+1,P i− k+1)refers to the a priori state Gaussian multivariate probability distribution estimated locally for each particle iinside the UKF or EKF, fN(ˆ Xi+ k+1,P i+ k+1)is the corresponding a posteriori Gaussian multivariate probability density and fN(ˆ Xi− k+1,P i− k+1)is the Gaussian multivariate density function for the measurements. The selected locally linearized Kalman filter for this application, due to its robustness for nonlinear functions, is the Unscented Kalman Filter (UKF) extended for quaternion attitude representation (Kraft (2003)). -Initial conditions for tk: ˆ X(tk)= ˆ X+ k,P(tk)=P+ k 1Compute and propagate the Sigma Points: Υk,i =ˆ Xk±cols(NpChol(P′ k)) ˙ Υi=f(Υk,i,ˆ Zp)→Υk+1,i,Z i=h(Υk+1,i) 2UKF application: ˆ 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 et al. / IFAC PapersOnLine 56-2 (2023) 4508–4513 where Chol(P) denotes the Cholesky decomposition of P. In order to implement the quaternion attitude representation in the state ˆ Xk, several considerations must be made for both the UKF and the PF algorithms. The main idea that allows to extend the UKF and the PF to include quaternions is to use as an auxiliary representation system the attitude vector, which is minimal and does behave like a vector. The state’s covariance matrix is computed considering that the attitude is given by a rotation vector θ(see, e.g. Wie (2015)). After the Smatrix is obtained, two separate sets of Sigma Points are stored; position and velocity are included in the vector Sigma Point VΥ, while the 3 components associated to attitude from each column of S,θχ, are used to compute the quaternion Sigma Points, qΥ=B Sˆq⊗   cos θχ 2 θχ θχ sin θχ 2   .(23) Thus, after the Sigma Points are computed, the pair of sets {VΥ,i,q Υ,i},i∈{1,...,18}are obtained. These Sigma Points can be used to determine the time evolution and the estimated additional measurements using the nonlinear functions fand hwithout any additional modification. Computing the covariance matrices involving qΥis not immediate, since the definition computing the difference between vectors does not hold for quaternions. In fact, it is necessary to obtain an alternative algorithm to compute the mean of a set of quaternions. Let {qi},i ∈{1,...,n} be a set of attitude quaternions. Let ⟨q⟩be the mean quaternion. The rotation quaternion rifrom the mean to any of the elements of the set verifies the general rotation composition relation from the Hamilton product qi= ri⊗⟨q⟩. In consequence, the set of rotation quaternions between qiand ⟨q⟩are given by ri=qi⊗¯ ⟨q⟩. Each rotation riis equivalent to a rotation vector θr,i, so that θr,i =2 ri ∥ri∥arccos ri,0.(24) Thus, the angle between any reference frame represented by qiand the mean orientation given by ⟨q⟩is θr,i. If ⟨q⟩is the mean quaternion, the mean rotation vector ⟨θr⟩must be zero. If ⟨q⟩is not the mean quaternion, ⟨θr⟩is nonzero and oriented towards the real mean direction. Using this property, the mean quaternion of a set can be obtained by using an iterative algorithm. In particular, the proposed intrinsic gradient descent described in Pennec (1998) is implemented. This technique is very interesting for the current application since the final set of rotation vectors θr,i is equivalent to the difference xi−⟨x⟩when computing the covariance matrix of a set of vectors xi. Hence, inside the UKF, the term Υk+1,i −ˆ X− k+1 is substituted by Υk+1,i −ˆ X− k+1 ≡  XS Υ,k+1,i −ˆ XS,− k+1 VB Υ,k+1,i −ˆ VB,− k+1 θr,i  (25) where XS Υ,k+1,i and VB Υ,k+1,i are the position and velocity Sigma Points. After the state change is computed in the Kalman Filter, the three components describing the change in attitude are converted to a rotation quaternion and applied to the uncorrected attitude quaternion to compute the filtered attitude. The LLPF was generalized to include quaternions following a similar reasoning. In this case, the normal multivariate probability distributions have to be extended to use quaternions as an attitude representation system. This situation appears when evaluating for each particle fN(ˆ 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| , for both the a priori and a posteriori distributions. Note that ( ˆ Xi k+1 −ˆ Xi− k+1) and ( ˆ Xi k+1 −ˆ Xi+ k+1) are not defined for the quaternion part. The LLPF can be extended by considering the following equivalences ˆ 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) where r± k+1 =B Sˆqi k+1 ⊗B S¯ ˆqi± k+1,θ± k+1 =2 r± k+1 ∥r± k+1∥arccos r± k+1,0. A correction is needed for the imposed symmetry condition, as stated in (18), which can otherwise be problematic. If XS 2≪XS 1, setting XS 2to zero does not change significantly the horizontal distance from the BV to the target, neither the BV’s orientation with respect to the Sreference system. If, on the contrary, XS 2is representative against XS 1, the considered equation can lead to an undesirable reduction of the distance to the target and an unexpected rotation of the Baxes relative to the S axes, thus reducing the algorithm’s overall performance. To guarantee that this equation behaves as rotation, the following corrections can be applied. Let XS 0be the position before applying the filter, and XS fits value after the filtering process. Their horizontal angle is cos ψ=XS 0,1XS f,1+XS 0,2XS f,2 �XS 0,12+�XS 0,22XS f,12+XS f,22,(27) where only the horizontal components were considered. Thus, the corrected distance after filtering is given by X′S f,1=XS f,1 cos ψ.(28) As for the needed attitude correction for B Sq, it can be obtained by introducing a rotation given by q′= (cos ψ/200 sinψ/2)T. Under this algorithm, the particles distribution can be propagated freely from the available information without constraints, and corrected when the laser receiver is active. This allows to maintain all the initially available information by propagating the unconstrained particles. When LOS measurements can be used, the considered equations are included in the filtering algorithm. 4. RESULTS The proposed navigation algorithm is implemented and tested in simulation. The initial state is set to 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=0rad/s. The variances of the additive Gaussian white noise for measurements are set to σ2 h= 100 m2,σ2 a=2·10−2m2/s4, σ2 ω=5·10−6rad2/s2and σ2 γ=5·10−5rad2. The initial particles distribution is sampled from a Gaussian multivariate state probability density, ˆ Xi 0∼N(µX,0,ΣX,0),i∈[1,N p].(29) Jose A. Rebollo et al. / IFAC PapersOnLine 56-2 (2023) 4508–4513 4513 where Chol(P) denotes the Cholesky decomposition of P. In order to implement the quaternion attitude representation in the state ˆ Xk, several considerations must be made for both the UKF and the PF algorithms. The main idea that allows to extend the UKF and the PF to include quaternions is to use as an auxiliary representation system the attitude vector, which is minimal and does behave like a vector. The state’s covariance matrix is computed considering that the attitude is given by a rotation vector θ(see, e.g. Wie (2015)). After the Smatrix is obtained, two separate sets of Sigma Points are stored; position and velocity are included in the vector Sigma Point VΥ, while the 3 components associated to attitude from each column of S,θχ, are used to compute the quaternion Sigma Points, qΥ=B Sˆq⊗   cos θχ 2 θχ θχ sin θχ 2   .(23) Thus, after the Sigma Points are computed, the pair of sets {VΥ,i,q Υ,i},i∈{1,...,18}are obtained. These Sigma Points can be used to determine the time evolution and the estimated additional measurements using the nonlinear functions fand hwithout any additional modification. Computing the covariance matrices involving qΥis not immediate, since the definition computing the difference between vectors does not hold for quaternions. In fact, it is necessary to obtain an alternative algorithm to compute the mean of a set of quaternions. Let {qi},i ∈{1,...,n} be a set of attitude quaternions. Let ⟨q⟩be the mean quaternion. The rotation quaternion rifrom the mean to any of the elements of the set verifies the general rotation composition relation from the Hamilton product qi= ri⊗⟨q⟩. In consequence, the set of rotation quaternions between qiand ⟨q⟩are given by ri=qi⊗¯ ⟨q⟩. Each rotation riis equivalent to a rotation vector θr,i, so that θr,i =2 ri ∥ri∥arccos ri,0.(24) Thus, the angle between any reference frame represented by qiand the mean orientation given by ⟨q⟩is θr,i. If ⟨q⟩is the mean quaternion, the mean rotation vector ⟨θr⟩must be zero. If ⟨q⟩is not the mean quaternion, ⟨θr⟩is nonzero and oriented towards the real mean direction. Using this property, the mean quaternion of a set can be obtained by using an iterative algorithm. In particular, the proposed intrinsic gradient descent described in Pennec (1998) is implemented. This technique is very interesting for the current application since the final set of rotation vectors θr,i is equivalent to the difference xi−⟨x⟩when computing the covariance matrix of a set of vectors xi. Hence, inside the UKF, the term Υk+1,i −ˆ X− k+1 is substituted by Υk+1,i −ˆ X− k+1 ≡  XS Υ,k+1,i −ˆ XS,− k+1 VB Υ,k+1,i −ˆ VB,− k+1 θr,i  (25) where XS Υ,k+1,i and VB Υ,k+1,i are the position and velocity Sigma Points. After the state change is computed in the Kalman Filter, the three components describing the change in attitude are converted to a rotation quaternion and applied to the uncorrected attitude quaternion to compute the filtered attitude. The LLPF was generalized to include quaternions following a similar reasoning. In this case, the normal multivariate probability distributions have to be extended to use quaternions as an attitude representation system. This situation appears when evaluating for each particle fN(ˆ 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| , for both the a priori and a posteriori distributions. Note that ( ˆ Xi k+1 −ˆ Xi− k+1) and ( ˆ Xi k+1 −ˆ Xi+ k+1) are not defined for the quaternion part. The LLPF can be extended by considering the following equivalences ˆ 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) where r± k+1 =B Sˆqi k+1 ⊗B S¯ ˆqi± k+1,θ± k+1 =2 r± k+1 ∥r± k+1∥arccos r± k+1,0. A correction is needed for the imposed symmetry condition, as stated in (18), which can otherwise be problematic. If XS 2≪XS 1, setting XS 2to zero does not change significantly the horizontal distance from the BV to the target, neither the BV’s orientation with respect to the Sreference system. If, on the contrary, XS 2is representative against XS 1, the considered equation can lead to an undesirable reduction of the distance to the target and an unexpected rotation of the Baxes relative to the S axes, thus reducing the algorithm’s overall performance. To guarantee that this equation behaves as rotation, the following corrections can be applied. Let XS 0be the position before applying the filter, and XS fits value after the filtering process. Their horizontal angle is cos ψ=XS 0,1XS f,1+XS 0,2XS f,2 �XS 0,12+�XS 0,22XS f,12+XS f,22,(27) where only the horizontal components were considered. Thus, the corrected distance after filtering is given by X′S f,1=XS f,1 cos ψ.(28) As for the needed attitude correction for B Sq, it can be obtained by introducing a rotation given by q′= (cos ψ/200 sinψ/2)T. Under this algorithm, the particles distribution can be propagated freely from the available information without constraints, and corrected when the laser receiver is active. This allows to maintain all the initially available information by propagating the unconstrained particles. When LOS measurements can be used, the considered equations are included in the filtering algorithm. 4. RESULTS The proposed navigation algorithm is implemented and tested in simulation. The initial state is set to 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=0rad/s. The variances of the additive Gaussian white noise for measurements are set to σ2 h= 100 m2,σ2 a=2·10−2m2/s4, σ2 ω=5·10−6rad2/s2and σ2 γ=5·10−5rad2. The initial particles distribution is sampled from a Gaussian multivariate state probability density, ˆ Xi 0∼N(µX,0,ΣX,0),i∈[1,N p].(29) The initial bias µX,0of the state estimation is of 80 m for each position coordinate, 15 m/s for each velocity component and 0.3 rad for each Euler angle. The covariance matrix describing the initial uncertainty is set to ΣX,0= Diag (8000 8000 8000 200 200 200 0.05 0.05 0.05)T . A set of Np= 200 particles is considered, with an update frequency of 40 Hz. The Filter is programmed using C++ on a Intel(R) Core(TM) i7-3537U with no GPU acceleration. With this limited capacity, the simulations including the 40 Hz navigation system are executed with a ratio of around 1 second of computation per simulation second, thus the algorithm operates in real time. Figure 1 shows the particle trajectories. The algorithm propagates and filters the particles distribution considering only the information accessible on-board. Note the significant uncertainty prior to the convergence of the navigation system. as a consequence of the lack of information on the system’s state. When the LOS measurement is available, a rapid convergence is obtained. Fig. 1. Path of each particle in the navigation algorithm The proposed navigation algorithm correctly converges to the desired state. Moreover, the time between the LOS initial measurements and convergence is, in most of the simulations, very short, as a consequence of the resampling process. Each time a particle obtains a very good approximation of the BV’s state, all the other particles are resampled to that state, as its weight is nearly 1. Thus, the hypervolume filled by the particles distribution within the state space is significantly reduced. After the resampling step, each particle randomly propagates to a different state, slightly increasing the size of the particles distribution and therefore avoiding degeneracy. This excellent behavior is obtained in a variety of initial conditions, uncertainty and measurement noise. The parameters within the quaternion UKF and the number of particles can be tuned to improve performance for a given setup. 5. CONCLUSIONS This work introduced a Locally Linearized Particle Filter based on UKF and symmetry exploitation. The proposed navigation algorithm converges rapidly to an accurate estimate when the laser receiver activates. Although the number of particles used in the PF was small, the LLPF performed well in the simulations. To improve the LLPF’s performance in scenarios with more uncertainties or measuring noise, the number of particles could be increased, but this would increase computational costs. The particles used were sampled from an arbitrary probability distribution, but if the initial state envelope is known, the particles can be initialized accordingly, resulting in more accurate estimates. Thus, the algorithm can be easily modified by changing the initial probability distribution to include any available information about the operation. The starting set of particles can be tailored to any operational conditions without any knowledge of filtering theory. These results could be extended to other vehicle state estimation problems such as GPS-denied aircraft navigation or spacecraft rendezvous with non-cooperative tumbling targets (e.g. space debris). ACKNOWLEDGEMENTS We acknowledge support by grant TED2021-132099B-C33 funded by MCIN/ AEI/ 10.13039 /501100011033 and by “European Union NextGenerationEU/PRTR.” REFERENCES Clarke, F. (1976). On the inverse function theorem. Pacific Journal of Mathematics, 64(1), 97–102. Gelb, A. (1974). Applied Optimal Estimation. The MIT Press. Gordon, N.J., Salmond, D.J., and Smith, A.F. (1993). Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE proceedings F, 140(2), 107–113. Johnson, R.A. (2022). Applied Multivariate Statistical Analysis. Pearson College Div, subsequent edition. Kraft, E. (2003). A quaternion-based unscented Kalman filter for orientation tracking. In Proceedings of FUSION 2003, volume 1, 47–54. IEEE. Lin, Z., Yao, Y., and Ma, K.M. (2005). The design of LOS reconstruction filter for strap-down imaging seeker. In 2005 ICMLC, volume 4, 2272–2277. Ma, C., Zheng, Z., Chen, J., and Yuan, J. (2020). Jet transport particle filter for attitude estimation of tumbling space objects. Aerospace Science and Technology, 107, 106330. Maley, J.M. (2015). Line of sight rate estimation for guided projectiles with strapdown seekers. In AIAA GNC Conference, 0344. Pennec, X. (1998). Computing the mean of geometric features application to the mean rotation. Ph.D. thesis, INRIA. Ristic, B., Arulampalam, S., and Gordon, N. (2003). Beyond the Kalman Filter: Particle Filters for Tracking Applications. Artech House. Rudin, R. (1993). Strapdown stabilization for imaging seekers. In Annual Interceptor Technology Conference, 2660. Tae-Hun, K., Jong-Han, K., and Philsung, K. (2017). New guidance filter structure for homing missiles with strapdown IIR seeker. International Journal of Aeronautical and Space Sciences, 18(4), 757–766. Wei, C., Han, Y., Cui, N., and Xu, H. (2017). Fifth-degree cubature Kalman filter estimation of seeker line-of-sight rate using augmented-dimensional model. Journal of Guidance, Control, and Dynamics, 40(9), 2355–2362. Wie, B. (2015). Space Vehicle Dynamics and Control. AIAA, 3rd edition. Wu, A.D., Johnson, E.N., Kaess, M., Dellaert, F., and Chowdhary, G. (2013). Autonomous flight in GPSdenied environments using monocular vision and inertial sensors. Journal of Aerospace Information Systems, 10(4), 172–186.