A Symmetry-Based Unscented Particle Filter for Rapid State Estimation for SAL Guided Vehicles

A Symmetry-Based Unscented Particle Filter for Rapid State Estimation for SAL Guided Vehicles Jose A. Rebollo Dpto. de Ingenieria Aeroespacial, Universidad de Sevilla, Spain. Francisco Gavilan Dpto. de Ingenieria Aeroespacial, Universidad de Sevilla, Spain. Rafael Vazquez, Senior Member, IEEE Dpto. de Ingenieria Aeroespacial, Universidad de Sevilla, Spain. Abstract—The state estimation problem for vehicles with highly uncertain initial conditions and limited, varying sensors is crucial for both aircraft and spacecraft navigation. This work introduces a Locally Linearized Particle Filter based on a quaternion-adapted Unscented Kalman Filter to estimate the state of a free-fall laserguided bomb with minimal sensors and uncertain initial conditions. The available sensors include accelerometers, gyroscopes, a barometric altimeter, and a semi-active laser (SAL) receiver that activates only when the target is close and within line-of-sight. Assuming no communication between the carrier aircraft and the bomb (so that the aircraft cannot feed the bomb its launch position and velocity), the algorithm exploits the problem’s symmetry to rapidly reconstruct the relative position, velocity, and attitude of the target, even with uncertain initial conditions and insufficient sensor data. In addition, the filter initiates an identification algorithm to estimate the ballistic coefficient, which predicts the miss distance. The proposed algorithm shows promising results in Monte Carlo simulations, quickly converging to an accurate trajectory estimate and providing a high quality aerodynamic model and future trajectory predictions. Index Terms—Ballistic vehicles, particle filter, unscented Kalman filter, symmetry-based observer, navigation problem, attitude estimation, system identification. I. Introduction Precision-guided munitions (PGMs) have revolutionized the modern warfare, allowing for precise targeting while minimizing collateral damage. Indeed, guided missiles and bombs can be considered everyday weapon Manuscript received XXXXX 00, 0000; revised XXXXX 00, 0000; accepted XXXXX 00, 0000. (Corresponding author: F. Gavilan). All authors contributed equally. J. A. Rebollo, F. Gavilan and R. Vazquez are with the Department of Aerospace Engineering, Universidad de Sevilla, 41092 Sevilla (Spain). E-mail: ([email protected];fga[email protected];rv[email protected]). Color versions of one or more of the figures in this article are available online at http://ieeexplore.ieee.org. 0018-9251 © 2024 IEEE systems, present in the inventory of all modern armies. Nowadays, thanks to the miniaturization of electronics and the rising of unmanned aviation, there is a noticeable interest in the development of small-sized and low-cost guided missiles and bombs, paving the way for a cost-effective implementation of these weapons systems in small and versatile tactical unmanned aircrafts. In this regard, among all the wide variety of existing sensors for target positioning (see [1]), Semi-Active Laser (SAL) seekers (especially the strapdown mountings) have emerged as an efficient technology, well suited for small and low cost weapon systems [2]. SAL guidance utilizes a laser designator, which can be positioned either airborne or ground-based, to illuminate the target with a focused laser beam. The missile’s seeker, located in its nose, detects the direction of the reflected laser light, providing information about the Line of Sight (LOS) angles between the target and the seeker. The design of guidance systems for SAL-based weapons has been widely studied in the literature, and there are classical guidance laws that have been successfully employed in practice (a review can be found in [3]). Furthermore, the performance of guidance systems is enhanced by using gimbaled seekers (which directly enable for the application of well-known Proportional Navigation laws, and its variants) and accurate inertial navigation systems (which can also be aided by GNSS sensors, and by the carrier aircraft navigation system through an umbilical link), providing precise measurements of attitude angles, position and velocity, that can be used by the guidance law. However, measuring additional states for low-cost and small-sized bombs or missiles can pose a challenge. For example, the inertial sensors used in such weapons, which often rely on less-accurate MEMS sensors, do not provide sufficient precision to reconstruct the weapon’s status throughout the entire flight, from takeoff, owing to the significant bias that would accumulate. One possible solution would be to use an umbilical link to acquire the first fix from the launch vehicle. Nevertheless, it may not be costeffective due to the need for a complex aircraft-weapon integration process. But even if the initial launch fix were accessible, the weapon’s inertial system could face challenges due to the considerable initial accelerations from the rocket engine or bomb ejection rack. This can result in saturation of low-cost accelerometers or gyros. Due to the challenges of integrating a full inertial navigation system onto small, SAL-guided weapons, there is growing interest in developing guidance strategies that rely solely on Line-of-Sight (LOS) measurements from the seeker. For instance, the IACCG guidance law introduced in [4] utilizes only LOS angles and rates (which can be estimated using filters, as shown in [5]), while the Bearing-Only IACG approach suggested in [6] requires only LOS angles. Similarly, Reference [7] presents a guidance method for impact angle control based solely on bearing measurements, augmented with a Tobit Kalman Filter to improve estimation of the look angle, LOS an- IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. XX, No. XX XXXXX 2024 1 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ gles, and rates. While these methods demonstrate promising performance, the accuracy and robustness of SAL- guided weapons could be further enhanced by incorporating full knowledge of the weapon’s state, including its position relative to the target. This would enable optimized flight trajectories, navigation beyond the seeker’s visual field, and real-time prediction of miss distance. In this context, this work aims to contribute to the development of low-cost laser-guided weapons. Specifically, it will focus on the development of a state estimation system for a free-fall bomb (but the presented technique could also be extended to missiles). Such a system is assumed to have on-board inertial sensors (three accelerometers and three gyroscopes), together with a barometric altimeter, but it is assumed that it has neither GNSS, nor an umbilical link with the carrier plane (which would enable data sharing with the airplane navigation system, and hence could provide the bomb with an accurate estimation of its state at the launch time). Consequently, there is great uncertainty about the launch position, attitude and velocity (it is only assumed that the launch is performed within the launch acceptability region, which allows to ensure the capture of the laser beam during the ballistic fall, and whose estimation is out of the scope of this paper, see [8] to illustrate this problem). This prevents the use of conventional filtering techniques to propagate the measurements obtained by the inertial sensors, so that the bomb’s state would be rather uncertain during the ballistic fall. As for the laser seeker, a strapdown configuration is assumed (which is simpler mechanically and reduces costs), so that it provides the LOS angles of the target (with respect to the bomb’s body frame). It is also assumed that LOS rates can be obtained from the seeker (which, as will be explained later, is required for fast convergence). Although the problem of line-of-sight (LOS) rate reconstruction is not a trivial one (and is prone to uncertainties), it has been widely studied the literature and there are several techniques which successfully address this issue (see, for instance [9], which proposes a fifthdegree cubature Kalman Filter estimation method based on an augmented-dimensional state model to estimate the LOS rates; [10], which presents a LOS reconstruction filter based on an exact LOS dynamic model of strapdown seeker; or [11], in which this problem is addressed 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, the SAL seeker’s effectiveness is assumed to be limited to distances below a certain detection threshold, which is typically less than standard bombing ranges. This means that the bomb initially flies ”blind” during the first portion of its ballistic trajectory, lacking target position information. The bomb eventually detects the laser beam at some point along its trajectory, gaining information about the target’s relative location. It is at this point that traditional guidance laws (like those previously described) would typically engage, utilizing LOS measurements to adjust the control fins. However, this work demonstrates that, by briefly remaining uncontrolled after laser detection, the bomb can leverage the collected measurements to rapidly reconstruct its complete state. This reconstructed state enables the potential implementation of more sophisticated guidance strategies (which are beyond the scope of this paper). Laser Detection threshold - Uncontrolled trajectory - LLPF active - Laser unavailable - First laser detection - LLPF starts to converge - Uncontrolled trajectory - LLPF active - Laser available - Ongoing convergence - LLPF converged - Full state available - Miss distance predicted - Bomb starts to maneuver C o n t r o l l e d t r a j e c t o r y Fig. 1. Problem statement. This work proposes a full state estimation system implemented as a Locally Linearized Particle Filter (LLPF) based on a quaternion-adapted Unscented Kalman Filter (UKF). This system leverages geometric and aerodynamic symmetries inherent to uncontrolled ballistic flight (described in detail later) to achieve local invertibility. This, in turn, enables very rapid filter convergence once the laser receiver becomes active. As depicted in Fig. 1, upon detecting the laser beam, the proposed guidance system would briefly delay maneuvering to ensure filter convergence. This delay stems from the LLPF formulation, which requires, among other hypotheses, that the bomb maintain zero angle of attack. Once convergence is reached, state propagation can continue using conventional methods, such as an Extended Kalman Filter. This approach provides a reliable state estimate at the cost of slightly reducing the time available for maneuvering. This estimate can then be used by more sophisticated control systems to achieve improved guidance accuracy. Particle filters, also known as Sequential Monte Carlo methods, have emerged as powerful tools for state estimation in dynamic systems (see [12] for the basis). Among many advantages, they are well-suited for systems with non-linear dynamics and non-Gaussian noise, which can be challenging for traditional filters like the Kalman filter. They also excel in situations where the initial state is highly uncertain, by representing the state estimate as a set of particles dispersed over the state space, they can capture and reduce this initial uncertainty as new measurements are received. These features make this kind of filters especially well posed for a wide variety of tracking and estimation problems (see, for instance, [13], [14]), where nonlinearities arise, and which are oftentimes under high uncertainty levels. Improved versions of Particle Filters for highly nonlinear problems, well suited for engineering 2 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. XX, No. XX XXXXX 2024 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ applications, have been proposed. In particular, in [15], an Unscented Particle Filter (UPF) was introduced. This method approximates the computation of the optimal particle weights through implicit local linearization, namely, utilizing the strengths of the unscented transform, together with a Kalman Filter. The aerospace applications of Particle Filters are as diverse as astronomy and remote sensing [16], bearings-only tracking of vehicles [17], ballistic missile tracking [18], navigation in GNSS-denied environments [19], space object tracking [20], attitude determination of space objects using light curves [21], space debris detection in optical astronomical images [22], or attitude estimation of tumbling space objects [23]. However, to the authors’ knowledge, there is no previous work considering this particular problem, in which the complete state reconstruction of a SAL guided weapon (having neither GNSS sensors nor an accurate initial fix). 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, [24]). 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. Finally, a system identification algorithm for the drag coefficient and a trajectory predictor are also proposed, that would be of high interest for guidance. This work is an updated and extended version of the conference paper [25]. The structure of this paper is as follows. Section II establishes the basic notation and definitions used throughout the paper, provides system and sensor models for the problems under consideration, and gives a detailed statement of the estimation objective. Next, in Section III, a procedure to determine the state by exploiting constraints and symmetries is developed. Section IV introduces the Particle Filter, providing details of the particular algorithm implemented to solve the problem, including the quaternion-adapted UKF tailored to the specific system models. In section V, a system identification strategy for the vehicle’s aerodynamics is proposed as to implement a state prediction algorithm. Simulation results are given in Section VI, and the paper is closed in Section VII with some concluding remarks. II. Problem statement This Section serves to both introduce the equations needed for the trajectory and measurements characterization and present the notation which is used throughout this paper. 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 address the state estimation problem for an aircraft-dropped free-fall ballistic vehicle (BV), which could be the case of a laser-guided bomb. 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. As shown in Figure 2,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 OS and moves along with the Earth’s surface. The zSaxis points towards the center of the Earth while xSand yS are 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. The effect of the Earth’s rotation is minimal, so that the relative angular velocity between frames Sand Ican 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. laser beam Fig. 2. Reference frames. Let XSbe the position of the center of mass of the BV referred to (S). Let VS=˙ XS, and VBits components in B. Let ωBbe the angular velocity of the BV relative to an inertial frame or, equivalently, to S.AB Gand AB NG are defined as the gravitational and non-gravitational inertial accelerations in OB. For the attitude representation, the attitude quaternion qof the Bframe with respect to Sis used (see, for instance, [26]). For the current problem, some additional simplifying hypotheses can be made, without introducing appreciable errors. 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. Finally, no wind is considered in this implementation. While a system identification algorithm similar to the one described in Section Vcould be used to compute a wind model in real time, this is left out of this work to avoid even more additional complexities and REBOLLO ET AL.: SYMMETRY-BASED PARTICLE FILTER FOR STATE ESTIMATION OF SAL GUIDED VEHICLES 3 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ better focus in the challenges posed by the state estimation problem. From the considered hypotheses, the equations of motion of the BV are reduced to ˙ XS=VS,(1) ˙ VB=−ωB×VB+AGkB S+AB NG,(2) ˙q=1 2q⊗0 ω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 by using the quaternion rotation operator (see [26]). 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 (providing just bearing angles to the target). For ideal sensors, not including measurement errors, which are characterized later, the corresponding ideal measurement models can be written as AB Acc =˙ VB+ωB×VB−AB G,(4) ωB Gyr =ωB,(5) hBaro =−XS 3,(6) γ1= arctan 2(XB 3, XB 1),(7) γ2= arctan 2(XB 2, XB 1),(8) where arctan 2(y, x)is a variation of the arctangent function which avoids singularities (see [27]). Vector AB G is 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 two vectors define the propagation measurements, Zp, so that Zp=AB Acc ω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 ([28]). 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 detection 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 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) Note that, the time derivative of position in body axes requires the addition of an inertial term, so they must be computed using the following expression: d dtXB=−ω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 ˙ h, ˙γ1,˙γ2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. III. State determination from constraints and symmetries This section describes the proposed measurement extension, based on symmetries and constraints, which is a major contribution of this work and allows a fast convergence of the estimator. Let Xbe the 10-uple of magnitudes defining the BV’s state to be estimated, defined as X=  XS VB q .(14) Note that, since 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, see [26]), the state of the BV has 9 degrees of freedom. If no initial fix were available, a set of 9 independent equations derived from measurable magnitudes would be needed to fully characterize X. As described in Section IV, the proposed navigation system must be initialized to some state distribution, though with high uncertainty. Generally, a recursive estimation algorithm does not need local invertibility as a condition for convergence. However, local invertibility is of high interest, as it enhances the particles’ weighting process and leads to a rapid convergence after LOS measurements are available, as will be shown in Section VI. This is highly desirable for applications such as the one considered in this work, and therefore, the goal of this section is to find 9 algebraic 4 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. XX, No. XX XXXXX 2024 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ equations for the state variables (by exploiting as much as possible the available measurements and symmetries), enabling the local invertibility. Despite the BV being equipped with accelerometers, gyroscopes, a barometric altimeter, and a laser seeker, only the last two sensors provide direct measurements of the vehicle’s state, yielding three measurement equations at a given time, as described in (6)–(8), Za=  hBaro γ1 γ2 =h1(X).(15) These equations are insufficient to compute Xinverting h. For this purpose, a good estimation of the LOS rates, together with the vertical speed, can be computed as described, for instance, in [9], [10], [11] or [29], among others, leading to ˙ Za=  ˙ hBaro ˙γ1 ˙γ2 =h2(X, Zp),(16) where h2is as shown in equations (10)–(12). Note that, besides computational or measurement errors, a higher order derivative of Zawould not be useful 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 locally 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 B frame. 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 zSaxis, so that the Bcomponents are unchanged. This situation is represented in Figure 3. laser beam laser beam Fig. 3. Rotation of the Sreference frame around the zSaxis. 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 2 can 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 ySaxes. The zSdirection, however, is explicit in (15)– (16) as a consequence of the altimeter’s measurements.1 Note 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=p(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,(18) XS 2=0.(19) 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, as the wind airspeed is zero, the velocity vector in the B axes can be simplified to VB=  U 0 0 .(20) This condition reduces in two the number of unknown variables. Let the additional measurement vector be expanded as 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). (21) It can be proved that the function his locally invertible for Xinside a significant domain (see, e.g. [30]). 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. These results are sufficient for the proposed PF to rapidly achieve convergence, provided that the number and distribution of particles is adequate to guarantee that at least one particle is inside the region in which the above conditions are satisfied. 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. REBOLLO ET AL.: SYMMETRY-BASED PARTICLE FILTER FOR STATE ESTIMATION OF SAL GUIDED VEHICLES 5 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ IV. Particle Filter 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=f(X, Zp),(22) where f(X, Zp)is given in equations (1)–(3). The real propagation measurements ˆ Zpare corrupted with noise, so that ˆ Zp=ˆ AB Acc ˆ ωB Gyr=AB Acc ωB Gyr+δAB Acc δωB Gyr.(23) The corresponding errors are modeled as samples from white noise Gaussian independent processes (see, e.g. [31]), as given by δZp=δAB Acc δωB Gyr∼ N6(0,Σp).(24) There are additional measurements, Za, which contain information about the trajectory as shown in equation (21), but corrupted by an additive noise sampled from a white Gaussian multivariate distribution, ˆ Za=h(X, Zp) + δZa,(25) δZa∼N9(0,Σa),(26) 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, so that the accessible information is used efficiently. Indeed, for this estimation problem, the initial state probability distribution is widespread, as the initial point is uncertain. Furthermore, the propagation and measurement functions f, g are manifestly nonlinear within this domain, and therefore a linearized Kalman Filter might not be 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 or the Gaussian distribution hypothesis (see e.g. [32]). 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 [13]). The structure of the proposed filter is similar to an Unscented Particle Filter (UKF), as proposed in [15], but differs substantially in the extension of the methodology for attitude quaternions, which are constrained to norm one, both within the unscented transform and for the weights’ computation. Additionally, the structure of the problem is exploited in terms of a symmetry around the vertical axis and additional constraints, leading to a rapid convergence with a low number of samples. 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 Np particles that characterize the state probability distribution by using a certain state propagation filter (in this work, as explained below, an UKF scheme is adopted, but other propagation algorithms like an EKF could be explored, see for instance, [33]). The PF assigns to each particle i a 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: 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 PNp 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 X i=1 1 Np δ(ˆ Xk+1 −ˆ Xi k+1) ˆ Xk+1 = Np X 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 propagation filter (UKF, in this work), fN(ˆ Xi+ k+1,P i+ k+1)is the corresponding a posteriori Gaussian multivariate probability density and fN(h(ˆ Xi k+1),P i νν )is the Gaussian multivariate density function for the measurements. Thus, the weights are obtained from the a priori and a posteriori probability distributions, as described next. As stated above, the selected locally linearized Kalman filter for this application, due to its robustness for nonlinear functions, is the Unscented Kalman Filter (UKF), which has been reformulated to take into account the quaternion attitude representation ([24]). -Initial conditions for tk: ˆ X(tk) = ˆ X+ k, P(tk) = P+ k 6 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. XX, No. XX XXXXX 2024 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ 1Compute and propagate the Sigma Points: Υk,i =ˆ Xk±cols(pNpChol(P′ k)) ˙ Υi=f(Υk,i,ˆ Zp)→Υk+1,i, Zi=h(Υk+1,i) 2UKF application: ˆ X− k+1 =1 2Np 2Np X i=1 Υk+1,i,¯ Z=1 2Np 2Np X i=1 Zi P− k+1 =1 2Np 2Np X i=1 2Np X j=1 (Υk+1,i −ˆ X− k+1)(Υk+1,i −ˆ X− k+1)′ Pxz =1 2Np 2Np X i=1 2Np X j=1 (Υk+1,i −ˆ X− k+1)(Zi−¯ Z)′ Pνν =R+1 2Np 2Np X i=1 2Np X 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′ where Chol(P)denotes the Cholesky decomposition of P. Therefore, the mean and covariance matrix characterizing each probability distribution involved in computing the weights are sampled from the sigma points. 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. [26]). After the Smatrix is obtained, two separate sets of Sigma Points are stored. On one hand, position and velocity are included in the vector Sigma Point VΥ. On the other hand, the 3 components associated to attitude from each column of S,θχ, are transformed into a quaternion rotation and used to compute the quaternion Sigma Points, qΥ= ˆq⊗ cos θχ 2 θχ θχsin θχ 2!.(27) 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⟩.(28) In consequence, the set of rotation vectors between qiand ⟨q⟩are given by ri=qi⊗¯ ⟨q⟩.(29) Each rotation riis equivalent to a rotation vector θr,i, so that θr,i = 2 ri ∥ri∥arccos ri,0.(30) 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, as detailed in [24]. Firstly, an initial guess for the mean quaternion is needed. In the navigation algorithm, the mean of the set of quaternion Sigma Points can be initially approximated by the attitude quaternion qfrom the prior iteration. Let this starting point be ⟨q⟩0. The set of rotation quaternions riis computed from (29). The equivalent rotation vectors θr,i are obtained from (30) so that the mean rotation vector is computed from the usual vector definition ⟨θr⟩=1 n n X i=1 θr,i.(31) If the norm of ⟨θr⟩is smaller than a predefined tolerance set to consider that the algorithm has converged, ⟨q⟩0 is accepted as the mean quaternion. Otherwise, the next mean quaternion estimation ⟨q⟩1is computed rotating ⟨q⟩0 as indicated by ⟨θr⟩, qr= cos ⟨θr⟩ 2 sin ⟨θr⟩ 2 ⟨θr⟩ ⟨θr⟩!,(32) ⟨q⟩1=qr⊗ ⟨q⟩0.(33) This process is repeated until the mean quaternion is obtained. 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  (34) 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, which are a rotation vector, are converted to a rotation quaternion and applied to the uncorrected attitude quaternion to compute the filtered attitude. With these generalizations, the UKF is extended to include quaternions, so that the basic algorithm stays unchanged. REBOLLO ET AL.: SYMMETRY-BASED PARTICLE FILTER FOR STATE ESTIMATION OF SAL GUIDED VEHICLES 7 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ The LLPF was generalized to include quaternions following a similar reasoning. In this case, the normal multivariate probability distributions, which allow computing the particles’ weights, 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)(ˆ Xi k+1) = exp −1 2(ˆ Xi k+1 −ˆ Xi− k+1)′(Pi− k+1)−1(ˆ Xi k+1 −ˆ Xi− k+1) q(2π)9|Pi− k+1| , (35) fN(ˆ Xi+ k+1,P i+ k+1)(ˆ Xi k+1) = exp −1 2(ˆ Xi k+1 −ˆ Xi+ k+1)′(Pi+ k+1)−1(ˆ Xi k+1 −ˆ Xi+ k+1) q(2π)9|Pi+ k+1| , (36) where (ˆ Xi k+1 −ˆ Xi− k+1)and (ˆ Xi k+1 −ˆ Xi+ k+1)are not defined for the quaternion part. As written in (28)–(29), the rotation quaternion between two attitude quaternions is easily obtained by using the Hamilton product. The corresponding attitude vector is given by (30). 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  ,(37) ˆ Xi k+1 −ˆ Xi+ k+1 ≡  ˆ XS,i k+1 −ˆ XS,i+ k+1 ˆ VB,i k+1 −ˆ VB,i+ k+1 θ+ k+1  ,(38) where r− k+1 =ˆqi k+1 ⊗¯ ˆqi− k+1,(39) θ− k+1 =2r− k+1 ∥ri∥arccos r− k+1,0,(40) r+ k+1 =ˆqi k+1 ⊗¯ ˆqi+ k+1,(41) θ+ k+1 =2r+ k+1 ∥ri∥arccos r+ k+1,0.(42) As for fN(h(ˆ Xi k+1),P i νν ), a conventional multivariate Gaussian distribution for vector variables can be used. The described variants allow to completely generalize the LLPF and its corresponding UKF algorithms to use of quaternions as the attitude representation system. A correction is needed for the imposed symmetry condition, as stated in (18)–(19), 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 Saxes, thus reducing the algorithm’s overall performance. To guarantee that this equation behaves as rotation, the following corrections can be applied. Let XS 0 be the position before applying the filter, and XS fits value after the filtering process. The horizontal angle between these vectors can be computed from the definition of the dot product cos ψ=   XS 0,1 XS 0,2 0 ·  XS f,1 XS f,2 0  qXS 0,12+XS 0,22rXS f,12 +XS f,22, (43) where only the horizontal components were considered. Thus, the corrected distance after filtering is given by X′S f,1=XS f,1 cos ψ.(44) As for the needed attitude correction for q, it can be obtained by introducing a rotation given by q′=    cos ψ 2 0 0 sin ψ 2     .(45) Under this algorithm, the particles distribution can be propagated freely from the available information without constraints, and corrected when laser measurements are available. This allows to maintain all the initially available information by propagating the unconstrained particles. When LOS measurements are available, the considered equations are included in the filtering algorithm. V. Application for ballistic coefficient estimation Assuming an statically-stable, unpowered and axisymmetric (with respect to its longitudinal axis, XB) ballistic vehicle, equation (2) can be simplified by neglecting lift and cross forces against drag (notice that the vehicle is also assumed to be flying at zero angle of attack, so that VB 2, V B 3≈0, and the total aerodynamic force will be aligned with the airspeed vector). Hence, the aerodynamic acceleration can be written as follows AB NG =−D mb VB |VB|=−pq β VB |VB|(46) where mbis the vehicle’s mass, pqis the dynamic pressure, and βis the ballistic coefficient, defined as β=mb SCD ,(47) being CDthe drag coefficient, and Sthe reference surface. Notice that, under the aforementioned assumptions, the ballistic coefficient is the single vehicle-dependent parameter appearing in the equations of motion (1)–(3). This is to say, if the initial state were known, the impact point could be predicted if a proper ballistic coefficient model were available. Thus, since the ballistic coefficient would be extremely useful for the design of predictive guidance systems for such vehicles (for instance, those based on MPC, like [34]), the structure of the proposed 8 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. XX, No. XX XXXXX 2024 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ LLPF is exploited to also provide an online estimation of β. To do so, define the drag per unit of mass d=D mb =1 2mb ρVB 12SCD,(48) which depends on altitude (through the air density, ρ) and on the BV’s state through CD. Nonetheless, after the LOS measurements are available, density does not change sensibly and thus can be considered a constant. As both VB 2and VB 3are considered zero by the the steady-state attitude condition, a general model for the drag coefficient is assumed to be CD∝VB 1n.(49) Equations (48) and (49) allow to write d=αVB 1n+2 , α > 0.(50) If αand nwere known, the drag coefficient could be computed from VB 1and the propagation problem would be solved. As αis necessarily positive, taking the logarithm of both sides and substituting d=AB Acc, one has: log |AB Acc|= log α+ (n+ 2) log |VB 1|,(51) It is useful to define the measured vectors and matrices for each iteration yk= log |ˆ AB Acc,k|(52) Xk=1 log |ˆ VB 1,k|.(53) Similarly, let the estimated coefficients be given by ˆ αk=log α n+ 2.(54) This estimation problem can be solved iteratively by means of a Recursive Least Squares (RLS) method. In particular, let the cost function be given by Jk= k X i=1 hλk−i∥yk−Xkˆ α∥2+λk∥ˆ α−α0∥P−1 0i, (55) where λis the forgetting factor and P0is the covariance matrix of the initial estimation α0. The optimal solution for the LS problem is recursively computed as Pk+1 =1 λPk−1 λPkX′ k(λI+XkPkX′ k)−1XkPk(56) ˆ αk+1 =ˆ αk+Pk+1X′ k(yk−Xkˆ αk).(57) This algorithm can be executed in real time after the BV’s state is estimated from the LOS measurements. The initialization point α0can be obtained by applying a Batch Least Squares (BLS) method to a set of past states generated through a trajectory reconstruction algorithm. It is particularly simple to integrate equations (1)–(3) backwards in time from the present state and using the stored measurements for ωB Gyr and AB Acc. For a set of nLS measurements, let X0and y0be the stacked matrices y0=     yk yk−1 . . . yk−nLS      ∈RnLS , X0=     Xk Xk−1 . . . Xk−nLS      ∈RnLS ×2. (58) The initial estimation α0is computed as α0= (X′ 0X0)−1X′ 0y0.(59) This initialization process allows to compute good predictions from the moment the state is correctly estimated. Note that, although no significant changes are expected for ˆ αduring the RLS, the initial point is not critical, as the forgetting factor can be adjusted to only consider the last set of measurements. In particular, a usual value of λis of around 0.98. For a sampling frequency of 40Hz, this means that measurements taken 2 seconds in the past are weighted only a 20% against newly obtained measurements. A higher λmakes the system more robust against noise and perturbations. The situation is similar with P0, which can be arbitrarily set to the 2-dimensional identity matrix. VI. Results To analyze the performance of the proposed algorithm, a simulation model based on a small free-fall bomb has been employed. The aerodynamic model of the bomb has been obtained using semi-empirical methods (as detailed in reference [35]). Given that this work exclusively focuses on the free-fall flight phase (without control), the primary aerodynamic parameter governing the trajectory is the ballistic coefficient, which is illustrated in Fig. 4. 0.2 0.4 0.6 0.8 1 Mach 3500 4500 5500 = m/(S CD) [kg/m2] Fig. 4. Ballistic coefficient Regarding the filter design, a set of Np= 200 particles is considered, with an update frequency of 40 Hz. The complete Particle Filter algorithm is programmed using C++ to evaluate its computational performance. The available CPU to perform the simulations in this work is an Intel(R) Core(TM) i7-3537U. No GPU acceleration was used. With this limited processing capacity, the simulations including the 40 Hz navigation system update rate are executed with a ratio of around 1 second of computation for each 1 simulation second, this is, the algorithm operates in real time. Additionally, a very interesting property of the proposed navigation system is its high degree of parallelizability. Indeed, as described in [36], a Parallel Quaternion Particle Filter (PQPF) could be implemented to effectively reduce the computational time between iterations. REBOLLO ET AL.: SYMMETRY-BASED PARTICLE FILTER FOR STATE ESTIMATION OF SAL GUIDED VEHICLES 9 This article has been accepted for publication in IEEE Transactions on Aerospace and Electronic Systems. This is the author's version which has not been fully edited and content may change prior to final publication. Citation information: DOI 10.1109/TAES.2024.3475995 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/