Abstract
The benefits of GNSS have created dependencies on navigation in modern day aviation systems. Many of these systems operate with no backup navigation source. This makes the capabilities supported by precise navigation vulnerable. This paper investigates a contemporary approach to terrain-referenced navigation (TRN), used to preserve an aircraft’s navigation solution during periods of GNSS denial. Traditionally, TRN has been accomplished using a single measurement sensor pointed nadir to an aircraft. Although shown to be effective, this approach limits the achievable navigation accuracy by restricting the measurable terrain gradients to those below an aircraft. This paper explores an alternative approach to TRN that maximizes navigation information through optimal steering of a gimbaled laser. A Cramér-Rao lower bound analysis as well as a high-fidelity simulation establishes the utility of optimal steering while employing TRN. This original approach to TRN shows the order of magnitude improvements in navigation performance over existing methods.
1 INTRODUCTION
Modern aviation systems have developed a critical dependency on the precision, navigation, and timing (PNT) information supplied by Global Navigation Satellite Systems (GNSS). This dependency grows more severe at a time when methods of GNSS denial are becoming cheaper, more sophisticated, and highly proliferated. If accurate PNT is critical to safe aviation operations, one of two technical solutions must be realized: make GNSS signals more reliable or develop alternative sources of precision, navigation, and timing (APNT) information. This paper proposes a contemporary approach to terrain-referenced navigation (TRN) as a viable source of APNT information. TRN is advantageous in a contested environment because it does not rely on signals from outside an aircraft and cannot be easily denied by an adversary. This is in significant contrast to the ease at which a GNSS signal-jammer can be built and employed.
TRN systems have been around since the 1950s when they were used as Inertial Navigation System (INS) aiders for early cruise missile development (Hallmark, 1967). They remained relevant into the late 1980s as a navigational aid. However, the advent of GNSS transitioned most terrestrial TRN system development from navigational aids to ground collision avoidance systems (Honeywell, 2007). Legacy TRN systems required significant pre-mission planning to create reference Digital Elevation Models (DEMs) and were generally only relevant at lower altitudes where a radar altimeter’s return was considered reliable (Boozer & Fellerhoff, 1985). The estimation filters were also prone to false fixes due to insufficient modeling of the highly nonlinear terrain-matching problem (Zhang et al., 2017). Although many legacy TRN systems were proven effective, these operational drawbacks largely prevented their fielding as methods of GNSS matured.
Recent advances in technology have provided an avenue to overcome many of the historical challenges experienced by TRN systems. DEM information is now globally available and free for download on the internet, greatly reducing pre-mission workload. Also, the high power and low beam divergence of laser technology has enabled TRN systems to be relevant at higher altitudes and enabled accurate measurements when employed at slant angles (not nadir). During a flight test program at the USAF Test Pilot School (TPS) in 2019, accurate slant-range laser measurements were achieved at distances up to 10 kilometers away from the test aircraft. In this case, TRN solutions were generated using non-nadir pointing lasers for an F-16 flying at 40,000 feet altitude (Carroll, 2020). This result significantly expands the “operating envelope” in which TRN can be employed as an effective source of PNT information. Additionally, the advent of large dimension, nonlinear TRN algorithms, which treat slant-range measurements as shown in Carroll (2020), enables a compelling use case for slant-range TRN. Consider that electromagnetic spectrum data, other than radio frequency (RF) information, is being used increasingly in modern aviation. Many aircrafts are being equipped with steerable sensors that report information from the infrared (IR), visible, and ultra-violet (UV) regions. Since this data is already available on many platforms, it is ripe for service in navigation applications. Building on prior work (Carroll, 2020; Kim et al., 2018), this paper further establishes the relevance of slant-angle measurement approaches to TRN. Specifically explored is the optimal steering of a laser altimeter for the purpose of maximizing the available measurement information to a position estimation filter. Section 1 of this paper gives a brief background on the different approaches to TRN and DEM selection. Section 2 establishes the navigation system architecture employed in this study and provides the mathematical details on the algorithm used to generate our results. Section 3 describes the ray-casting sub-algorithm used in the generation of expected measurements. Sections 4 and 5 present the case for optimal steering of the laser sensor and detail the generation of steering angles for the purpose of maximizing measurement information. Results, detailed in Sections 6 and 7, are presented for both a Cramér-Rao lower bound (CRLB) analysis and a fully developed TRN simulation. Potential is shown for the order of magnitude improvement in navigation performance over legacy, nadir-oriented sensors.
1.1 A background on TRN
TRN systems, sometimes called terrain-aided navigation systems (TAN), compare measurements of an aircraft’s height above the local terrain with a DEM to produce an estimate of position. In its traditional form, TRN employs ranging measurements taken from a downward pointing radar altimeter and queries the DEM, often a Digital Terrain Elevation Database (DTED), to calculate an approximate aircraft position. This position is then applied as an update to an INS for the purpose of correcting drift.
A TRN system contains four main components: an INS, a map source (DEM), a measuring device, and an estimation algorithm. The form each of these components takes can be tailored to operational requirements and available technologies. In general, a navigation or tactical grade INS is used. The DEM is chosen based upon the desired resolution of terrain data and available vehicle computing power. The measuring device is generally a radar-based altimeter, camera, or in recent years, a laser-based altimeter (Carroll et al., 2019) or laser scanner (Campbell, 2006; Vadlamani, 2010). The choice of algorithm, however, is not so clear. TRN is a highly nonlinear estimation process. This is due to the fact that height above terrain measurements does not map linearly into a vehicle’s position or the velocity states. Rather, these measurements are a function of vehicle pose and terrain variability. To solve such a problem, the use of nonlinear estimation algorithms is required. Nonlinear estimation algorithms are suboptimal by nature and often computationally intensive. Exploring the viability of different nonlinear estimators has been a central focus of TRN development since the 1990s. Estimation methods involving extended Kalman filters, particle filters, sigma-point filters, grid-based filters, and batch methods have been explored. The trade space between combinations of DEM, the measuring device, and the estimation algorithm is an active area of study. A search of the literature reveals extensive study in the area of TRN over the last 70 years. Numerous studies have been performed in both simulation and flight test. For example, in 1999, Bergman et al. (1999) conducted a study on TRN employing a grid-based filter and nadir-oriented radar altimeter; their results suggested accuracies in the tens of meters were possible. Additionally, in 2002, Nordlund and Gustafsson (2009) conducted a study employing a marginalized particle filter (MPF) (also referred to as a Raó-Blackwellized particle filter or RBPF) and a nadir radar altimeter. Results here also indicated accuracies in the tens of meters were likely. In yet another combination of components, Honeywell in 2003 demonstrated a TRN system employing Interferometric Synthetic Aperture Radar and an estimation filter employing both batch and recursive elements (Honeywell, 2007). Accuracy in this case was noted in the 20-meter range. Many additional studies involving unique and creative combinations of the four components have also been performed. For additional background, the interested reader should see Bergman (1998); Campbell (2006); Copp and Subbarao (2015); Boozer and Fellerhoff (1985); Flayac et al. (2018); Hallmark (1967); Hostetler and Andreas (1983); Jeon et al. (2018); Johnson and Montgomery (2008); Koch and Evans (1980); Metzger et al. (2005); Norlund and Gustafsson (2002); Pierrottet et al. (2014); Schön et al. (2005); Shidner (2016); Cowie et al. (2008); Turan and Kutay (2016); Vadlamani and Uijt De Haag (2009); Ward et al. (2019); Zhang et al. (2017); and Zhao et al. (2015). The studies referenced in this section are by no means an exhaustive list, but demonstrate a good cross section of TRN-related development from post-WWII times through modern day. Applications range from aircraft and cruise missile navigation to lunar and martian entry, descent, and landing (EDL) guidance.
Most TRN systems are descendant from two algorithms developed in the 1960s and 1970s: TERrain COntour Matching (TERCOM) and Sandia-Inertial Terrain-Aided Navigation (SITAN). TERCOM was an algorithm that used batch estimation to assess the position error of a vehicle. Over some predefined amount of time, a set of height above terrain measurements was collected. This set of data was then compared by either a Mean Absolute Difference (MAD) or Mean Squared Difference (MSD) algorithm to find the best fit between the acquired measurements and the terrain database (Hallmark, 1967). The best fit was then used to estimate position error and apply corrective control inputs to bring a vehicle back to preplanned flight routing. Early TERCOM systems were noted as having accuracies in the 100s of meters (Golden, 1980). Although TERCOM was effective in achieving fast localization with large initial position error, it performed poorly in real time and restricted aircraft maneuvering during update (Zhang et al., 2017). In contrast to the batch estimation used by TERCOM, SITAN employed recursive estimation and was based on the extended Kalman filter. SITAN processed measurements sequentially and compared them with an onboard DEM. The state estimation of position was performed by linearizing the terrain slopes in the latitude and longitude directions and then individually applying height above terrain measurements. Significant testing of the SITAN algorithm was accomplished by the US Air Force in the mid 1980s through the Advanced Fighter Technology Integration (AFTI)/SITAN project. Navigation accuracies as low as 75-meters CEP were achieved (Boozer & Fellerhoff, 1985). However, SITAN was noted as being prone to false-fixes during conditions of large initial position error or large terrain gradient variance (Zhang et al., 2017). Many modern studies have employed variants of both TERCOM and SITAN together. The use of TERCOM in “acquisition” modes and SITAN in “tracking” modes was popular and is evident in the literature (Boozer & Fellerhoff, 1985; Hicks, 1993; Hostetler & Andreas, 1983; Nielson, 1994; Sineglazov, 2014; Zhang et al., 2017).
1.2 Slant-range TRN
Traditionally, TRN has been employed using sensors-oriented nadir to an aircraft’s body reference frame. This has limited the terrain information available for measurement to only the areas directly below an aircraft’s flight path. Historically, this has proven problematic when flying over extended areas of flat terrain (Boozer & Fellerhoff, 1985; Zhang et al., 2017). Furthermore, this assumption has often not been met in practice due to the constant attitude fluctuations of aircraft. A contemporary approach to TRN seeks to measure aircraft to ground ranges at arbitrary angles (Jeon et al., 2018). This type of system, referred to as an “arbitrary angle” or “slant-range” TRN system, uses sensors that are not restricted in the angle of emission from the aircraft’s body reference frame. In Kim et al. (2018), an Interferometric Radar Altimeter (IRA) was used to introduce slant-range measurements into a navigation filter, and in Carroll (2020), slant-range measurements from three fixed (non-steerable) laser sensors were demonstrated. Both of these slant-range strategies demonstrated improved navigation performance over nadir-oriented measurement sensors.
Notable in many previous TRN investigations is the heavy use of the nadir-oriented radar altimeter as a measurement device. We argue that TRN is better performed using a laser instead because it enables slant-range applications and increases effective measurement range. Traditionally, radar altimeters have been used because they are all-weather capable. While they have the advantage of being able to penetrate clouds, their range is limited relative to a laser making their employment in slant-range applications unrealistic. Additionally, radar altimeter returns introduce large amounts of measurement uncertainty into the estimation problem limiting achievable navigation accuracy. These inaccuracies stem from several sources. First, the dispersing beam of a radar altimeter measures the average terrain height over an area rather than the height at an exact location. Second, incorrect assumptions are often made that a measurement is taken directly below the aircraft, which is only true in straight and level flight. Third, the radar altimeter returns from the tops of trees, vegetation, and urban development are not representative of the low-resolution DEMs used by the navigation system. Employing a laser sensor in place of the radar altimeter solves all of these problems. Further, the pursuit of a steerable laser allows for the possibility of intentionally targeting areas of rugged or mountainous terrain. Steering toward areas of the map where measurements will more closely match DEMs is also possible (such as areas free of tall vegetation). Slant-range laser-based TRN, as shown in Carroll (2020), increases the quality of the navigation information available to the TRN filter and allows for greatly improved navigation performance.
The slant-range sensor approach to TRN is in contrast to previous research conducted into laser-based TRN (Campbell, 2006; De Haag et al., 2006; Vadlamani, 2010) where a scanning LIDAR was used to generate point cloud data sets of terrain information for use in terrain-referenced precision approach guidance. In this application, the measurement data set consisted of large point clouds returned by the scanning LIDAR(s). The point clouds were compared in a batch style algorithm with custom, high-resolution LIDAR Digital Surface Models (DSMs) to produce position error estimates. A DSM is a terrain database that takes into account surface objects such as trees and buildings; a DEM has undergone processing to remove them. As tested, the system was capable of positioning accuracies less than 10 meters. However, two significant limitations were noted. First was the requirement and storage capability for the custom generated, high-resolution LIDAR DSMs. The availability of these DSMs was, and continues to be, sparse such that their implementation on a wide scale is not currently viable. Second, the scanning LIDAR was heavily impacted by atmospheric particulates (Vydhyanathan, 2006). The low dwell time of the laser on each point of measurement resulted in low intensity returns that could be rapidly attenuated by fog, clouds, or other atmospheric particulates. This last limitation would be realized by all laser-based systems, but more severe in scanning systems employing low dwell times.
Our approach also differs from the slant-range IRA-based method given in Kim et al. (2018). In this application, an IRA was used to measure the closest terrain point to the aircraft. These slant-range measurements were then incorporated into a navigation solution using a particle filter algorithm. Simulation results suggested positioning accuracies of 10 meters were possible. The key difference between the IRA and steerable-laser slant-range approaches is that the IRA only yields measurements of the closest terrain point to the aircraft, it does not have the ability to target specific terrain gradients. Selecting specific terrain points for measurement to maximize the information available to the navigation filter is the principal benefit of the steerable-laser approach.
1.3 Sourcing digital elevation models
The availability of a reference DEM is one of the most critical considerations when designing a TRN system. There are many options available in this domain across a wide spectrum of resolutions ranging from the 900-meter post-spacing of the DTED level zero National Imagery and Mapping Agency (2000) to the satellite imagery-based, commercially produced 0.5-meter post-spacing of the Maxar databases (Maxar, 2020). It is fairly well established that using higher resolution DEM data results in better navigation performance (Carroll, 2020). However, most high resolution DEMs are only available over small geographic areas, making their use in global navigation applications not currently realistic. This research employed the 30-meter post-spacing DTED2 DEM due to its global coverage and no-cost availability. DTED2 DEMs can be found on the National Oceanic and Atmospheric Administration’s (NOAA) Interagency Elevation Inventory (NOAA, USGS, & FEMA, 2018).
2 SLANT-RANGE TRN NAVIGATION SYSTEM ARCHITECTURE
2.1 Filter dynamics model
The steerable-laser TRN system uses laser range measurements to aid a drifting INS. A dynamics model propagates inertial errors forward in time, and a measurement model relates the laser range measurements to INS errors. The states of the system are established based on an INS error model. This slant-range TRN system can be represented with the following Bayesian recursion equations:
1
2
where 𝐱 are the system states, and 𝐰𝑘 and 𝐯𝑘 are zero mean Gaussian noise sources with covariances of 𝐐 and 𝐑, respectively. In our context, the dynamics function 𝑓 is describing an INS error model. The measurement model ℎ describes how the steerable-laser measurements relate to the system’s states. We will make use of the well-known linear Pinson error model (Titterton & Weston, 2004), described in detail in Appendix A to model the dynamics of the INS. This allows for the state propagation equation to be rewritten as
3
The states 𝐱 in the Pinson error model are given by
4
where 𝛿𝛒 are position errors in the North, East, Down (NED) frame, 𝛿𝐯 represent component velocity errors in the same frame, 𝛜 are platform tilt errors, and 𝐚 and 𝐠 are model sensor bias errors in the INS’s accelerometers and gyroscopes. The sensor biases are modeled as first-order Gauss-Markov (FOGM) random processes, a type of time-correlated moving bias, and are contained in the Pinson-15 error model.
The full Pinson error model described by 𝐅 is given in Appendix A. The continuous time dynamics described by the Pinson error model can be discretized as
5
where 𝚽𝑘 is the state transition matrix given by
6
and 𝐰𝑑 is the discrete Gaussian process noise with zero mean and covariance given by . 𝐐𝑑 is the discrete noise covariance matrix whose first-order approximation is 𝐐𝑑 = 𝐐Δ𝑡. The noise matrix 𝐐𝑑 is given by
7
where 𝐕𝐑𝐖 is a velocity random walk in all three velocity states, 𝐀𝐑𝐖 is an angular random walk in all three tilt states, and 𝐚𝐝 and 𝐛𝐝 are driving noise sources for the accelerometer and gyroscope FOGM bias states. The accelerometer and gyroscope bias states are parameterized by a standard deviation of 𝜎𝑎 and 𝜎𝑔 and a time constant 𝜏𝑎 and 𝜏𝑔, respectively. These biases are modeled in the filter by passing white Gaussian noise with strength 𝐚𝐝 and 𝐛𝐝 through linear shaping filters of the form
8
with the same form employed for the gyroscope states. The driving noise terms 𝐚𝐝 and 𝐛𝐝 have strength equal to
9
Values for the noise sources in Equation (7) are given in Table 1.
INS Specifications for HG9900 Navigation-Grade INS
2.2 Filter Measurement Model
The measurement function ℎ describes the nonlinear mapping between the system states and expected slant-range measurements. On an arbitrary DEM, this cannot be given by a closed-form mathematical equation but must rather be found with a ray-casting algorithm, given in Section 3. The ray-casting algorithm takes in several parameters needed to compute the slant-range measurement from the aircraft to the DEM. The form of the measurement, given in Equation (2), can be expanded to
10
where 𝛿𝜌𝑙𝑎𝑡,𝑘, 𝛿𝜌𝑙𝑜𝑛,𝑘, and 𝛿𝜌𝑎𝑙𝑡,𝑘 are filter states given in Equation (4) (specifically INS position errors), and 𝐚𝐮𝐱𝑘 is additional non-filter state information needed to perform the measurement update. The distinction of filter vs non-filter quantities is important in understanding the derivation of the measurement Jacobian 𝐻, described in Section 4.1. The measurement Jacobian is obtained using finite differencing with respect to just the 15 filter states given in Equation (4).
The quantity 𝐚𝐮𝐱𝑘 is made up of information related to the INS, the terrain map, and the gimbaled laser steering system. It includes:
𝐷, the Digital Elevation Model (DEM)
𝑙𝑎𝑡𝑖𝑛𝑠,𝑘, the INS-estimated latitude
𝑙𝑜𝑛𝑖𝑛𝑠,𝑘, the INS-estimated longitude
𝑎𝑙𝑡𝑖𝑛𝑠,𝑘, the INS-estimated altitude
, the INS-estimated aircraft attitude
𝜃𝑘,𝑔𝑖𝑚, the gimbaled laser’s pitch
𝜓𝑘,𝑔𝑖𝑚, the gimbaled laser’s yaw
The gimbaled laser’s pitch and yaw angles (with roll=0) can be used to form a direction cosine matrix , which together with the aircraft attitude can provide a pointing vector for the laser in the navigation reference frame. Section 3 will further define the function given by Equation (10). Note that the only filter states that appear in Equation (10) are 𝛿𝜌𝑙𝑎𝑡, 𝛿𝜌𝑙𝑜𝑛, and 𝛿𝜌𝑎𝑙𝑡. Also, note that there is no external source of altitude information, such as a barometer, which is common in many forms of TRN. As shown in Section 6, the filter has strong observability of INS altitude errors, removing the need for external altitude information.
As shown in Equation (2), the measurements include noise given by 𝑣𝑘 with a covariance of 𝑅. We modeled 𝑅 = 10 meter𝑠2 for the simulations in this paper. The actual accuracies of the considered laser systems are significantly better than 10 meter𝑠2, but the measurement covariance 𝑅 must encompass all un-modeled errors that would cause a mismatch between the laser range measurements and the expected measurements from the DEM. Dominant errors in this measurement equation are likely DEM errors. Other error sources include atmospheric effects, boresight errors, and sensor errors.
The INS tilt error states could be included in the measurement equation, but this is intentionally avoided for simplicity. The INS tilt errors for a navigation grade inertial are on the order of hundredths of a degree, which we found to not be a dominant measurement error source compared to other errors, like DEM errors. For example, a 1-kilometer laser range measurement intersecting flat ground at a 45-degree angle will have its range measurement change by just 1.75 meters with a 0.1-degree attitude pointing error. This type of measurement error is well below the 10-meter modeled measurement noise of the system. Furthermore, any applied inertial feedback using the filter solution will correct the INS attitude solution with filter estimated tilt errors, further reducing the need for this explicit modeling of tilt errors in the measurement equation. Note that the same analysis of the need for modeling INS tilt errors applies to overall boresight errors for the gimbaled laser system. During flight tests of the author’s previous work in Carroll (2020), boresight alignments of the laser sensors were achieved to within 0.01 degrees. Given that result, for this study, we will assume a measured boresight for the laser system accurate to 0.1 degrees and ignore any boresight errors due to the fact that they should manifest as measurement errors below the modeled noise level of the measurements. Additionally, in this paper, we will assume the gimbaled laser is installed such that it shares the same reference frame as the aircraft’s body frame. A fielded system would need to measure and apply any rotation and lever arm between the sensor frame and aircraft body frame; although for simplicity, we do not consider these parameters in our simulations.
2.3 The marginalized particle filter
Figure 1 depicts the flow of information within the overall proposed steerable TRN algorithm at a single point in time. The algorithm starts by forming an estimate of current position using the INS solution as well as the current filter INS error estimates. This position solution is used to determine an optimal steering angle for the gimbal laser. The gimbaled laser steers to the desired angle and takes a measurement. This measurement is incorporated into the navigation filter, and updated estimates of the INS error states are computed. Finally, the INS error states are propagated forward in time, before the process repeats itself again.
A snapshot of the estimation algorithm at time 𝑡𝑘, illustrating the flow of information over a single propagate-update cycle of the recursive steerable-laser TRN filter. The implementation of ray-casting and optimal steering angle selection is discussed in Sections 3 and 4.
The standard Bayesian recursion scenario of Equations (1)-(2) can be solved with a variety of nonlinear estimators. This work builds on Carroll (2020) in which a MPF was used to solve the slant-range TRN problem. Note that the MPF is also sometimes referred to as a Rao-Blackwellized particle filter. The MPF acts as a bank of Kalman filters, in which certain chosen “particle” states are propagated and updated with particle filter operations, while the remaining states are propagated and updated with Kalman filter operations. This filter is chosen for two reasons. The first is the highly nonlinear behavior of the slant-range measurement equation, which can lead to non-Gaussian posterior distributions after a measurement update. These non-Gaussian distributions are well represented by a set of particles. A pure particle filter, however, is not a good choice due to the high number of states in the inertial error model (15 states). Particle filters are known to suffer from the “curse of dimensionality” and tend to break down when using more than 3-4 states. The MPF provides the best of both worlds—able to handle both highly non-Gaussian states as well as a state vector of large dimension. For our application of slant-range TRN, only the position error states are treated as particle states. Implementation of the MPF closely follows Schön et al. (2005). The full MPF algorithm is given in Appendix B.
3 RAY-CASTING FOR TERRAIN-REFERENCED NAVIGATION
Ray-casting is a process of examining the intersection of rays emanating from a source with digitally defined surfaces. Historically used in computational geometry and computer graphics generation, ray-casting is performed by projecting a light ray onto a scene. Once cast, the ray can be traced along to find the intersection with an object in the scene (Roth, 1982). For this research, the desire existed to assess terrain range measurements at arbitrary angles from the body frame of an aircraft to a DEM. To evaluate this measurement function, a ray-casting process was used. Figure 2 illustrates this process. Note that in computational geometry ray-casting can be performed very efficiently on a GPU. It is not trivial to gain these speed increases within the larger framework of a particle filter where large amounts of overhead at every time step would be spent passing information to and from a GPU. This is an active area of research and was not considered within the scope of this work. Instead, a highly efficient CPU binary search algorithm finds the intersection points quickly enough to run far faster than real time even with tens of thousands of particles.
Illustration of the ray-casting process and the binary search method used to find a laser beam’s intersection with a DEM. Note: the initial “R” value is either 50 meters (chosen minimum range measurement) or the last valid slant-range value from previous update cycles of the MPF.
The exit of the laser’s optical cavity was considered the source of each ray. A unit vector was defined in the sensor frame to point nadir: 𝐮 = [0 0 1]𝑇. This unit vector in the sensor frame goes through two frame rotations to point at a location on the DEM. The first rotation describes the orientation of the laser with respect to the aircraft body frame and is determined by the gimbal angles. The DCM, which describes this rotation, is given as and is computed from the roll, pitch, and yaw Euler angles of the gimbaled laser system by the standard method of applying three subsequent rotations about the 𝜓𝑘𝑔𝑖𝑚 (yaw), 𝜃𝑘𝑔𝑖𝑚 (pitch), and 𝜙𝑘 (roll) axes. Note that the roll is equal to zero.
11
The second rotation takes the laser from the aircraft body frame to the local navigation frame. It employs the aircraft’s pitch, roll, and yaw attitude angles from the INS and is given by the DCM . Together, these two rotations applied to 𝐮, define a unit vector 𝐮𝑛𝑎𝑣 that points from the aircraft to the DEM ground intercept point as shown in Equation (12). Note that this unit vector is the “ray-cast unit vector” given in Figure 2.
12
Once in the navigation frame, a ray is cast down the unit vector and traced along via a binary search method until intersection with the underlying DEM is indicated, as described in Figure 2. Queries of the DEM at each point along the trace were accomplished to compare the altitude of both the terrain and the current position along the ray. The binary search algorithm is a simple method, which relies on the unit pointing vector 𝑢𝑛𝑎𝑣 and a starting ray range 𝑅. These two quantities define two points—the first being the aircraft (point 𝐴) and the second being 𝑅 units down the unit vector (point 𝐵). The algorithm also stores the starting aircraft position as point 𝑂. The algorithm begins by doubling the length of the original ray length 𝑅 until point 𝐵 is below the DEM. Next, the algorithm bisects points 𝐴 and 𝐵 with another point 𝐶. The algorithm forces there to always be one point above the DEM and one point below the DEM by appropriately setting either 𝐴 or 𝐵 to 𝐶. Once the bisected value is within a predetermined distance of the terrain, a final range value was generated by evaluating the root sum square of the 3D position difference between the aircraft (point 𝑂) and the terrain intercept (point C).
13
This predetermined distance value used for comparing the cast ray and terrain map was referred to as the “resolution” and was an adjustable tuning parameter.
The ray-casting algorithm given in this section is computed on a per particle basis within the MPF. It provides the expected measurement from each particle location, which is then compared to the actual measurement. This residual between actual and expected measurements is used to weigh each particle with lower residuals resulting in higher weights and vice versa. The weighting equation used is given in Appendix B.
4 OPTIMAL LASER-STEERING STRATEGIES USING THE FISHER INFORMATION MATRIX
This research seeks to build on the techniques of slant-range TRN by optimally steering a gimbaled laser to point at areas of a DEM with high-information content. The method of steerable TRN is highly relevant since many current aircraft already possess steerable-laser sensors capable of generating slant-range measurements. In many cases, these steerable lasers are used for target illumination purposes, or ranging applications not related to navigation. The ability to implement steerable-laser TRN with no additional hardware while gaining an order of magnitude improvement over nadir TRN makes the proposed technique very compelling. This section will motivate and derive optimal steering techniques, which can greatly improve navigation performance.
4.1 The Fisher information matrix for steerable TRN
Consider the standard Bayesian Recursion problem given by Equations (1)-(2). Following from Taylor (1979) and Bai and Clarke (2020), the Fisher information matrix (FIM) for problems described by Equations (1)-(2) is given by the recursive equation:
14
where 𝐅𝑘 and 𝐇𝑘 are the Jacobians of the nonlinear propagation function 𝑓 and nonlinear update function ℎ, respectively.
15
and
16
𝐉𝑘 is the FIM and the CRLB — the minimum covariance for any unbiased estimator, i.e.,
17
We seek to minimize the covariance of our state estimates by maximizing the FIM. We will not attempt to change our aircraft’s trajectory or dynamics, which would cause unnecessary mission constraints. The optimization parameters (gimbaled laser steering angles) only appear in the second term of Equation (14). We will eventually maximize the trace of this matrix. Note that the trace operator can be distributed over the two terms that make up the FIM in Equation (14). The optimization parameters do not appear in the aircraft dynamics causing this first term to become a constant with respect to a maximization problem over the gimbaled laser steering angles. This allows us to only consider the “new information” or measurement contribution term of the FIM when computing optimal steering angles. We can further drop the measurement covariance 𝐑 as it also does not effect the location of the maximum. This leads to a maximization problem with a simpler form given by
18
Note that the computed gimbal steering angles will maximize the FIM in Equation (14). Also note that the Jacobian shown in Equation (18) emphasizes the role of the gimbal steering angles but should be understood to still be a function of the same variables as the full measurement model given in Equation (10).
Given that only the new information matters in determining the gimbaled steering angles, we must control the magnitude of the matrix 𝐇, which is the Jacobian of the ray-casting algorithm given in Section 3, by adjusting the steering angles of the gimbaled laser. It is clear that the ray-casting measurement in Equation (10) does not have a closed-form solution which can be differentiated. To obtain measurement Jacobians, we apply finite differencing to the ray-casting algorithm.
19
where ℎ(𝐱, 𝐚𝐮𝐱) represents the ray-casting operation and 𝐱 the filter estimated latitude, longitude, and altitude INS errors. Of the 15 states in the Pinson error model, only the position states appear in the measurement model, giving the measurement Jacobian the form
20
The finite difference step size is given by 𝜖. Equation (19) was evaluated by calling the ray-casting function at different component values of the aircraft’s position and observing the change in the measurement value returned. The parameter 𝜖 had to be sufficiently large to capture the change in measurement with varying position, but not too large to omit capturing the effect of local terrain variations on the measurements. In this work, 𝜖 was chosen to be 30 meters. The motivation for choosing 30 meters comes from the DEM post-spacing of DTED Level 2, which is 30 meters and defines the Nyquist frequency of the DEM. Given that smooth interpolation functions are evaluated on the map, choosing an 𝜖 smaller than 30 meters will yield a similar gradient with the added risk of numerical issues as 𝜖 gets smaller.
4.2 Information content of a single nadir laser range measurement
At this point, it is worth motivating the type of information which can exist in a single nadir laser-range measurement. For this intuitive analysis, consider the DEM to be locally linear in the immediate vicinity of a laser range measurement. On a smooth 30-meter post-spacing DEM, this is a good assumption. Consider a nadir-pointing laser over perfectly flat terrain. Intuitively, it is easy to grasp that no useful information would exist in this measurement. This can be seen by looking at the Jacobian of the range measurement with respect to the North and East directions. In the case of perfectly flat terrain, these Jacobian terms would both be zero.
Now, consider the case where there exists a moderate North-South terrain gradient. In this case, the laser-range measurement can only provide positional information in the North-South direction. This is reflected in the fact that the Jacobian of the laser-range measurement function is zero in the East direction. What may be less intuitive is that (under the assumption of local linearity and nadir measurements) a single laser range measurement can only provide navigation information in one direction— the direction of the locally linear gradient of the DEM. Additional intuition can be gained by realizing the single laser range measurement is a function of both the North and East positions and therefore is an under-determined system of equations at a single point in time. In the case of nadir-pointing lasers, increasing measurement information content can only be achieved by varying the aircraft’s trajectory to fly over varying DEM gradients, which exist ideally in orthogonal directions in order to maximize information in both the North and East directions. This technique has been previously demonstrated in Flayac et al. (2018). This strategy is generally undesirable for practical aircraft missions.
4.3 Information content of a single slant-range laser measurement
The use of a steerable laser provides opportunities to increase the information content provided by the measurement Jacobian. In the case of slant-range measurements, there are two primary ways to increase the measurement Jacobian.
The first strategy, similar to the nadir case, is to point the laser at steep terrain gradients. Figure 3 illustrates this concept using simple finite differencing of the ray-casting measurement. Two sets of ray-casting measurements are received—a shallow terrain gradient labeled 𝑎1 and a steep terrain gradient labeled 𝑏1. A set of black lines to each terrain gradient shows the slant-range measurement. A second set of red lines is shown to reflect the measurement when the aircraft moves ΔNorth meters to the right. In this case, the slant-range measurements are given by 𝑎2 and 𝑏2. The Jacobian of the measurement function is approximated by simple finite differencing, calculating the Jacobian as the change in the slant-range divided by the change in the North position. It is clear from the figure that the steep gradient will have a larger measurement Jacobian (change in slant range) than the shallow gradient. This is similar to the nadir case with one key difference. Encountering steep terrain gradients in the nadir case required changing the aircraft trajectory. In the steerable-laser case, we can simply point the laser at an area of steep terrain. Additionally, the burden of bringing in information that aids both the North and East directions of the filter is realized by simply pointing the laser to various orthogonal terrain gradients, rather than having to physically fly your aircraft over them in the nadir case.
Ray-casting Jacobian w.r.t. terrain gradient
The second strategy is to maximize the slant angle when steering the laser to take a measurement. This is demonstrated in Figure 4, where 𝜃 denotes the slant angle. Three sets of measurements are taken at increasing slant angles. Measurement 𝑎 is the no-information case described previously of a nadir laser over flat terrain. Measurements 𝑏 and 𝑐 are slant-range measurements to identical terrain gradients, only varying by the actual slant angle. It is clear from Figure 4 that increasing the slant angle will increase the ray-casting measurement Jacobian. In this case, the slant-range of measurement 𝑐 changes the most with respect to a change in the North position. In summary, pointing a laser at the maximum slant angle will bring in more information to a navigation filter. Note that in practice the slant angle will be limited by several factors. First, most gimbaled lasers will have a limited field of view. Second, higher slant angles lead to much further laser range measurements, and eventually, the range may exceed sensor specifications. Longer laser range measurements will begin to have errors dominated by atmospheric effects. This indicates that in practice, with a limited field of view which does not cause laser range measurements to exceed sensor specifications, the optimal steering angles will tend to fall toward the outer limits of the field of view of the gimbaled laser.
Ray-Casting Jacobian w.r.t. slant-angle
5 MAXIMIZING THE FIM FOR STEERABLE TRN
We have established that steering a laser can provide opportunities to increase the information content of a slant-range measurement. Methods for calculating the optimal steering angles are now discussed.
Recall that at any given time we are trying to maximize the information content of slant-range measurements using Equation (18). It is not correct to say we are maximizing the FIM, however, because the maximum of a matrix is not a well-defined mathematical concept. The trace of the matrix (specifically the position error states) is a good heuristic to try. To gain an understanding of the function we are attempting to maximize, we will discretize the continuous set of possible laser gimbal angles (pitch and yaw) over a realistic DEM. An illustration of this concept is shown in Figure 5. The red cone in Figure 5 indicates all the possible angles the laser could be steered. At every angle, the measurement Jacobian of the slant-range measurement can be calculated with finite differencing. The two black lines originating at the top of the cone denote two possible steering angles for the laser.
Illustration of gimbaling a laser by two different pitch and yaw angles. The measurement Jacobians for each case can be evaluated and compared for information content. The red cone gives the field of view of the laser, and the two black lines show two possible steering angles.
Figure 6 shows the first of two flight profiles over California along with the associated DEM (DTED level 2). A point halfway through this trajectory was chosen to compute the laser slant-range measurement Jacobian with respect to the North direction. Figure 7 shows the Jacobian of the slant-range measurement on a two-dimensional grid for all yaw angles from 0-360 degrees and all pitch angles from 0-60 degrees (both with a resolution of 1 degree). There are several important observations that can be seen in Figure 7. First, there is clear confirmation in the fact that larger slant angles yield larger measurement Jacobians. This was demonstrated in Figure 4. Notice that the largest absolute values occur at a near 60-degree pitch. Second, it is obvious that the Jacobian function is very multimodal. This indicates that classic minimization methods like gradient descent are not likely to ever correctly converge on the global minimum. This is unfortunate because global optimization is a far more difficult problem than local optimization, which can rely on gradient-descent methods. When maximizing or minimizing highly nonlinear and highly multi-modal objective functions, techniques like particle swarm optimization can be used. Particle swarm optimization is a Monte-Carlo global optimization technique. As a Monte-Carlo technique, it suffers from the “curse of dimensionality” and only works well in low dimensional problems. Fortunately, we only have two variables to optimize over—the pitch and yaw angles of the gimbaled laser. Particle swarm optimization is therefore a good candidate for identifying the optimal gimbal angles. An off-the-shelf implementation of this algorithm in MATLAB was used to maximize our objective function. Finally, note that the dynamics of the gimbal are ignored in this simulation. The gimbal is assumed to point at the commanded angle at every time step.
Flat-terrain flight profile over California
Change in ray-casting measurement w.r.t. 30-meter change in North direction. Note the many local minimums and maximums of the function.
6 STEERABLE-LASER TRN CRLB RESULTS
This section details the results of a CRLB analysis for steerable-laser TRN when maximizing the information content of the laser-range measurements. Recall, we are maximizing this information by solving for optimal gimbal angles of our laser according to Equation (18). We will use the inverse of the FIM of Equation (14) to calculate the CRLB, which will predict navigation accuracies of the system.
The first flight test profile and associated DTED map was shown in Figure 6. This is labeled the “flat” profile as there was not a large amount of terrain height variation along this trajectory. An additional flight trajectory was considered, which is labeled the “mountainous” trajectory. This trajectory is shown in Figure 8
Mountainous-terrain flight profile over California
The results presented here seek to motivate the improvement gained by attempting to steer the laser into areas of high information content. The results expand on the findings of the author’s previous work in Carroll (2020). Three strategies employing the same process and measurement models given in Section 2, and encompassing both fixed and steering cases, will be evaluated and analyzed:
Single Laser Nadir Case (fixed)
Three Laser Tri-Pod Configuration Case (fixed)
Trace FIM Maximization Case
The nadir case is the standard method used by most legacy TRN systems. The terrain gradients directly below the aircraft’s trajectory determine the information content of the measurements, and no attempt is made to fly into areas of differing gradients.
The tri-pod case establishes a baseline to compare to the optimal steering strategy. At each time step, the laser uses a set of three fixed lasers spread out by 120 degrees of yaw and each at a maximum of 60 degrees of pitch from nadir. This configuration was similar to that used in the author’s prior work in Carroll (2020). This case exists to test the theory that bringing in higher quality measurements are more important than bringing in a higher quantity of measurements. This strategy is likely to bring in more varied terrain gradients than the nadir case because it collects three times the measurement information. Further, we have already established that pointing the lasers at steep slant angles is beneficial. While we expect this case to be better than the nadir case, it is important to note that no attempt to steer the laser into areas of high information content is used for the tri-pod case.
The trace FIM maximization seeks to use particle swarm optimization to maximize Equation (18). The outer product of the ray-casting measurement Jacobian 𝐇 creates a square matrix. When maximizing this matrix, we seek to maximize the trace of a subset of this matrix—the subblock associated with the position error states.
A comparison of the CRLB for the three steering strategies over the flat terrain profile is given in Figure 9. A comparison of the CRLB for the three steering strategies over the mountainous terrain profile is given in Figure 10. There are several important observations about the results of this CRLB analysis. The first result is that attempting optimal steering appears to drastically improve navigation accuracy. In both the flat profile as well as the mountainous profile, the optimal steering strategy far outperforms the baseline strategies of nadir and tri-pod. The flat profile shows improvements by about a factor of four, while the mountainous profile shows improvements up to a factor of 20. Note that the tri-pod case was utilizing three separate lasers at each time step, while the two optimal steering strategies were using only a single laser. This validates that it is more important to bring in informative measurements than simply bringing in more measurements. Single, steerable-laser systems are already a common sensor type on many military platforms. The fact that a new software strategy can utilize existing hardware sensors and obtain such large navigation accuracy improvements is a significant result.
Comparison of North CRLB for three steering strategies over flat terrain described in Section 6
Comparison of North CRLB for three steering strategies over mountainous terrain described in Section 6
It is interesting to note that the mountainous profile has greater relative improvements during specific periods of the flight compared to the flat profile. This appears reasonable as no amount of optimal steering can change the situation of flying over completely flat ground. If there are no “features” to steer the laser to, then no additional information can be brought into the navigation system. In the case of the mountainous profile, there are rich areas of orthogonal terrain gradients which can be utilized to drastically improve the CRLB. The results of the flat and mountainous profiles are summarized in Tables 2-3.
Comparison of steering strategy statistics for the flat trajectory profile
Comparison of steering strategy statistics for the mountainous trajectory profile
Perhaps un-intuitively, the proposed method also has excellent observability of INS altitude errors. Recall that most terrain following systems rely on some type of external altitude measurement, like a barometer. This is not needed with the proposed method, as shown in Figure 11. The filter clearly bounds the drift of the altitude error state. Note that the Pinson-15 dynamics model is unstable in the vertical channel and without measurement aiding would be quickly growing without bound. No other altitude aiding is occurring in the inertial mechanization algorithm. The filter reduces altitude errors in the steered cases to well below one meter.
Comparison of altitude error CRLB for three steering strategies over mountainous terrain described in Section 6
Finally, additional limitations of the steerable-TRN system must be discussed. While laser altimeters have certain clear advantages over radar altimeters, there are also downsides. In clear weather conditions, the laser altimeters can have extremely long ranges, and the narrow beam width allows for dramatic navigation improvements using optimal steering. Unlike a radar altimeter, however, the laser altimeter can become unusable if cloud cover exists along the beam’s flight-path. This leads to the interesting proposition that this technique may be most useful in situations like NASA’s upcoming Artemis missions. These Moon missions cannot rely on GNSS and have already listed TRN as a technology that will be explored. Using steerable TRN for lunar exploration missions realizes all of the demonstrated benefits of the method with none of the weather-related constraints that you would encounter within an atmosphere.
7 STEERABLE-LASER SIMULATION RESULTS
In addition to the CRLB results, a set of full simulations using the proposed steerable-laser TRN system was conducted. These simulations used the same flat and mountainous trajectories as shown previously but actually created realizations of drifting INS and noisy slant-range laser measurements. It is important to note one major change, which occurs in the optimal steering between the CRLB case and the full simulation case. In the CRLB case, the steering algorithm attempts to steer the laser to an optimal location based on the true position of the vehicle. Clearly, this is not possible in a navigation system whose very purpose is to estimate position. This creates a “chicken and egg” problem—how do you optimally steer a laser to point at a particular part of a DEM if the vehicle location is not actually known? In practice, this paradox is resolved by simply using the filter’s estimated position to calculate the steering angle. Realize that these TRN algorithms can achieve accuracies in the tens of meters even over mostly flat terrain and the steering algorithms are trying to point at large terrain features. A small amount of position error does not change or invalidate the steering algorithms’ attempt to point at a location of high information content within the DEM. The simulation results confirm that this apparent paradox does not stop the proposed method from greatly improving navigation accuracy.
The simulation attempted to demonstrate the benefits gained by optimal steering with respect to nadir measurements as a baseline. Figures 12-14 shows the results of the MPF position errors for the mountainous case.
MPF East error (mountainous case) optimal vs nadir steering
MPF North error (mountainous case) optimal vs nadir steering
MPF Altitude error (mountainous case) optimal vs nadir steering
Note that the optimal steering case shows clear improvement over the nadir case with the mean radial spherical error (MRSE) from the optimal steering case twice as small as the nadir case. Furthermore, the filter’s predicted covariance (1-Sigma) understands that the solution is known to a much more precise level for the optimal steering case with far tighter covariance bounds. For the mountainous case, the optimal steering achieved MRSE of 3.1 meters. The simulation did not make overly optimistic assumptions about measurement errors—assuming 10-meter 1-Sigma measurement errors. Errors like atmospheric effects and DEM errors would be described by this 10-meter 1-Sigma measurement uncertainty.
The steerable-laser TRN system was also simulated for the flat case as shown in Figures 15–17. It is clear that in general navigation performance decreases with respect to the mountainous case. This is an expected result. The optimal steering still shows a large relative improvement over the nadir case. Furthermore, the benefit of optimal steering is better demonstrated in the flat case, where there appears to be a larger relative improvement over the nadir case. This makes sense as there is a greater need to steer towards high information content parts of the DEM when flying over flatter terrain. The results for the flat-terrain trajectory are summarized in Table 4. The results for the mountainous trajectory are summarized in Table 5. The MRSE given are defined by Equation (21):
MPF East error (flat case) optimal vs nadir steering
MPF North error (flat case) optimal vs nadir steering
MPF Altitude error (flat case) optimal vs nadir steering
MPF Results: Comparison of optimal steering vs nadir steering on flat terrain
MPF Results: Comparison of optimal steering vs nadir steering on mountainous terrain
21
The flat case had a relative improvement of a factor of five while the mountainous case had a relative improvement of a factor of two. Note that while the overall relative improvements are around factors of 2-5, there are parts of the trajectory where the relative improvement is significantly larger—up to a factor of 20. It is also notable that the covariance of the nadir case shows large variations in the filter-predicted covariance. These changing covariances can be directly linked back to the changing Jacobians of the measurement function. When the measurement Jacobians are small, the covariance will tend to grow. When the measurement Jacobian is large, the predicted covariance will decrease. It is obvious that the covariance for the steerable case is far more consistent than the nadir case. This is because the steerable case is consistently pointing at areas with high terrain gradients, whereas the nadir case is sometimes flying over flat terrain. When flying over flat terrain, the nadir-case covariance grows significantly, while the steerable case seeks out high terrain gradients and keeps a low covariance. It is clear that the benefits of steerable-laser TRN are supported not only by the CRLB analysis but also by a full and realistic MPF-based simulation over real terrain data.
8 CONCLUSIONS
Steerable-laser TRN has many benefits over legacy TRN systems. The lasers can take measurements at much further distances with a narrow beam-width allowing strategies like optimal steering to be pursued. The optimal steering of a laser within a slant-range TRN algorithm can increase overall performance by a factor of three to four. One of the only downsides of using steerable-laser TRN is the inability to obtain measurements through weather. This indicates steerable TRN may find its niche in exo-atmospheric applications or low flying UAV’s, where weather disruptions are either nonexistent or have a much smaller effect. Finally, the fact that steerable-laser systems already exist on many airborne platforms offers an opportunity to take advantage of a viable source of APNT information with no need for new hardware development. Implementation of the method would simply require the development of software designed to employ the existing lasers for navigation purposes. When compared to other methods of APNT, steerable-laser TRN is a simple and cost-effective solution that warrants further pursuit.
HOW TO CITE THIS ARTICLE
Carroll JD, Canciani AJ. Terrain-referenced navigation using a steerable-laser measurement sensor. NAVIGATION. 2021;68:115–134. https://doi.org/10.1002/navi.406
AUTHOR CONTRIBUTIONS
The views expressed in this paper are those of the authors and do not reflect the official policy or position of the United States Air Force, Department of Defense, or U.S. Government.
PRODUCT ENDORSEMENT DISCLAIMER
Reference herein to any specific commercial product, process, or service by trade name, trademark, manufacturer, or otherwise does not necessarily constitute or imply its endorsement, recommendation, or favor by the United States Government, the Department of Defense, or the Air Force Institute of Technology.
APPENDIX A: PINSON 15 INS ERROR MODEL
The Pinson 15 is a mathematical model that employs 15 states to model inertial navigation system drift errors. This appendix gives expanded detail regarding the model, specifically defining the dynamics matrix and all of its components. Recall that the dynamics model, which defines the problem’s Bayesian recursion, is given by
A.1
where 𝐱 is the state vector, 𝐹 the dynamics matrix, and 𝐰 the contributing process noise.
𝐹 is a 15 by 15 dynamics matrix that describes the relationships between the states (Raquet, 2018). At the highest level:
A.2
where
A.3
A.4
A.5
A.6
A.7
A.8
A.9
A.10
A.11
is a direction cosine matrix (DCM) that rotates a vector from the sensor frame to the local North, East, Down navigation frame. Table A1 describes all parameters used in the above matrices.
Values used in the definition of the Pinson 17 Error Model
APPENDIX B: MARGINALIZED PARTICLE FILTER
The MPF takes advantage of states in the systems that have linear dynamics. Through a process called Rao-Blackwellization (Doucet et al. 2001), linear substructures within a nonlinear system are marginalized into linear states. The state vector is then partitioned into a nonlinear (𝑛) and a linear (𝑙) portion as shown in B.1.
B.1
With the state vector partitioned, the MPF runs a particle filter to estimate the nonlinear states and parallel Kalman filters, one for each particle, to estimate the linear states. This method allows for focusing of available computing resources on the nonlinear states while preserving the ability to consider other system variables. The following equations follow closely from Schön et al. (2005). Consider the discrete time, nonlinear system below.
B.2
B.3
Here, 𝐰𝑘 and 𝐯𝑘 are AWGN, and 𝑓(⋅) and ℎ(⋅) are nonlinear process and measurement models acting on the state vector. System inputs 𝐮𝑘 have been omitted from this example for simplicity. If the system described by Equations (B.2) and (B.3) can be reformulated as shown below, then a MPF can be used.
B.4
B.5
B.6
Equations (B.4), (B.5), and (B.6) define how the components of the partitioned state vector (Equation (B.1)) affect each other in the system characterized by Equations (B.2) and (B.3). Here, 𝐟 𝑛 refers to the portion of the state transition matrix that defines how the nonlinear states interact with each other between time steps. The other terms, 𝐟 𝑙, 𝐀𝑛 and 𝐀𝑙 are similarly defined and given by
B.7
where
B.8
B.9
B.10
When applying the MPF described above, it is helpful to consider the propagate and update steps from the basic Kalman filter. The MPF can be implemented in 6 steps:
STEP 1 - Filter Initialization and Particle Weighting (Update)
Initialize the particle states by drawing 𝑖 samples from a chosen distribution (often a Gaussian due to ease of use). Initialize all Kalman filters of the linear states with the same initial mean and covariance.
B.11
B.12
B.13
Using the prior distribution as proposal and understanding to be Gaussian, the weights for each particle and their associated KF can be calculated by
B.14
where represents a measurement residual and
the measurement covariance given by
B.15
B.16
The matrix 𝐂 represents a mapping of the linear states into the measurement.
STEP 2 - Kalman Filter Mean and Covariance Update
The Kalman filters in the MPF represent linear states, which can be considered Gaussian. Applying measurement information to update these states is accomplished by tracking each state’s mean and covariance.
B.17
B.18
where
B.19
B.20
STEP 3 - Calculate Filter Output
To achieve a useful output from the filter, a mean and covariance of each state’s distribution can be computed (Note: this may not be appropriate if the posterior distributions are highly non-Gaussian. In this case, other statistics may need to be considered). To calculate a total system mean and covariance, the particle and linear states must be treated separately.
B.21
B.22
B.23
B.24
Once the particle and linear state statistics have been computed, the partitioned state vector and covariance matrix can be constructed:
B.25
B.26
STEP 4 - Resampling
Resample the system when the number of effective particles, as described by Equation (B.27), drops below a user defined threshold. Because each particle has an associated Kalman filter, the bank of Kalman filters must also be resampled, taking care to carry both the KF mean and covariance forward in the resampling process.
B.27
STEP 5 - Particle Propagation
Allowing the discrete process model to act on the particles will propagate the particle states from one time step to the next. Redraw the samples as follows:
B.28
Here, represents the Cholesky decomposition of 𝑎 and 𝐖𝑛 a random sample drawn from the unit normal distribution of a size similar to
.
STEP 6 - Kalman Filter Propagation
Propagation of the Kalman filters between time steps is accomplished with the following equations:
B.29
B.30
where
B.31
B.32
and
B.33
B.34
B.35
Important Special Case - Single P Matrix
In the general MPF, each particle has an associated Kalman filter described by its own mean vector and covariance matrix. An important special case of the MPF occurs when 𝐀𝑛, 𝐀𝑙, and 𝐂 are independent of the nonlinear states 𝐱𝑛. In this situation, a single 𝐏 matrix can be used to describe the covariance associated with all of the Kalman filters.
For greater detail on the MPF and derivations of the equations presented, the interested reader should see Schön et al. (2005), Nordlund and Gustafsson (2009), and Doucet et al. (2001).
Footnotes
Present Address: Aaron J. Canciani 2950 Hobson Way WPAFB, OH 45433-7765
- Received March 26, 2020.
- Revision received October 13, 2020.
- © 2021 Institute of Navigation
This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.