A Distributed Radio Beacon/IMU/Altimeter Integrated Localization Scheme with Uncertain Initial Beacon Locations for Lunar Pinpoint Landing

Abstract

As a growing number of exploration missions have successfully landed on the Moon in recent decades, ground infrastructures, such as radio beacons, have attracted a great deal of attention in the design of navigation systems. None of the available studies regarding integrating beacon measurements for pinpoint landing have considered uncertain initial beacon locations, which are quite common in practice. In this paper, we propose a radio beacon/inertial measurement unit (IMU)/altimeter localization scheme that is sufficiently robust regarding uncertain initial beacon locations. This scheme was designed based on the sparse extended information filter (SEIF) to locate the lander and update the beacon configuration at the same time. Then, an adaptive iterated sparse extended hybrid filter (AISEHF) was devised by modifying the prediction and update stage of SEIF with a hybrid-form propagation and a damping iteration algorithm, respectively. The simulation results indicated that the proposed method effectively reduced the error in the position estimations caused by uncertain beacon locations and made an effective trade-off between the estimation accuracy and the computational efficiency. Thus, this method is a potential candidate for future lunar exploration activities.

1. Introduction

Safe and soft pinpoint landing (within 100 m at $3\sigma$ from the target site [1]) on an extraterrestrial body has been a central objective since the beginning of human space exploration missions. To date, many manned and unmanned landers have successfully landed on moons (Apollo and Chang’E), planets (Curiosity and Opportunity) and asteroids (Rosetta and Hayabusa-2), with landing footprints in the scale of kilometers [2,3]. Among all these celestial bodies, a growing number of exploration missions will land on the Moon in the future, as it is the most suitable outpost for deep space exploration. A robotic lunar base followed by a human base will likely be constructed during these missions, which will be the first potential implementation of co-located pinpoint landing [4]. With the existence of these infrastructures, the natural consequence is to make use of more ground sources to enhance the current onboard navigation capability for lunar landing missions using, for instance, radio beacons.

Radiometric localization is an extensively used technology on Earth. Although there are many specific kinds of signals and modulations for different applications, the information provided by radio beacons can be divided into three categories [5]: (a) range measurements, (b) range rate measurements and (c) bearing measurements. All of these have drawn attention to improving the accuracy of pinpoint landing. In 2008, NASA proposed a radio measurement-enhanced navigation architecture based on lunar relay satellites (LRS) and lunar communication terminals (LCT) for the Autonomous Landing and Hazard Avoidance Technology (ALHAT) project [6]. By combining range and range rate measurements from LRS and LCT with other basic measurements of the ALHAT sensor suite, the landing accuracy could reach a level of 10 m at $3\sigma$ [7].

Several studies regarding the optimization of beacon configurations have been presented, both in terms of the filter accuracy [7,8] and observabilities [9,10]. On this basis, Theil et al. from the German Space Center (Deutsches Zentrum für Luft- und Raumfahrt, DLR) investigated the impact of one to four ground beacons for the small integrated navigation for planetary exploration (SINPLEX) project of ESA [4,11]. In this research, bearing measurements, as well as range and range rate measurements, were utilized by the navigation filter to augment the navigation baseline of SINPLEX. Different cases of beacon locations and setups were compared in the Autonomous Terrain-Based Optical Navigation (ATON) project of DLR, which indicated that the impact of additional bearing measurements was quite small.

All studies listed above were based on a strong assumption that uncertainties only existed in the sensor measurements, which means that all beacons were stationary and their locations were precisely pre-determined. In fact, most ground beacons can only be located either by orbiters or by the deep space network on the order of hundreds of meters [12]. Without the protection of the atmosphere, the diurnal temperature variation on the Moon is more than 300 ${}^{°}$C. If radio beacons are stationary, meaning that they will be exposed to a harsh environment for a long time, the onboard electronic equipment will be fatally damaged. One possible solution is to maintain radio beacons in the lunar base and redeploy them before each landing mission. Such scenarios serve as a strong motivation to investigate a novel navigation scheme that can locate the lander and update all beacon locations at the same time, which is similar to the range-only simultaneous localization and mapping (RO-SLAM) approach in the field of robotics.

Almost all existing radio beacons/inertial measurement unit (IMU)-integrated navigation algorithms in pinpoint landing are formulated using the state by the covariance matrix $\mathbf{\Sigma }$ and the mean vector $\mathbf{\mu }$ of the multivariate Gaussian distribution and tracked via the extended Kalman filter (EKF) [4,11,13,14,15] or unscented Kalman filter (UKF) [16,17]. However, the computational complexity and memory space of these algorithms are both quadratic to the dimension of state vectors [18], which makes them unsuitable for dealing with this SLAM-like localization problem.

A large amount of strategies for RO-SLAM [19,20,21,22,23,24] have been developed in recent decades, in which the sparse extended information filter (SEIF) proposed by Thrun et al. [18] has been widely used. Unlike Kalman filters, SEIF represents the state by the information matrix $\mathbf{\Lambda }={\mathbf{\Sigma }}^{-1}$ and the information vector $\mathit{\eta }=\mathbf{\Lambda }\mathit{\mu }$, which makes the update stage more efficient due to the natural sparseness of the information matrix [25]. A conditional sparsification operation is implemented before the prediction stage to maintain the sparse representation of $\mathbf{\Lambda }$, which leads to a constant execution time online.

While SEIF has been demonstrated to be more efficient and scalable in many applications [26,27,28], the consistency and accuracy of SEIF may be worse than EKF due to three approximations [29]: (a) the linearization of the motion and measurement models, (b) the recovery of the mean vector by iterations and (c) the sparsification via approximated conditional independence. Several methods have been designed to refine the performance of SEIF, such as exactly SEIF (ESEIF) [30], exactly sparse delayed-state filter (ESDSF) [31] and hybrid SEIF (HSEIF) [32]; each method only addresses one aspect of the approximation errors.

Iterated versions of EKF and UKF can increase their consistency and robustness against approximation errors compared to the original versions [33]; however, the rate of convergence is frustrated due to the dense covariance matrix. Due to the sparseness of the information filter, the computational burden is no longer a problem for iterated SEIF. He et al. [34] proposed the first iterated SEIF (ISEIF) for autonomous underwater vehicles, which utilized the Gauss–Newton algorithm for measurement updates rather than the traditional linear combination of predictions and innovations. However, divergence may occur in the Gauss–Newton algorithm when the initial values are far from the optimal values or the coefficient matrix in the normal equation is approximately singular [35].

In addition to the efficiency, the update stage in SEIF is also additive, which makes it capable of deployment in distributed systems. Torres et al. [23] first proposed a distributed RO-SLAM scheme based on SEIF for an autonomous ground vehicle (AGV). The prediction stage and update stage are implemented on the AGV and surrounding beacons, respectively. Except for the traditional direct robot–beacon measurements, additional inter-beacon measurements are also integrated into the SEIF, which reduces uncertainty both in the map estimation and localization accuracy. This work was then extended to a 3D resource-constrained operation with an auxiliary selection tool which only integrates the most informative measurements into the SEIF [36]. Further analysis regarding the effect of inter-beacon measurements on the building of an information matrix followed in [37], validating the preservation of the main properties of SEIF.

Beacon initialization is another challenge in RO-SLAM due to the partial observability of range measurements. As a multi-mode problem, the initial estimation of beacon locations can be obtained either by delayed or undelayed initialization methods. The trilateration method [38,39,40] is the most simple and efficient delayed initialization method; however, it suffers from measurement noise and the odometry error of robots. Olson et al. [41] attempted to address this problem with a 2D probability grid, with a performance that was limited by the size and resolution of the grid.

The shortcomings of these two methods resulted in them being quickly replaced by another widely used delayed initialization method: particle filters (PFs) [23,36,37,42,43]. PFs model the probability density of the beacon location as a uniform distribution around the measurement circle, and then the weight of each particle is updated and finally converges to a single solution by accumulating successive range measurements. Even though PFs can provide more accurate results, their significant computational burden cannot be neglected, which leads to several corresponding investigations [44,45].

In contrast to the delay initialization method, the initial beacon location is modeled with a multi-hypothesis distribution, which makes it available to integrate range measurements from the start. Blanco et al. [46] first proposed a Gaussian mixture model (GMM)-based initialization strategy, which provides accurate results but makes the integration of inter-beacon measurements impossible as each beacon is inserted in an independent EKF. On the basis of this research, Felipe et al. [47] proposed a centralized EKF-based initialization framework, which allowed the integration of inter-beacon measurements, and they enhanced it with a outlier rejection filter [48,49]. Geneve et al. [50] proposed a short-delayed composite initialization method based on Euclidean parameterization and a two-hypothesis GMM, which showed a lower computational cost.

Based on all the existing studies listed above, a novel distributed radio beacon/IMU-integrated localization scheme based on an adaptive iterated sparse extended hybrid filter (AISEHF) is presented in this paper. To the best of our knowledge, this is the first time that beacon location errors have been taken into consideration in the integrated navigation architecture for lunar pinpoint landing. This scheme is inspired by the SEIF-based distributed framework sketched in [37] and the ISEIF proposed in [34], while several aspects have been modified according to the characteristics of lunar pinpoint landing:

• Only lander-beacon range measurements are utilized as inter-beacon measurements and are easily blocked by obstacles, such as craters on the lunar surface.
• A batch-least-squares trilateration algorithm similar to [39] was adopted for the initialization of beacon locations, as their prior rough estimations are available and the inertial measurement unit (IMU) used in our application is much more accurate than that used in the majority of RO-SLAM applications.
• The navigation state between two successive measurements was propagated by a hybrid form of the “mean vector + information matrix” to avoid the frequent conversion from the information vector to the mean vector, which can improve the computational efficiency of the prediction stage.
• An adaptive iteration algorithm with the damping factor was adopted in the update stage, which can both improve the accuracy and efficiency of the estimator.

The remainder of this paper is structured as follows. The problem formulation is given in Section 2. In Section 3, we detail the implementations of the proposed distributed localization scheme. In Section 4, we verify the performance of the proposed algorithm by simulations, which is followed by further discussions. Eventually, our conclusions are presented in Section 5.

2. Problem Formulation

As illustrated in Figure 1, the target application scenario of this paper is the final phase of the power descent, which is short both in terms of the flight time and downrange distance (commonly around 200 s and 5–10 km). Without generality, the landing frame ($\mathcal{L}$) can be chosen as the navigation frame. The origin of the $\mathcal{L}$ frame ${\mathit{o}}_l$ is located at the targeted landing site, and three coordinate axes (${\mathit{x}}_l$, ${\mathit{y}}_l$, and ${\mathit{z}}_l$) are aligned with the geographic directions east, north and up (ENU), respectively. Another significant coordinate system is the body frame $\mathcal{B}$, whose origin lies in the mass center of the lander, and the directions of the three axes are front–left–up (FLU); i.e., the ${\mathit{x}}_b$ axis points to the front along the body symmetric axis, the ${\mathit{y}}_b$ axis points to the left and is orthogonal to ${\mathit{x}}_b$ and the ${\mathit{z}}_b$ axis completes the right-handed orthogonal coordinate system. In addition, measurements of the laser altimeter are present in the sensor frame ($\mathcal{S}$), whose origin and ${\mathit{x}}_s$ axis are the same as the $\mathcal{B}$ frame but whose ${\mathit{y}}_s$ axis and ${\mathit{z}}_s$ axis are opposite to ${\mathit{y}}_b$ and ${\mathit{z}}_b$.

The kinematic equations of the lunar lander, defined with respect to the landing frame $\mathcal{L}$, are given as follows: [51]:

$$\left\{\begin{array}{cc}\hfill {\dot{\mathit{p}}}^{\mathcal{L}}& ={\mathit{v}}^{\mathcal{L}}\hfill \\ \hfill {\dot{\mathit{v}}}^{\mathcal{L}}& ={\mathit{C}}_{\mathcal{B}}^{\mathcal{L}}{\mathit{f}}_i^{\mathcal{B}}-{\left[2{\mathit{\omega }}_{im}^{\mathcal{L}}\right]}^{\wedge }{\mathit{v}}^{\mathcal{L}}+{\mathit{g}}^{\mathcal{L}}\hfill \\ \hfill {\dot{\mathit{C}}}_{\mathcal{B}}^{\mathcal{L}}& ={\mathit{C}}_{\mathcal{B}}^{\mathcal{L}}{\left[{\mathit{\omega }}_{ib}^{\mathcal{B}}\right]}^{\wedge }\hfill \end{array}\right.$$

(1)

where ${[·]}^{\wedge }$ is the skew symmetric operator; ${\mathit{p}}^{\mathcal{L}}$ is the lander position; ${\mathit{v}}^{\mathcal{L}}$ is the lander velocity; ${\mathit{C}}_{\mathcal{B}}^{\mathcal{L}}$ is the rotation matrix from $\mathcal{B}$ to $\mathcal{L}$; ${\mathit{f}}_i^{\mathcal{B}}$ and ${\mathit{\omega }}_{ib}^{\mathcal{B}}$, respectively, denote the specific force and the angular rate sensed by IMU, whose further interpretations can be found in [52]; and ${\mathit{g}}^{\mathcal{L}}$ is the gravity vector, whose magnitude is assumed to vary with the altitude as follows [51]:

$$\parallel \mathit{g}\parallel =\frac{g_0}{{\left(1+h/R_m\right)}^2}$$

(2)

where $g_0=1.622$ m/s${}^2$ is the gravity constant at the lunar surface, h is the current altitude of the lander and $R_m$ is the radius of the Moon. The superscripts that denote the corresponding frame are omitted for notational convenience in the following parts if there is no confusion.

The beacon receiver is attached on the bottom of the lander, which makes it directly visible from the ground beacons. Several radio beacons are mounted on rovers and pre-deployed along the designed ground track of the landing mission, while their prior rough locations are determined by the onboard navigation systems of the rover. Due to the limited effect of bearing measurements, each beacon is assumed to be equipped with a range-only sensor whose measurements $r_{li}$ are affected by independent Gaussian white noise $v_b$:

$$\left\{\begin{array}{cc}\hfill r_{li}& ={\overline{r}}_{li}+v_b,v_b\sim \mathcal{N}\left(0,{\sigma }_b\right)\hfill \\ \hfill {\overline{r}}_{li}& =\sqrt{\delta x^2+\delta y^2+\delta z^2}=\sqrt{{\left(x_l-x_{b_i}\right)}^2+{\left(y_l-y_{b_i}\right)}^2+{\left(z_l-z_{b_i}\right)}^2}\hfill \end{array}\right.$$

(3)

where ${\mathit{p}}_l=\left[x_l,y_l,z_l\right]$ and ${\mathit{p}}_{b_i}=\left[x_{b_i},y_{b_i},z_{b_i}\right]$, respectively, denote the positions of the lander and the ith beacon. In addition, only the lander-beacon range measurements are available, as there are too many obstacles (such as craters or huge rocks) on the lunar surface, which could heavily degrade the inter-beacon measurements. Considering the terrain accessibility of the rover, all ground beacons are assumed to be on the same horizontal plane. This coplanarity will lead to uncertainty in altitude estimation; thus, a laser altimeter is mounted next to the beacon receiver to offer additional range measurements $r_a$:

$$r_a=\frac{z_l}{cos\gamma cos\theta }+v_a,v_a\sim \mathcal{N}\left(0,{\sigma }_a\right)$$

(4)

where $\theta$ is the pitch angle, $\gamma$ is the roll angle and $v_a$ is the Gaussian white noise.

The flow diagram of the proposed distributed localization scheme is shown in Figure 2, which can be divided into two parts: the lander and the beacon. The prediction stage is continuously conducted onboard based on the IMU measurements. After the visible beacons have been detected, a message that contains the current estimated lander position ${\widehat{\mathit{p}}}_l$ will be transmitted to all visible beacons. When this message has been received by beacon $b_i$, it will switch from the “Standby” mode to the “Initialization” mode, in which the beacon location is initialized by minimizing the following batch-least-squares cost function with N range measurements:

$${\mathit{p}}_{b_i}^{*}=arg\underset{{\mathit{p}}_{b_i}}{min}\sum _{k=1}^N{∥r_{li}^k-\sqrt{{\left({\widehat{\mathit{p}}}_l^k-{\mathit{p}}_{b_i}\right)}^T\left({\widehat{\mathit{p}}}_l^k-{\mathit{p}}_{b_i}\right)}∥}^2$$

(5)

where N can be pre-determined offline and Equation (5) can be solved by the nonlinear optimization methods given in [53] with prior rough beacon locations as initial values.

When the initialization of $b_i$ is convergent, it starts to work in the “Localization” mode. Its contribution (local information vector ${\tilde{\mathit{\eta }}}_t^i$ and information matrix ${\tilde{\mathbf{\Lambda }}}_t^i$) to the update stage is calculated and sent back to the lander. Then, the locations of all visible beacons are inserted into the state vector, and their contributions are summed together as follows:

$${\tilde{\mathit{\eta }}}_t={\widehat{\mathit{\eta }}}_t+\sum _{i=1}^k{\mathit{S}}_i{\tilde{\mathit{\eta }}}_t^i,{\tilde{\mathbf{\Lambda }}}_t={\widehat{\mathbf{\Lambda }}}_t+\sum _{i=1}^k{\mathit{S}}_i{\tilde{\mathbf{\Lambda }}}_t^i{\mathit{S}}_i^T$$

(6)

where $\widehat{\left(·\right)}$ and $\widetilde{\left(·\right)}$ denote the variables obtained by the state prediction and measurement update, respectively. ${\mathit{S}}_i={\left[\begin{array}{ccccc}\mathbf{0}& \cdots & \mathit{I}& \cdots & \mathbf{0}\end{array}\right]}^T$ is a projection matrix that allocates ${\tilde{\mathit{\eta }}}_t^i$ and ${\tilde{\mathbf{\Lambda }}}_t^i$ at the corresponding position of ${\tilde{\mathit{\eta }}}_t$ and ${\tilde{\mathbf{\Lambda }}}_t$:

$${\mathit{S}}_i{\tilde{\mathit{\eta }}}_t^i=\left[\begin{array}{c}\mathbf{0}\\ ⋮\\ {\tilde{\mathit{\eta }}}_t^i\\ ⋮\\ \mathbf{0}\end{array}\right],{\mathit{S}}_i{\tilde{\mathbf{\Lambda }}}_t^i{\mathit{S}}_i^T=\left[\begin{array}{ccccc}\mathbf{0}& \cdots & \mathbf{0}& \cdots & \mathbf{0}\\ ⋮& \ddots & ⋮& & ⋮\\ \mathbf{0}& \cdots & {\tilde{\mathbf{\Lambda }}}_t^i& \cdots & \mathbf{0}\\ ⋮& & ⋮& \ddots & ⋮\\ \mathbf{0}& \cdots & \mathbf{0}& \cdots & \mathbf{0}\end{array}\right].$$

(7)

Finally, an iterated update was performed based on the global information vector ${\tilde{\mathit{\eta }}}_t$ and information matrix ${\tilde{\mathbf{\Lambda }}}_t$, which was followed by the sparsification step of SEIF when the number of visible beacons were out of bounds or some links in the information matrix were too weak [34].

3. Adaptive Iterated Sparse Extended Hybrid Filter

3.1. State Definition and Jacobians

Like EKF, the first-order Taylor series expansion was applied to linearize the state space model as follows in Equation (8) to deal with nonlinear propagation and measurement functions, where ${\mathit{w}}_t$ and ${\mathit{v}}_{t+1}$ are the white process noise and measurement noise, respectively.

$$\left\{\begin{array}{cc}\hfill {\mathit{\xi }}_{t+1}& =f({\mathit{\xi }}_t,{\mathit{u}}_t)+{\mathit{w}}_t\approx {\mathit{F}}_t{\mathit{\xi }}_t+{\mathit{G}}_t{\mathit{u}}_t+{\mathit{w}}_t,{\mathit{w}}_t\sim \mathcal{N}\left(\mathbf{0},{\mathit{Q}}_t\right)\hfill \\ \hfill {\mathit{z}}_{t+1}& =h\left({\mathit{\xi }}_{t+1}\right)+{\mathit{v}}_{t+1}\approx {\mathit{H}}_{t+1}{\mathit{\xi }}_{t+1}+{\mathit{v}}_{t+1},{\mathit{v}}_{t+1}\sim \mathcal{N}\left(\mathbf{0},{\mathit{R}}_{t+1}\right)\hfill \end{array}\right.$$

(8)

where ${\mathit{\xi }}_t={[{\mathit{x}}_t^T,{\mathit{B}}^T]}^T$ denotes the state vector at time t comprised of the lander state ${\mathit{x}}_t={[{\mathit{p}}_{lt}^T,{\mathit{v}}_{lt}^T]}^T$ and the set of visible beacons locations $\mathit{B}={[{\mathit{p}}_{b_1}^T,\cdots ,{\mathit{p}}_{b_n}^T]}^T$. As a navigation (or localization) filter, it is common to choose the specific force ${\mathit{f}}_i$ and gravity vector $\mathit{g}$ as the control input ${\mathit{u}}_t={[{\mathit{f}}_i^T,{\mathit{g}}^T]}^T$.

As $\mathit{B}$ is kept unchanged and is independent of ${\mathit{x}}_t$, we compute the Jacobians $\mathit{F}$ and $\mathit{G}$ from the kinematic model (Equation (1) with respect to the ${\mathit{x}}_t$, which is more efficient. The results are given as follows:

$$\mathit{F}=\frac{\partial f(\mathit{\xi },\mathit{u})}{\partial \mathit{x}}=\left[\begin{array}{cc}\mathit{I}& \Delta t\mathit{I}\\ \mathbf{0}& \mathit{I}-\Delta t{\left[2{\mathit{\omega }}_{im}^{\mathcal{L}}\right]}^{\wedge }\end{array}\right]$$

(9)

$$\mathit{G}=\frac{\partial f(\mathit{\xi },\mathit{u})}{\partial \mathit{u}}=\left[\begin{array}{cc}\mathbf{0}& \mathbf{0}\\ \Delta t{\mathit{C}}_{\mathcal{B}}^{\mathcal{L}}& \Delta t\mathit{I}\end{array}\right].$$

(10)

Similarly, based on the measurement function of Equation (3) and (4), the measurement Jacobian $\mathit{H}$ is given in Equations (11)∼(13), where all entries that are not given are zero.

$$\mathit{H}=\left[\begin{array}{c}{\mathit{H}}_{li}\\ ⋮\\ {\mathit{H}}_{lj}\\ {\mathit{H}}_a\end{array}\right]=\left[\begin{array}{ccccccc}\frac{\partial r_{li}}{\partial {\mathit{x}}_t}& \cdots & \frac{\partial r_{li}}{\partial {\mathit{p}}_{b_i}}& \cdots & \mathbf{0}& \cdots & \mathbf{0}\\ ⋮& & ⋮& \ddots & ⋮& & ⋮\\ \frac{\partial r_{lj}}{\partial {\mathit{x}}_t}& \cdots & \mathbf{0}& \cdots & \frac{\partial r_{lj}}{\partial {\mathit{p}}_{b_j}}& \cdots & \mathbf{0}\\ \frac{\partial r_a}{\partial {\mathit{x}}_t}& \cdots & \mathbf{0}& \cdots & \mathbf{0}& \cdots & \mathbf{0}\end{array}\right]$$

(11)

$${\mathit{H}}_{li}=\left[\begin{array}{cccccccc}\frac{\delta x}{r_{li}}& \frac{\delta y}{r_{li}}& \frac{\delta z}{r_{li}}& \cdots & -\frac{\delta x}{r_{li}}& -\frac{\delta y}{r_{li}}& -\frac{\delta z}{r_{li}}& \cdots \end{array}\right]$$

(12)

$${\mathit{H}}_a=\left[\begin{array}{cccc}0& 0& \frac{1}{cos\gamma cos\theta }& \cdots \end{array}\right].$$

(13)

3.2. Temporal Alignment of Asynchronous Measurements

Signal transmission or triggering delays will lead to a temporal misalignment between measurements from different sensors, which cannot be ignored in the integrated localization scheme. As depicted in Figure 3, a virtual measurement method based on linear interpolations was adopted in our scheme to eliminated the temporal misalignment. Assuming there are j IMU measurements between every two sequential beacon measurements and the two closest IMU measurements to $t_k$ are denoted as ${\mathit{m}}_{t_{k-1/j}}^i$ and ${\mathit{m}}_{t_{k/1}}^i$, the virtual measurement at $t_k$ can be obtained as follows:

$${\mathit{m}}_{t_k}^i=\frac{t_{k/1}-t_k}{t_{k/1}-t_{k-1/j}}{\mathit{m}}_{t_{k-1/j}}^i+\frac{t_k-t_{k-1/j}}{t_{k/1}-t_{k-1/j}}{\mathit{m}}_{t_{k/1}}^i.$$

(14)

When all measurements are temporally aligned, the state prediction and measurement update stage of SEIF are then executed separately.

3.3. Hybrid State Prediction

The typical prediction stage in SEIF can be separated into two main processes [31]—state augmentation and state marginalization—assuming the state vector ${\mathit{\xi }}_t$ is subjected to the conditional distribution given, ${\mathit{z}}_t$ and ${\mathit{u}}_{t-1}$, as expressed in Equation (15):

$$p\left({\mathit{\xi }}_t\right|{\mathit{z}}_t,{\mathit{u}}_{t-1})\sim \mathcal{N}(\left[\begin{array}{c}{\mathit{\mu }}_{{\mathit{x}}_t}\\ {\mathit{\mu }}_{\mathit{B}}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{{\mathit{x}}_t{\mathit{x}}_t}& {\mathbf{\Sigma }}_{{\mathit{x}}_t\mathit{B}}\\ {\mathbf{\Sigma }}_{\mathit{B}{\mathit{x}}_{\mathit{t}}}& {\mathbf{\Sigma }}_{\mathit{B}\mathit{B}}\end{array}\right])\sim {\mathcal{N}}^{-1}(\left[\begin{array}{c}{\mathit{\eta }}_{{\mathit{x}}_t}\\ {\mathit{\eta }}_{\mathit{B}}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Lambda }}_{{\mathit{x}}_t{\mathit{x}}_t}& {\mathbf{\Lambda }}_{{\mathit{x}}_t\mathit{B}}\\ {\mathbf{\Lambda }}_{\mathit{B}{\mathit{x}}_{\mathit{t}}}& {\mathbf{\Lambda }}_{\mathit{B}\mathit{B}}\end{array}\right]).$$

(15)

At time $t+1$, a new variable ${\mathit{x}}_{t+1}$, which represents the newest lander state, is first inserted into the state vector ${\mathit{\xi }}_t$, and the augmented distribution $\mathit{p}\left({\mathit{x}}_t,{\mathit{x}}_{t+1},\mathit{B}\right)\sim {\mathcal{N}}^{-1}\left({\overline{\mathit{\eta }}}_{t+1},{\overline{\mathbf{\Lambda }}}_{t+1}\right)$ is given as follows: 1.5

$${\overline{\mathbf{\Lambda }}}_{t+1}=\left[\begin{array}{ccc}{\mathbf{\Lambda }}_{{\mathit{x}}_t{\mathit{x}}_t}+{\mathit{F}}^T{\mathit{Q}}_t^{-1}\mathit{F}& -{\mathit{F}}^T{\mathit{Q}}_t^{-1}& {\mathbf{\Lambda }}_{{\mathit{x}}_t\mathit{B}}\\ -{\mathit{Q}}_t^{-1}\mathit{F}& {\mathit{Q}}_t^{-1}& \mathbf{0}\\ {\mathbf{\Lambda }}_{\mathit{B}{\mathit{x}}_t}& \mathbf{0}& {\mathbf{\Lambda }}_{\mathit{B}\mathit{B}}\end{array}\right]=\left[\begin{array}{cc}{\overline{\mathsf{\Lambda }}}_{t+1}^{\alpha \alpha }& {\overline{\mathsf{\Lambda }}}_{t+1}^{\alpha \beta }\\ {\overline{\mathsf{\Lambda }}}_{t+1}^{\beta \alpha }& {\overline{\mathsf{\Lambda }}}_{t+1}^{\beta \beta }\end{array}\right]$$

(16)

$${\overline{\mathit{\eta }}}_{t+1}=\left[\begin{array}{c}{\mathit{\eta }}_{{\mathit{x}}_t}-{\mathit{F}}^T{\mathit{Q}}_t^{-1}\left(f({\mathit{\mu }}_{{\mathit{x}}_t},{\mathit{u}}_t)-\mathit{F}{\mathit{\mu }}_{{\mathit{x}}_t}-\mathit{G}{\mathit{u}}_t\right)\\ {\mathit{Q}}_t^{-1}\left(f({\mathit{\mu }}_{{\mathit{x}}_t},{\mathit{u}}_t)-\mathit{F}{\mathit{\mu }}_{{\mathit{x}}_t}-\mathit{G}{\mathit{u}}_t\right)\\ {\mathit{\eta }}_{\mathit{B}}\end{array}\right]=\left[\begin{array}{c}{\overline{\mathit{\eta }}}_{t+1}^{\alpha }\\ {\overline{\mathit{\eta }}}_{t+1}^{\beta }\end{array}\right].$$

(17)

The propagated distribution $\mathit{p}\left({\mathit{x}}_{t+1},\mathit{B}|{\mathit{z}}_{\mathit{t}},{\mathit{u}}_t\right)$ can then be recovered by marginalizing out ${\mathit{x}}_t$ from the joint distribution $p\left({\mathit{x}}_t,{\mathit{x}}_{t+1},\mathit{B}|{\mathit{z}}_{\mathit{t}},{\mathit{u}}_t\right)$:

$${\widehat{\mathbf{\Lambda }}}_{t+1}={\overline{\mathbf{\Lambda }}}_{t+1}^{\beta \beta }-{\overline{\mathbf{\Lambda }}}_{t+1}^{\beta \alpha }{\left({\overline{\mathbf{\Lambda }}}_{t+1}^{\alpha \alpha }\right)}^{-1}{\overline{\mathbf{\Lambda }}}_{t+1}^{\alpha \beta }$$

(18)

$${\widehat{\mathit{\eta }}}_{t+1}={\overline{\mathit{\eta }}}_{t+1}^{\beta }-{\overline{\mathbf{\Lambda }}}_{t+1}^{\beta \alpha }{\left({\overline{\mathbf{\Lambda }}}_{t+1}^{\alpha \alpha }\right)}^{-1}{\overline{\mathit{\eta }}}_{t+1}^{\alpha }.$$

(19)

From Equations (16) and (17), the recovery of ${\mathit{\mu }}_t$ from ${\mathit{\eta }}_t$ must be performed before the state augmentation to re-linearize $\mathit{F}$ and $\mathit{G}$ [18]. This is not a problem for a synchronous application, as ${\mathit{\eta }}_t$ will be utilized in the following update stage, whereas in an asynchronous application, the prediction stage will be followed by several other prediction stages, meaning that the state recovery will be implemented several times. As the state recovery is commonly performed based on iterated algorithms, as with a pre-conditioned gradient, these extra steps will lead to an unnecessary computational burden.

Based on the analysis above, we proposed a hybrid form of the “mean vector + information matrix” for the prediction stage, which will be called the sparse extended hybrid filter (SEHF) in the following. SEHF propagates the uncertainty of ${\mathit{\xi }}_t$ using ${\mathbf{\Lambda }}_t$ as with the typical SEIF, while the expectation of ${\mathit{\xi }}_t$ is propagated by ${\mathit{\mu }}_t$ instead of ${\mathit{\eta }}_t$ as follows:

$${\widehat{\mathit{\mu }}}_{t+1}=\left[\begin{array}{cc}\mathit{F}& \mathbf{0}\\ \mathbf{0}& \mathit{I}\end{array}\right]{\mu }_t+\left[\begin{array}{c}\mathit{G}\\ \mathbf{0}\end{array}\right]{\mathit{u}}_t=\left[\begin{array}{c}\mathit{F}{\mathit{\mu }}_{{\mathit{x}}_t}+\mathit{G}{\mathit{u}}_t\\ {\mathit{\mu }}_{\mathit{B}}\end{array}\right].$$

(20)

Figure 4 shows a comparison of the typical form and our hybrid form prediction when there are two prediction stages between successive update stages. Clearly, in the hybrid form, the state recovery only needs to be performed once before the filter switches from the update stage to the prediction stage, regardless of the number of extra prediction stages, whereas it will increase with the number of extra prediction stages in the typical form. Although there is an extra operation in the hybrid form that constructs ${\mathit{\eta }}_t$ for the update stage, the corresponding computational cost is quite small, as this is a simple matrix multiplication.

3.4. Adaptive Iterated Measurement Update

3.4.1. Iterated Measurement Update

When the current measurement ${\mathit{z}}_{t+1}$ is achieved, the SEIF typically updates the new posterior $\mathit{p}\left({\mathit{\xi }}_{t+1}|{\mathit{z}}_{t+1},{\mathit{u}}_t\right)$ according to the chain rule based on existing distributions $\mathit{p}\left({\mathit{\xi }}_{t+1}|{\mathit{z}}_{\mathit{t}},{\mathit{u}}_t\right)\sim {\mathcal{N}}^{-1}\left({\widehat{\mathit{\eta }}}_{t+1},{\widehat{\mathbf{\Lambda }}}_{t+1}\right)$ and $\mathit{p}\left({\mathit{z}}_{t+1}|{\mathit{\xi }}_{t+1}\right)\sim \mathcal{N}\left(h\left({\mathit{\xi }}_{t+1}\right),{\mathit{R}}_{t+1}\right)$:

$${\tilde{\mathbf{\Lambda }}}_{t+1}={\widehat{\mathbf{\Lambda }}}_{t+1}+{\mathit{H}}^T{\mathit{R}}_{t+1}^{-1}\mathit{H}$$

(21)

$${\tilde{\mathit{\eta }}}_{t+1}={\widehat{\mathit{\eta }}}_{t+1}+{\mathit{H}}^T{\mathit{R}}_{t+1}^{-1}\left({\mathit{z}}_{t+1}-h\left({\widehat{\mathit{\mu }}}_{t+1}\right)+\mathit{H}{\widehat{\mathit{\mu }}}_{t+1}\right).$$

(22)

Varying from solving the measurement update via a linear combination of the prediction and innovation in the traditional filter, the iterated filter models it as a nonlinear least squares (NLS) problem as follows:

$$\begin{array}{cc}\hfill {\mathit{\xi }}_{t+1}^{\ast }=& arg\underset{{\mathit{\xi }}_{t+1}}{min}\left\{\frac{1}{2}{\left({\mathit{z}}_{t+1}-h\left({\mathit{\xi }}_{t+1}\right)\right)}^T{\mathit{R}}_{t+1}^{-1}\left({\mathit{z}}_{t+1}-h\left({\mathit{\xi }}_{t+1}\right)\right)\right.\hfill \\ \hfill +& \left.\frac{1}{2}{\left({\mathit{\xi }}_{t+1}-{\widehat{\mathit{\mu }}}_{t+1})\right)}^T{\widehat{\mathbf{\Sigma }}}_{t+1}^{-1}\left({\mathit{\xi }}_{t+1}-{\widehat{\mu }}_{t+1})\right)\right\}\hfill \\ \hfill =& arg\underset{{\mathit{\xi }}_{t+1}}{min}{\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}\right)\hfill \end{array}$$

(23)

where ${\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}\right)$ is the objective function of the NLS problem and ${\mathit{\xi }}_{t+1}^{\ast }$ is the optimal value when ${\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}\right)$ takes the minimum value, defined by the following matrices:

$$\mathit{Z}=\left[\begin{array}{c}{\mathit{z}}_{t+1}\\ {\widehat{\mathit{\mu }}}_{t+1}\end{array}\right],\mathit{G}\left({\mathit{\xi }}_{t+1}\right)=\left[\begin{array}{c}h\left({\mathit{\xi }}_{t+1}\right)\\ {\mathit{\xi }}_{t+1}\end{array}\right],\mathit{W}=\left[\begin{array}{cc}{\mathit{R}}_{t+1}^{-1}& \mathbf{0}\\ \mathbf{0}& {\widehat{\mathbf{\Sigma }}}_{t+1}^{-1}\end{array}\right].$$

(24)

The objective function ${\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}\right)$ can be rewritten into the following a matrix form [33]:

$$\begin{array}{cc}\hfill {\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}\right)& =\frac{1}{2}{\left(\mathit{Z}-\mathit{G}\left({\mathit{\xi }}_{t+1}\right)\right)}^T\mathit{W}\left(\mathit{Z}-\mathit{G}\left({\mathit{\xi }}_{t+1}\right)\right)\hfill \\ \hfill & =\frac{1}{2}\underset{e^T\left({\mathit{\xi }}_{t+1}\right)}{\underbrace{{\left(\mathit{Z}-\mathit{G}\left({\mathit{\xi }}_{t+1}\right)\right)}^T{\mathit{C}}^T}}\underset{e\left({\mathit{\xi }}_{t+1}\right)}{\underbrace{\mathit{C}\left(\mathit{Z}-\mathit{G}\left({\mathit{\xi }}_{t+1}\right)\right)}}\hfill \\ \hfill & =\frac{1}{2}{\parallel e\left({\mathit{\xi }}_{t+1}\right)\parallel }^2\hfill \end{array}.$$

(25)

Then, the iterative sequences of ${\mathit{\xi }}_{t+1}^i$ and ${\mathbf{\Lambda }}_{t+1}^i$ can be achieved as follows [34]:

$${\mathit{K}}^i={\left({\mathit{H}}_i^T{\mathit{R}}^{-1}{\mathit{H}}_i+{\widehat{\mathbf{\Lambda }}}_{t+1}\right)}^{-1}{\mathit{H}}_i^T{\mathit{R}}^{-1}$$

(26)

$${\mathit{\xi }}_{t+1}^{i+1}={\widehat{\mathit{\mu }}}_{t+1}+{\mathit{K}}^i\left[{\mathit{z}}_{k+1}-h\left({\mathit{\xi }}_{t+1}^i\right)-{\mathit{H}}_i\left({\widehat{\mathit{\mu }}}_{t+1}-{\mathit{\xi }}_{t+1}^i\right)\right]$$

(27)

$${\mathbf{\Lambda }}_{t+1}^i={\mathit{H}}_i^T{\mathit{R}}^{-1}{\mathit{H}}_i+{\widehat{\mathbf{\Lambda }}}_{t+1}$$

(28)

where ${\mathit{H}}_i$ is the Jacobian of the measurement function at ${\mathit{\xi }}_{t+1}^i$.

3.4.2. Iterated Measurement Update with Damping Factor

The iterative form update equations given above are obtained by solving the following normal equation of the Gauss–Newton algorithm:

$${\mathit{J}}_i^T{\mathit{J}}_i\Delta {\mathit{\xi }}_{t+1}^i=-{\mathit{J}}_i^{T}e\left({\mathit{\xi }}_{t+1}^i\right)=\Delta {\mathit{r}}_i$$

(29)

where ${\mathit{J}}_i$ represents the Jacobian of $e\left({\mathit{\xi }}_{t+1}\right)$ with respect to ${\mathit{\xi }}_{t+1}$ evaluated at ${\mathit{\xi }}_{t+1}^i$. As the coefficient matrix ${\mathit{J}}_i^T{\mathit{J}}_i$ is only positive semidefinite, there is no guarantee that $\Delta \mathit{\xi }$ is always along the descent direction. $\Delta \mathit{\xi }$ would become unstable when the initial value was far away from the solution as the second derivative (Hessian) of the object function was approximated by the Jacobian. To address these issues, a damped Gauss–Newton algorithm was proposed by Levenberg and Marquardt, successivel [53]. In this method, a positive damping factor $\lambda$ was added to the coefficient matrix of the normal equation, which is known as the damping factor:

$$\left({\mathit{J}}_i^T{\mathit{J}}_i+\lambda \mathit{I}\right)\Delta {\mathit{\xi }}_{t+1}^i=\Delta {\mathit{r}}_i,\lambda >0.$$

(30)

Clearly, $\left({\mathit{J}}_i^T{\mathit{J}}_i+\lambda \mathit{I}\right)$ is always positive and the method is equivalent to the steepest descent algorithm and the Gauss–Newton algorithm when $\mu \mathit{I}\gg {\mathit{J}}_i^T{\mathit{J}}_i$ and $\mu \mathit{I}\ll {\mathit{J}}_i^T{\mathit{J}}_i$, respectively. With this modification, the iterative sequence of ${\mathit{\xi }}_t$ is modified as follows (see Appendix A for more details):

$${\mathit{K}}^i={\left({\mathit{H}}_i^T{\mathit{R}}^{-1}{\mathit{H}}_i+{\widehat{\mathbf{\Lambda }}}_{t+1}+\lambda \mathit{I}\right)}^{-1}{\mathit{H}}_i^T{\mathit{R}}^{-1}$$

(31)

$$\left\{\begin{array}{cc}\hfill {\mathit{\xi }}_{t+1}^{i+1}& ={\mathbf{\Gamma }}_{t+1}{\widehat{\mathit{\mu }}}_{t+1}+{\mathit{K}}^i\left[{\mathit{z}}_{t+1}-h\left({\mathit{\xi }}_{t+1}^i\right)-\mathit{H}\left({\mathbf{\Gamma }}_{t+1}{\widehat{\mathit{\mu }}}_{t+1}-{\mathit{\xi }}_{t+1}^i\right)\right]\hfill \\ \hfill {\mathbf{\Gamma }}_{t+1}& =\frac{{\widehat{\mathbf{\Lambda }}}_{t+1}}{\lambda }\left[\mathit{I}-{\left(\mathit{I}+\lambda {\widehat{\mathbf{\Lambda }}}_{t+1}\right)}^{-1}\frac{{\widehat{\mathbf{\Lambda }}}_{t+1}}{\lambda }\right]\hfill \end{array}\right.$$

(32)

As the uncertainty of ${\mathit{\xi }}_t$ will only be updated when the iteration converges [33], the update of ${\mathbf{\Lambda }}_t$ is the same as Equation (28). As a hybrid form is adopted for the following prediction stages in our scheme, there is no need to convert ${\mathit{\mu }}_t$ to ${\mathit{\eta }}_t$, which further reduces the computational burden.

3.4.3. Damping Factor and Stopping Criteria

A proper value of $\lambda$ is vital to the iteration. One possible solution is to choose the initial value of $\lambda$ depending on the maximum value of the coefficient matrix:

$${\lambda }_0=\tau ·max\left(diag\left({\mathit{J}}_0^T{\mathit{J}}_0\right)\right),\tau \in [{10}^{-8},1]$$

(33)

and then update $\lambda$ based on the gain ratio $\rho$ at each iteration [54]:

$$\lambda =\left\{\begin{array}{c}\lambda \ast max\left(\frac{1}{3},1-{\left(2\rho -1\right)}^3\right),\nu =2;\rho >0\hfill \\ \lambda \ast \nu ,\nu =2\ast \nu ;\rho \le 0\hfill \end{array}\right.$$

(34)

where $\rho$ is the ratio of the actual and approximated decrease of the objective function:

$$\rho =\frac{{\mathit{\chi }}^2\left({\mathit{\xi }}_{t+1}^i\right)-{\mathit{\chi }}^2({\mathit{\xi }}_{t+1}^i+\Delta {\mathit{\xi }}_{t+1}^i))}{L\left(\mathbf{0}\right)-L(\Delta {\mathit{\xi }}_{t+1}^i)}$$

(35)

$$\begin{array}{cc}\hfill L\left(\mathbf{0}\right)-L(\Delta {\mathit{\xi }}_{t+1}^i)& =-{(\Delta {\mathit{\xi }}_{t+1}^i)}^T{\mathit{J}}_i^{T}e\left({\mathit{\xi }}_{t+1}^i\right)-\frac{1}{2}{(\Delta {\mathit{\xi }}_{t+1}^i)}^T{\mathit{J}}_i^T{\mathit{J}}_i\Delta {\mathit{\xi }}_{t+1}^i\hfill \\ \hfill & =\frac{1}{2}{(\Delta {\mathit{\xi }}_{t+1}^i)}^T\left(\Delta {\mathit{r}}_i+\lambda \Delta {\mathit{\xi }}_{t+1}^i\right).\hfill \end{array}$$

(36)

Another parameter of concern is the number of iterations, which balances the accuracy and efficiency of the iteration process. Commonly, this parameter is difficult to predetermine offline; thus, several adaptive stopping criteria are applied in the proposed algorithm. First, the iteration process should be terminated when the difference between two successive residuals is smaller than a threshold ${ϵ}_1$, which reflects the global minimum:

$$\parallel \Delta {\mathit{r}}_{i+1}-\Delta {\mathit{r}}_i\parallel <{ϵ}_1.$$

(37)

Another stopping criterion is related to the local minimum; i.e., the increment $\Delta {\mathit{\xi }}_{t+1}^i$ is smaller than the threshold ${ϵ}_2$:

$$\parallel \Delta {\mathit{\xi }}_{t+1}^i\parallel <{ϵ}_2.$$

(38)

Lastly, the maximum iterations $k_{max}$ should be settled in the case of a dead loop.

With all notations and definitions above, the complete working flow of the adaptive iterative update is summarized in Figure 5.

4. Results and Discussions

4.1. Simulation Scenario and Parameters

The simulation scenario is a final landing phase performed after the main break phase of the powered descent, as shown in Figure 6. The landing trajectory was simulated for 210 s with an initial position of $\left[-9797,0,5530\right]$ m, an initial velocity of $\left[85,0,0\right]$ m/s and an initial attitude of $\left[0,-15,0\right]$ in degrees. Figure 7 presents the profile of the specific force and angular rate in detail. Ten beacons were deployed uniformly along both sides of the ground track in the simulation, and their true locations are listed in

Table 1. The true locations of all ground beacons.

Beacon ID	Locations in $\mathcal{L}$ (m)	Beacon ID	Locations in $\mathcal{L}$ (m)
1	$\left[-\mathrm{10,467.97},-1353.06,0\right]$	2	$[-7647.32,1719.73,0]$
3	$\left[-7245.89,-1587.17,0\right]$	4	$\left[-5465.18,2107.20,0\right]$
5	$\left[-5149.39,-3005.92,0\right]$	6	$\left[-2578.25,2027.77,0\right]$
7	$\left[-2145.57,-876.16,0\right]$	8	$\left[-421.43,2305.71,0\right]$
9	$\left[676.64,-2427.07,0\right]$	10	$\left[1649.64,1817.82,0\right]$

. An initial position error of 200 m was added to their true positions to simulate the prior rough locations obtained by rovers.

With respect to the navigation filter, the number of measurements K in the beacon initialization in Equation (5) was chosen to be 50. The attitude was propagated based on raw gyroscope measurements and periodically calibrated by the star tracker independently with a residual around $9.1″$ [4]. The residuals of the initial position and velocity were set to 300 m and 30 m/s, respectively. The $1\sigma$ parameters of IMU, the laser altimeters and ground beacons are listed in

Table 2. Sensor parameters ($1\sigma$).

Parameter	Value	Parameter	Value
Accelerometer bias level	$0.29\mathrm{mg}$	Gyro bias level	$4.86\times {10}^{-4}\mathrm{rad}/\mathbf{s}$
Accelerometer random walk	$8.97\times {10}^{-2}\mathbf{m}\mathbf{g}/\sqrt{\mathbf{H}\mathbf{z}}$	Gyro random walk	$2.22\times {10}^{-5}\mathbf{r}\mathbf{a}\mathbf{d}/(\mathbf{s}·\sqrt{\mathbf{H}\mathbf{z}})$
Accelerometer white noise	$2.14\times {10}^{-3}\mathbf{m}\mathbf{g}/(\mathbf{s}·\sqrt{\mathbf{H}\mathbf{z}})$	Gyro white noise	$4.98\times {10}^{-6}\mathbf{r}\mathbf{a}\mathbf{d}/({\mathbf{s}}^2·\sqrt{\mathbf{H}\mathbf{z}})$
Beacon range noise	$10\mathbf{m}$	Altimeter range noise	$0.5\mathbf{m}$

, and their update frequencies are, respectively, settled to 200 Hz, 100 Hz, and 20 Hz.

Other parameters of the localization filter—i.e., the initial covariance matrix ${\mathit{p}}_0$, the process noise covariance matrices ${\mathit{Q}}_t$ and the measurement noise covariance matrices ${\mathit{R}}_{t+1}$—are given as follows:

$${\mathit{p}}_0=diag{\left[1\times {10}^4,1\times {10}^4,1\times {10}^4,1\times {10}^2,1\times {10}^2,1\times {10}^2,1\times {10}^4,\cdots ,1\times {10}^4\right]}_{(n+6)\times (n+6)}$$

(39)

$${\mathit{Q}}_k=diag{\left[0.5,0.1,5,0.005,0.0001,0.001\right]}_{6\times 6}$$

(40)

$${\mathit{R}}_{k+1}=diag{\left[1\times {10}^4,\cdots ,1\times {10}^4,25\right]}_{(n+1)\times (n+1)}$$

(41)

where n is the number of available beacons.

The simulation system was implemented in C++ using the Robot Operating System (ROS) on a laptop with an Intel Core™i7-4790 @ 3.68 GHz and 8 GB RAM. Each sensor runs an independent ROS node to simulate the real interactions.

4.2. Simulation Results and Analyses

The estimation errors of the navigation state obtained by the proposed AISEHF-based distributed localization scheme are shown in Figure 8. It can be seen from the results that the residuals converged into the $1\sigma$ uncertainty bound rapidly both in the triaxial position and velocity estimations with steady values less than 50 m and 5 m/s, which meet the demands of pinpoint landing.

Figure 9a presents the performance of the proposed method in the mapping of all ground beacons. Due to the initialization procedure, almost all beacon location errors converged within 50 s, and the average error was 32.41 m. From Figure 9b, there appears to be an offset between the estimated value and the true value both in the beacon locations and the ground track. This appearance is induced by the fact that there is neither an accurate initial position of the lander nor for ground beacons, which means that their absolute locations are not observable.

Then, the proposed scheme (Scheme 3) was further compared with another two schemes with the same sensor configurations: (a) the SEIF-based scheme proposed in [37] without inter-beacon measurements (Scheme 1) and (b) the ISEIF scheme proposed in [34] (Scheme 2). The performance of all three schemes was evaluated in 100 independent Monte Carlo runs. In each of the 100 runs, all random seeds (e.g., beacon locations and sensor noise) were changed randomly, while all parameters of estimator were kept unchanged. Two indicators—root-mean-square errors (RMSEs) and averaged RMSEs (ARMSEs), which are defined in Equation (42)—were adopted to acquire a fair result.

$$\begin{array}{cc}\hfill {ϵ}_{RMSE}\left(t\right)& =\sqrt{\frac{1}{K}\sum _{i=1}^K\frac{{\left({\tilde{\mathit{x}}}_t^i-{\mathit{x}}_t^i\right)}^T\left({\tilde{\mathit{x}}}_t^i-{\mathit{x}}_t^i\right)}{n}},\mathit{x}\in {\mathbb{R}}^n\hfill \\ \hfill {ϵ}_{ARMSE}& =\frac{1}{T}\sum _{t=0}^T{ϵ}_{RMSE}\left(t\right)\hfill \end{array}$$

(42)

where K is the number of Monte Carlo runs and T is the total time from the convergence to the end. ${\tilde{\mathit{x}}}_t^i$ and ${\mathit{x}}_t^i$, respectively, denote the estimated and true state at time t in the $i\mathrm{th}$ run.

Figure 10 gives the position RMSEs and the average computational cost of the three schemes mentioned above from 100 independent Monte Carlo runs. The improvement of Scheme 3 in the position estimation was $4.55\%$ w.r.t. Scheme 2 and $8.11\%$ w.r.t. Scheme 3. The velocity RMSEs are not given here due to the fact that the iteration algorithm mainly focused on addressing the linearization error caused by the linear approximation of the measurement function, and it can be seen from Equations (12) and (13) that triaxial velocities make no contribution to the measurement function in this case.

The computational cost of Scheme 3 was $3.64\%$ more w.r.t. Scheme 1 and $18.96\%$ less w.r.t. Scheme 2. The performance of the three schemes is summarized in

Table 3. The performance of different algorithms.

Algorithm	Position Averaged RMSE (ARMSE) (m)	Velocity ARMSE (m/s)	CPU Times (ms)
Scheme 1	29.71	2.69	20.26
Scheme 2	28.60	2.75	26.06
Scheme 3	27.30	2.68	20.93

, indicating that Scheme 3 results in a good trade-off between position estimation accuracy and computational efficiency.

The influence of the beacon number n is given in Figure 11. From the results, the position ARMSE continued to decrease when n was lower than 10 and increased to a large error in the order of kilometers, while the computational cost continued to increase with n. We defined an indicator call cost–ARMSE percentage, which was equal to $100/(Cost\ast ARMSE)$ to further illustrate the results and indicated that $n=10$ was the most suitable configuration in our case.

Figure 12 gives the comparison of the proposed scheme under different initial location errors. We can infer from the results that the batch-least-square trilateration initialization algorithm achieved a consistent residual (around 40 m) regardless of the beacon initial location error in our application, which also led to a good robustness of the proposed localization scheme to the beacon initial noise. Extensive results with the initialization procedure disabled are further illustrated, which further prove the conclusion.

Finally, another 100 independent Monte Carlo runs were performed to evaluate the final drops of the proposed method, and the results are plotted in Figure 13, showing a circular error probable (CEP) of 37.52 m and a mean value of $\left[35.66\mathbf{m},-12.17\mathbf{m}\right]$. Another interesting point is that the distribution of the final drops in the north (11.60 m) was much larger than that in the east (1.58 m). One possible reason for this phenomenon may lie in the geometrical configuration of all ground beacons, which has a significant effect on the observability of the whole system.

5. Conclusions

In this article, a distributed localization scheme was proposed to handle the situation of having no accurate prior knowledge regarding the beacon configuration in lunar pinpoint landing. This scheme integrated onboard IMU/altimeter measurements together with lander-beacon range measurements within a SEIF-based framework. The prediction and update stages of SEIF were replaced by a hybrid-form propagation and a damping iteration algorithm, respectively, which led to the so-called AISEHF. The simulation results indicated that the proposed scheme could satisfy the navigation demands of lunar pinpoint landing and showed a robustness to uncertain initial beacon locations. The Monte Carlo simulation indicated that our scheme makes a good trade-off between estimation accuracy and computational efficiency compared with the existing algorithms.

Although the research in this paper has proved to be significant, there are still some questions that require further investigation, such as the augmentation of the current range-only measurements with the range-rate and the optimization of the geometrical pattern of all ground beacons. Several realistic problems, such as transmission loss and glint noise, should be taken into consideration and verified by experiments. Our work will be expanded to include these issues in the future.