A Distributionally Robust Fusion Framework for Autonomous Multisensor Spacecraft Navigation during Entry Phase of Mars Entry, Descent, and Landing

Abstract

A robust multisensor navigation filter design for the entry phase of next-generation Mars entry, descent, and landing (EDL) is presented. The entry phase is the longest and most uncertain portion of a Mars landing sequence. Navigation performance at this stage determines landing precision at the end of the powered descent phase of EDL. In the present work, measurements from a ground-based radio beacon array, an inertial measurement unit (IMU), as well as an array of atmospheric and aerothermal sensors on the body of a Mars entry vehicle are fused using an M-estimation-based iterated extended Kalman filtering (MIEKF) framework. The multisensor approach enables an increased positioning accuracy as well as the estimation of parameters that are otherwise unobservable. Furthermore, owing to the proposed statistically robust filter formulation, states and parameters can be accurately estimated in the presence of non-Gaussian measurement noise. Deviations from normally distributed observation noise correspond to outlier events such as sensor faults or other sources of spurious sensor data such as interference. The proposed framework provides a significant reduction in estimation error at the parachute phase of EDL, thereby increasing the likelihood of a pinpoint landing at a chosen landing site. Six states and three parameters are estimated. The suggested method is compared to the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). Detailed simulation results show that the presented fusion architecture is able to meet future pinpoint planetary landing requirements in realistic sensor measurement scenarios.

1. Introduction

1.1. Background

As a planet that is believed to have conditions that could potentially support human life, Mars has been the focus of many interplanetary missions thus far. For future missions to Mars such as a Mars sample return and a Mars base, the need to have high-performance autonomous guidance, navigation, and control (GNC) systems has become apparent [1]. The 2020 NASA technology taxonomy presents entry, descent, and landing (EDL) as one of the foremost enablers of autonomous space exploration [2]. Future space missions will be expected to increasingly rely less on human intervention [3,4], this is especially true for the case of interplanetary missions because of large signal delays in communication systems which render real-time teleoperation impossible. A number of successful Mars landing missions have been carried out in the past few decades with a majority of them using Viking era EDL technology [5]. The accuracy requirements for future pinpoint landings on planetary surfaces have now become stringent. Significant strides have been made in the last few missions to Mars. However, the ability to have an increased total landed mass at high altitudes while at the same time having a high landing precision together with hazard avoidance capabilities still remains a challenge. Meeting future precision requirements necessitates innovations at different stages in the GNC pipeline which are beyond the state of the art [6].

The Safe and Precise Landing Integrated Capabilities Evolution (SPLICE) project has recently been introduced by NASA to meet future precision landing and hazard avoidance (PL and HA) requirements [7,8,9,10]. This project has set the landing requirement for future autonomous planetary and lunar missions to be a 50 m uncertainty ellipse (i.e., a maximum error of 50 m in both downrange and cross-range). The reasons for these requirements are the need to precisely land on areas with high scientific return, to be able to land fleets of autonomous spacecraft in close proximity to each other while mitigating the risk of collision and plume interaction between them, and the need to land safely within a region that contains landing hazards. Such high-precision requirements are particularly important for future Mars sample return, manned Mars landing, and Mars base missions. It is indicated within the SPLICE project that a cost- and performance-optimized multisensor approach at various stages of EDL is required to meet future landing requirements. Hence, the current IMU-only navigation procedures during Mars entry are inadequate for future precision landing missions.

The EDL procedure consists of an approach phase, a hypersonic entry phase, a parachute phase, and a powered descent phase [11,12]. The approach phase starts at the later stages of an Earth–Mars trajectory. Several trajectory correction maneuvers (TCMs) are performed in this stage in order to prepare for entry. The entry phase starts at about 125 km from the surface of Mars and terminates in a parachute deployment at approximately 10 km above the Mars surface. The entry vehicle loses much of its kinetic energy during that phase. The parachute phase further decelerates the lander and continues until the descent engines are activated. The powered descent stage is a rocket-powered maneuver that performs a soft landing and ends at touchdown. Guidance and navigation in this stage have received significant attention in the past decade [13,14,15]. The EDL sequence for NASA’s most recent mission called Mars 2020 is shown in Figure 1. At a high level, the present research focuses on increasing navigation accuracy during the entry phase so as to meet future autonomous pinpoint landing requirements.

The entry phase is the longest part of EDL. Furthermore, it is the most critical in terms of determining overall landing precision. The largest uncertainties within EDL are accumulated during that stage. There are three major factors that affect EDL performance during the entry phase: delivery errors at the atmospheric entry point, uncertain atmospheric conditions and wind disturbances on Mars, and errors in the estimates of aerodynamic parameters [16,17]. Accurate state and parameter estimation at the entry stage can significantly enhance the guidance and control performance of a lander, thereby giving desirable landing precision. Various aspects of Mars entry phase GNC are detailed in the aforementioned reference [1].

In contrast to other phases of EDL, there is a limited set of sensors that can be used for the entry phase navigation. In fact, all Mars missions so far have only used IMUs for entry navigation. Unless they are aided by external measurements, IMU-based navigation solutions are known to drift over time and give inadequate results for the needs of future Mars missions. Due to the entry vehicle heat-shield cover, onboard optical or radio navigation cannot be utilized during entry. In the literature, early work on improving Mars entry navigation was introduced by Pastor et al. [18]. That work proposed a set of surface or orbiting radio beacons to provide improved position solutions. IMU measurements were not included in the beacon-only filtering scheme that was presented. Furthermore, details on aerodynamic models and parameters were not provided. The authors in [19] described an IMU-based entry navigation method using a hierarchical filter architecture. Yet another IMU-based entry navigation scheme was the one introduced by Marschke et al. [20] where a multiple-model adaptive filter was utilized. The first comprehensive approach to integrated entry navigation for Mars landing was presented by Lévesque and Lafontaine [21]. The authors introduced a navigation scheme in which surface radio beacons and IMU data were fused using an unscented Kalman filter (UKF). A reasonably complete dynamic model as well as detailed observability analyses were provided for four different navigation scenarios. Robustness to any of the aforementioned error sources was not addressed in that work. Moreover, Doppler measurement, which is an important source of information, was not considered. Wu et al. [22] and Xiao et al. [23] presented an integrated navigation scheme in which unknown inputs to entry dynamics were considered. Their work illustrated the design of a robust three-stage Kalman filter to mitigate the effects of systematic errors caused by parameter uncertainties. The authors demonstrated three stages of a specific transformation that was applied in order to decouple the covariance matrices of an augmented system consisting of states and unknown inputs. The aforementioned work did not consider the estimation of the vehicle ballistic coefficient as well as the lift-to-drag ratio, which is critical for accurate control. Moreover, the unknown inputs were modeled as stochastic processes driven by white Gaussian noise. This assumption is relaxed in the present research to tackle non-Gaussian input measurement noise.

Research presented in [24] provided a mechanism to select optimized beacon locations for an integrated Mars entry navigation scheme that enhanced state estimation performance. The authors provided extensive observability analyses for various navigation scenarios. A desensitized extended Kalman filter for radio- and IMU-based entry navigation was presented by Li et al. [25]. An approach that attempted to mitigate the effects of unobservable uncertain parameters during entry was demonstrated in [26]. A multisensor approach that utilized real-time flush air data system information in addition to a single orbiting radio beacon and an IMU was detailed in [27]. An UKF was used to blend information from the three sources. This is the most recent approach in terms of adding more sensor modalities to enhance navigation performance and one that is being considered for future missions as well [28,29]. However, the authors did not consider the estimation of the vehicle ballistic coefficient nor the lift-to drag ratio, which are critical parameters for real-time entry navigation. Furthermore, the robustness of the navigation solution was not considered within its scope. Although not directly comparable to the present research, other research works that deserve mentioning involve methods that focus on a postflight analysis of EDL performance by combining data from various onboard sensors. Karlgaard et al. [30,31,32,33] produced a series of publications detailing the trajectory and atmospheric reconstruction for various Mars missions. The techniques outlined therewith were offline smoothing techniques that used data from IMU, aerodynamic, and aerothermal sensors for trajectory and atmospheric reconstruction.

1.2. Contribution

In the present research, the real-time fusion of measurements from multiple ground-based radio beacons, an IMU, and onboard environmental sensors for pressure and aeroheating is conducted to give highly accurate state and parameter estimates. The use of aerodynamic pressure and aerothermal sensor data for real-time navigation has not been realized on a Mars landing mission so far. The proposed sensor fusion is performed using a statistically robust (by statistically robust, we are referring to distributional robustness in particular) M-estimation-based iterated extended Kalman filter (MIEKF). Methods of robustifying the linear and extended Kalman filters against non-Gaussian noise have been presented in [34,35]. The present work handles non-Gaussian measurement noise in an iterated Kalman filtering context to get the added benefit of reduced linearization errors. This is a first attempt for the application considered in this paper, as far as the authors are aware. Outliers can occur in a number of realistic scenarios; in the case of sensor measurements, they can occur when sensors produce spurious data due to faults or they experience interference from external sources. The peculiar nature of noise phenomena that are influenced by outliers is that they are heavy-tailed (non-Gaussian). Hence, the conventional Gaussian noise model is inadequate to represent them. The performance of standard versions of the extended Kalman filter (EKF) and unscented Kalman filter (UKF) degrades appreciably in the face of outliers. On the contrary, the proposed method exhibits excellent performance when the Gaussian assumption on observation noise is violated. (Particle filters are good candidates for filtering in a non-Gaussian context. However, their computational expense is too high for potential onboard implementation for the application that is considered in this paper.)

Succinctly, the contribution of the present work is twofold:

• To our knowledge, this work is the first in the literature to tackle real-time multisensor Mars entry navigation in the face of non-Gaussian measurement noise. This accounts for more realistic measurement scenarios during the lander entry.
• Due to the utilization of additional sensors, the proposed scheme is, to our knowledge, the first to estimate the ballistic coefficient and reference atmospheric density independently and with high accuracy. Other related research works either estimate products of these parameters or require another projectile with a known ballistic coefficient to be launched from the entry vehicle to make the ballistic coefficient observable [21]. Other approaches involve trying to mitigate the effects of unobservable parameters on the state estimation using the considered Kalman filtering [26].

1.3. Contents of the Paper

The rest of the paper is organized as follows: Section 2 gives the details of the dynamic equations of motion and sensor observation relations. Section 3 provides a discussion on robust statistical estimation in the presence of measurement outliers. In Section 4, using the background developed in Section 3, the proposed M-estimation-based IEKF is introduced as an integrated navigation filter. Furthermore, for completeness, a brief description of alternative filtering methods is outlined in that section. In Section 5, the settings for our simulation experiments are presented. Results and discussion thereof are given in Section 6. Concluding remarks are made in Section 7. The appendices at the end of the paper provide information that supplements the main discussion.

2. Problem Formulation

2.1. Mars Entry Dynamic Equations of Motion

The geometry of motion of a Mars lander and the forces it experiences during entry are shown in Figure 2 [36].

The frame $X,Y,Z$ corresponds to the Mars-centered Mars-fixed (MCMF) frame, m. $X^{\prime },Y^{\prime },Z^{\prime }$ represents the position (vehicle-pointing) coordinate frame, p, which aligns its $X^{\prime }$-axis with the position vector of the entry vehicle. Its $Y^{\prime }$-axis points in the direction parallel to the equatorial plane and its $Z^{\prime }$-axis completes a right-handed system. For the illustration showing the aerodynamic forces, we take the vertical plane to be the $\mathit{r}-\mathit{v}$ plane, and the lift vector L is rotated out of this plane by the bank angle $\sigma$. M is the position of the vehicle. $X^{″}$,$Y^{″}$,$Z^{″}$ is an axis from the vehicle position M parallel to the vehicle-pointing axis. $x_1,y_1,z_1$ is a frame generated by rotating the $X^{″},Y^{″},Z^{″}$ frame by $\psi$ in the horizontal plane and then by $\gamma$ in the vertical plane. The vehicle is considered as a point-mass during entry, which eliminates the need for consideration of attitude dynamics. This assumption is widely supported in the literature [16,36]. Therefore, only a three-degree-of-freedom translational model is considered in this research.

Assuming a spherical planet model, the entry equations of motion in the MCMF frame are given by

$$\begin{array}{cc}\dot{r}\left(t\right)\hfill & =v\left(t\right)sin\gamma \left(t\right)\hfill \\ \dot{v}\left(t\right)\hfill & =-D\left(t\right)-\frac{\mu }{r^2\left(t\right)}sin\gamma \left(t\right)\hfill \\ \hfill & +{\omega }^2\left(t\right)r\left(t\right)cos\varphi \left(t\right)(sin\gamma \left(t\right)cos\varphi \left(t\right)-cos\gamma \left(t\right)sin\varphi \left(t\right)sin\psi \left(t\right))\hfill \\ \dot{\gamma }\left(t\right)\hfill & =\left(\frac{v\left(t\right)}{r\left(t\right)}-\frac{\mu }{r^2\left(t\right)v\left(t\right)}\right)cos\gamma \left(t\right)+\frac{L\left(t\right)}{v\left(t\right)}cos\sigma \left(t\right)+2\omega \left(t\right)cos\varphi \left(t\right)cos\psi \left(t\right)\hfill \\ & +{\omega }^2\left(t\right)r\left(t\right)cos\varphi \left(t\right)(cos\gamma \left(t\right)cos\varphi \left(t\right)+sin\gamma \left(t\right)sin\varphi \left(t\right)sin\psi \left(t\right))\hfill \\ \dot{\theta }\left(t\right)\hfill & =\frac{v\left(t\right)cos\gamma \left(t\right)sin\psi \left(t\right)}{r\left(t\right)cos\varphi \left(t\right)}\hfill \\ \dot{\varphi }\left(t\right)\hfill & =\frac{v\left(t\right)cos\gamma \left(t\right)cos\psi \left(t\right)}{r\left(t\right)}\hfill \\ \dot{\psi }\left(t\right)\hfill & =\frac{v\left(t\right)}{r\left(t\right)}sin\psi \left(t\right)cos\gamma \left(t\right)tan\varphi \left(t\right)+\frac{L\left(t\right)}{v\left(t\right)cos\gamma \left(t\right)}sin\sigma \left(t\right)\hfill \\ \hfill & +2\omega \left(t\right)(tan\gamma \left(t\right)cos\varphi \left(t\right)sin\psi \left(t\right)-sin\psi \left(t\right))\hfill \\ & -\frac{{\omega }^2\left(t\right)r\left(t\right)}{v\left(t\right)cos\gamma \left(t\right)}sin\varphi \left(t\right)cos\varphi \left(t\right)cos\psi \left(t\right),\hfill \end{array}$$

(1)

where $r\left(t\right)$ is the range of the vehicle from the center of Mars. $v\left(t\right)$ is the magnitude of the velocity vector, $\gamma \left(t\right)$ is the flight path angle measured from the local horizontal plane to the velocity vector, $\theta \left(t\right)$ is the longitude, $\varphi \left(t\right)$ is the latitude, $\psi \left(t\right)$ is the heading angle measured from east to north about the $X^{\prime }$-axis where $\psi \left(t\right)=0$ indicates a heading in the east direction, and $\sigma \left(t\right)$ is the commanded bank angle used as a control variable. $\omega \left(t\right)$ accounts for planetary rotation. Terms involving this variable are small and can be ignored for short-range flights such as the one considered in the present work. $\mu =4.282837\times {10}^{13}\frac{{\mathrm{m}}^3}{{\mathrm{s}}^2}$ is the gravitational constant of Mars. $D\left(t\right)$ and $L\left(t\right)$ are aerodynamic drag and lift accelerations, respectively, and are defined as

$$\begin{array}{c}\hfill D\left(t\right)=\frac{1}{2}\rho \left(t\right)v{\left(t\right)}^2\frac{C_D\left(t\right)S}{m},L\left(t\right)=\frac{1}{2}\rho \left(t\right)v{\left(t\right)}^2\frac{C_L\left(t\right)S}{m},\end{array}$$

(2)

where $\rho \left(t\right)$ is Mars’s atmospheric density, S is the reference surface area of the entry vehicle, $C_D\left(t\right)$ and $C_L\left(t\right)$ are the aerodynamic drag and lift coefficients, respectively. We define $B\left(t\right)=\frac{C_D\left(t\right)S}{m}$ as the inverse of the ballistic coefficient, and the dynamic pressure $\overline{q}\left(t\right)$ is defined as $\frac{1}{2}\rho \left(t\right)v{\left(t\right)}^2$. The atmospheric density $\rho$(t) follows an exponential model given by

$$\begin{array}{c}\hfill \rho \left(t\right)={\rho }_0\left(t\right)exp\left(\frac{r_0-r\left(t\right)}{h_s}\right),\end{array}$$

(3)

where ${\rho }_0\left(t\right)$ is the reference atmospheric density of Mars, $r_0=3437.2$ km is the reference radial position at 40 km above Mars’s surface, and $h_s=7.5$ km is the atmospheric scale height. The expression for $D\left(t\right)$ can now be represented as

$$\begin{array}{c}\hfill D\left(t\right)=B\left(t\right)\overline{q}\left(t\right)=\frac{B\left(t\right){\rho }_0\left(t\right)}{2}exp\left(\frac{r_0-r\left(t\right)}{h_s}\right)v{\left(t\right)}^2.\end{array}$$

(4)

The control for the entry vehicle is computed using

$$\begin{array}{c}\hfill u\left(t\right)=\frac{L}{D}\left(t\right)cos\sigma \left(t\right),\end{array}$$

(5)

which makes $\frac{L}{D}\left(t\right)$ an important parameter to estimate for the purposes of accurate guidance and control. Other critical parameters to estimate are the inverse ballistic coefficient $B\left(t\right)$ and the reference atmospheric density ${\rho }_0\left(t\right)$. Conventionally, ${\rho }_0\left(t\right)$ is taken to be a constant based on empirical data assuming nominal conditions near Mars’s surface. If conditions deviate significantly from the aforementioned nominal conditions, it can drastically affect navigation performance. It is therefore important to have high-accuracy estimates of ${\rho }_0\left(t\right)$ in real time during entry (Chapter 2, [37]). The augmented state and parameter vector, $\mathit{x}\left(t\right)\in {\mathbb{R}}^9$, is given by

$$\begin{array}{c}\hfill \mathit{x}\left(t\right)={\left[\begin{array}{ccccccccc}r\left(t\right)& v\left(t\right)& \gamma \left(t\right)& \theta \left(t\right)& \varphi \left(t\right)& \psi \left(t\right)& B\left(t\right)& {\rho }_0\left(t\right)& \frac{L}{D}\left(t\right)\end{array}\right]}^T.\end{array}$$

(6)

We can now present the state-space representation of (1) with the aforementioned parameters augmented as constants to be estimated. In a standard state-space form, we have

$$\begin{array}{c}\hfill \dot{\mathit{x}}\left(t\right)=\mathit{f}(\mathit{x}\left(t\right),\mathit{u}\left(t\right),t)=\left[\begin{array}{c}\dot{r}\left(t\right)\\ \dot{v}\left(t\right)\\ \dot{\gamma }\left(t\right)\\ \dot{\theta }\left(t\right)\\ \dot{\varphi }\left(t\right)\\ \dot{\psi }\left(t\right)\\ \dot{B}\left(t\right)\\ \dot{{\rho }_0}\left(t\right)\\ \dot{\left(\frac{L}{D}\right)}\left(t\right)\end{array}\right]=\left[\begin{array}{c}v\left(t\right)sin\gamma \left(t\right)\\ -\frac{B\left(t\right){\rho }_0\left(t\right)}{2}exp\left(\frac{r_0-r\left(t\right)}{h_s}\right)v{\left(t\right)}^2-\frac{\mu }{r{\left(t\right)}^2}sin\gamma \left(t\right)\\ \left(\frac{v\left(t\right)}{r\left(t\right)}-\frac{\mu }{r{\left(t\right)}^{2}v\left(t\right)}\right)cos\gamma \left(t\right)+\frac{L}{D}\left(t\right)cos\sigma \left(t\right)\frac{B\left(t\right){\rho }_0\left(t\right)}{2}exp\left(\frac{r_0-r\left(t\right)}{h_s}\right)v\left(t\right)\\ \frac{v\left(t\right)cos\gamma \left(t\right)sin\psi \left(t\right)}{r\left(t\right)cos\varphi \left(t\right)}\\ \frac{v\left(t\right)cos\gamma \left(t\right)cos\psi \left(t\right)}{r\left(t\right)}\\ \frac{v\left(t\right)}{r\left(t\right)}sin\psi \left(t\right)cos\gamma \left(t\right)tan\varphi \left(t\right)+\frac{L}{D}\left(t\right)\frac{sin\sigma \left(t\right)}{cos\gamma \left(t\right)}\frac{B\left(t\right){\rho }_0\left(t\right)}{2}exp\left(\frac{r_0-r\left(t\right)}{h_s}\right)v\left(t\right)\\ 0\\ 0\\ 0\end{array}\right].\end{array}$$

(7)

by adding process noise to the expression in (7), we can describe the continuous-time entry dynamic equations of motion as

$$\begin{array}{c}\hfill \dot{\mathit{x}}\left(t\right)=\mathit{f}(\mathit{x}\left(t\right),\mathit{u}\left(t\right),t)+\mathit{w}\left(t\right),\end{array}$$

(8)

where $\mathit{f}:{\mathbb{R}}^9\mapsto {\mathbb{R}}^9$, $\mathit{u}\left(t\right)\in \mathbb{R}$ is the control input, and $t\in \mathbb{R}$. The random vector $\mathit{w}\left(t\right)\in {\mathbb{R}}^9$ is normally distributed and satisfies

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{w}\left(t\right)\right]=0,\mathbb{E}\left[\mathit{w}\left(t\right){\mathit{w}}^T\left(\tau \right)\right]=\mathit{Q}\left(t\right)\delta (t-\tau )\in {\mathbb{R}}^{9\times 9},\forall t,\end{array}$$

(9)

where $\mathit{Q}\left(t\right)$ is the power spectral density function of $\mathit{w}\left(t\right)$, and $\delta (t-\tau )$ is the Dirac delta function defined by the property (known as sifting property in signal processing).

$$\begin{array}{c}\hfill {\int }_{-\infty }^{\infty }g\left(t\right)\delta (t-\tau )d\tau =g\left(\tau \right),\end{array}$$

(10)

for any real-valued function $g\left(t\right)$ which is continuous at $t=\tau$. The initial state vector, which we define to be $\mathit{x}\left(t_0\right)\in {\mathbb{R}}^9$, has a mean random vector given by

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{x}\left(t_0\right)\right]={\widehat{\mathit{x}}}_0,\end{array}$$

(11)

and its covariance is given by

$$\begin{array}{c}\hfill \mathbb{E}\left[(\mathit{x}\left(t_0\right)-{\widehat{\mathit{x}}}_0){(\mathit{x}\left(t_0\right)-{\widehat{\mathit{x}}}_0)}^T\right]=\mathit{P}\left(t_0\right)\in {\mathbb{R}}^{9\times 9}.\end{array}$$

(12)

we further assume that

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{w}\left(t\right){(\mathit{x}\left(t_0\right)-{\widehat{\mathit{x}}}_0)}^T\right]=0,\forall t.\end{array}$$

(13)

2.2. Measurement Models

Four information sources are considered in the proposed multisensor navigation scheme: an IMU on board the entry vehicle, a set of radio beacons on Mars’s surface, an array of pressure sensors on the entry vehicle heat shield, and a set of thermocouples on the entry vehicle forebody. The measurement relations for each information source are derived below.

2.2.1. Inertial Measurement Unit (IMU)

An inertial measurement unit is used to measure the nongravitational accelerations experienced by the entry vehicle. The measurements are carried out with respect to the entry vehicle body frame. IMU observations are affected by errors such axis misalignment ${\mathbf{\Delta }}_a\in {\mathbb{R}}^{3\times 3}$, scale-factor errors ${\mathit{S}}_a\in {\mathbb{R}}^{3\times 3}$, biases ${\mathit{b}}_a\in {\mathbb{R}}^3$, and random noise ${\mathit{\zeta }}_a\left(t_i\right)\in {\mathbb{R}}^3$. The accelerometer measurement ${\tilde{\mathit{a}}}^b\left(t_i\right)$ in the body frame, b, is given in terms of the true acceleration, ${\mathit{a}}^b\left(t_i\right)$ as

$$\begin{array}{c}\hfill {\tilde{\mathit{a}}}^b\left(t_i\right)=({\mathit{I}}_{3\times 3}+{\mathbf{\Delta }}_a)({\mathit{I}}_{3\times 3}+{\mathit{S}}_a)({\mathit{a}}^b\left(t_i\right)+{\mathit{b}}_a+{\mathit{\zeta }}_a\left(t_i\right)),\end{array}$$

(14)

where

$$\begin{array}{c}\hfill {\mathbf{\Delta }}_a=\left[\begin{array}{ccc}0& {\delta }_{a_{xz}}& -{\delta }_{a_{xy}}\\ -{\delta }_{a_{yz}}& 0& {\delta }_{a_{yx}}\\ {\delta }_{a_{zy}}& -{\delta }_{a_{zx}}& 0\end{array}\right],{\mathit{S}}_a=\left[\begin{array}{ccc}{s}_{a_x}& 0& 0\\ 0& s_{a_y}& 0\\ 0& 0& s_{a_x}\end{array}\right],\end{array}$$

(15)

and

$$\begin{array}{c}\hfill {\mathit{b}}_a=\left[\begin{array}{c}{b}_{a_x}\\ b_{a_y}\\ b_{a_z}\end{array}\right],{\mathit{\zeta }}_a\left(t_i\right)=\left[\begin{array}{c}{\zeta }_{a_x}\left(t_i\right)\\ {\zeta }_{a_y}\left(t_i\right)\\ {\zeta }_{a_z}\left(t_i\right)\end{array}\right].\end{array}$$

(16)

the measured acceleration in the velocity coordinate frame ${\mathit{a}}^v$ is given by

$$\begin{array}{cc}\hfill {\mathit{a}}^v& ={\left[\begin{array}{ccc}-D\left(t\right),& L\left(t\right)sin\sigma \left(t\right),& L\left(t\right)cos\sigma \left(t\right)\end{array}\right]}^T\hfill \\ \hfill & ={\left[\begin{array}{ccc}-D\left(t\right),& D\left(t\right)\frac{L}{D}\left(t\right)sin\sigma \left(t\right),& D\left(t\right)\frac{L}{D}\left(t\right)cos\sigma \left(t\right)\end{array}\right]}^T,\hfill \end{array}$$

(17)

and the velocity frame is related to the body frame by $a^b=T_v^b{a}^v$, where the definition of $T_v^b$ is given in Appendix A of this paper.

We chose the MCMF coordinate system for navigation. We therefore transformed the accelerometer measurements in the velocity frame to the MCMF frame using the following transformations.

$$\begin{array}{c}\hfill T_v^p=\left[\begin{array}{ccc}cos\gamma & sin\gamma & 0\\ -sin\gamma cos\psi & cos\gamma cos\psi & -sin\psi \\ -sin\gamma sin\psi & cos\gamma sin\psi & cos\psi \end{array}\right],T_p^m=\left[\begin{array}{ccc}cos\varphi cos\theta & -sin\theta & -sin\varphi cos\theta \\ cos\varphi cos\theta & cos\theta & -sin\varphi sin\theta \\ sin\varphi & 0& cos\varphi \end{array}\right],\end{array}$$

(18)

where p represents the position coordinate frame. We finally obtained the acceleration in MCMF frame as

$$\begin{array}{c}\hfill a^m=T_p^m{T}_v^p{T}_b^v{a}^b.\end{array}$$

(19)

let $T_p^m{T}_v^p{T}_b^v=T_b^m$. The discrete-time IMU measurements in the MCMF frame become

$$\begin{array}{c}\hfill {\tilde{\mathit{a}}}^m\left(t_i\right)=({\mathit{I}}_{3\times 3}+{\mathbf{\Delta }}_a)({\mathit{I}}_{3\times 3}+{\mathit{S}}_a)(T_b^m{\mathit{a}}^b\left(t_i\right)+{\mathit{b}}_a+{\mathit{\zeta }}_a\left(t_i\right)).\end{array}$$

(20)

2.2.2. Ground-Based Radio Beacon Array

Using radio-based ranging to aid in entry navigation comes with a unique challenge. The plasma sheath around the entry body can block radio waves, thereby causing signal blackouts. However, it was indicated in [21,38] that radio beacons that work at high frequencies can overcome the signal blackout issue. We propose using three beacons that work in the UHF band placed on the surface of Mars. The Electra transceiver discussed in [39] is well suited for such an application. The placement of the beacons is such that observability is maximized according to the Fisher information criterion [24]. An illustration of the measurement setup is shown in Figure 3. The range measurement from the kth beacon to the lander is given by

$$\begin{array}{c}\hfill {\tilde{R}}_k\left(t_i\right)=R_k\left(t_i\right)+b_{R_k}+{\zeta }_{R_k}\left(t_i\right),k=1,\dots ,3,\end{array}$$

(21)

where $R_k\left(t_i\right)\in \mathbb{R}$ is the true range from the kth beacon to the lander, $b_{R_k}\in \mathbb{R}$ is the range bias, and ${\zeta }_{R_k}\left(t_i\right)\in \mathbb{R}$ is the measurement noise.

The true range is given by

$$\begin{array}{c}\hfill R_k\left(t_i\right)={\parallel {\mathit{r}}_{lander}\left(t_i\right)-{\mathit{r}}_{beacon}{\left(t_i\right)}_k\parallel }_2,\end{array}$$

(22)

where ${\mathit{r}}_{lander}\left(t_i\right)\in {\mathbb{R}}^3$ is the lander position given by

$$\begin{array}{c}\hfill {\mathit{r}}_{lander\left(t_i\right)}=\left[\begin{array}{c}r\left(t_i\right)cos\varphi \left(t_i\right)cos\theta \left(t_i\right)\\ r\left(t_i\right)cos\varphi \left(t_i\right)sin\theta \left(t_i\right)\\ r\left(t_i\right)sin\varphi \left(t_i\right)\end{array}\right],\end{array}$$

(23)

and ${\mathit{r}}_{beacon}{\left(t_i\right)}_k\in {\mathbb{R}}^3$ is the known location of the kth beacon. The UHF transceivers further provide Doppler measurements. For the kth transceiver, we have

$$\begin{array}{c}\hfill {\tilde{\dot{R}}}_k\left(t_i\right)={\tilde{V}}_k\left(t_i\right)=\frac{{({\mathit{r}}_{lander}\left(t_i\right)-{\mathit{r}}_{beacon}{\left(t_i\right)}_k)}^T({\mathit{v}}_{lander}\left(t_i\right)-{\mathit{v}}_{beacon}{\left(t_i\right)}_k)}{R_k\left(t_i\right)}+{\zeta }_{V_k}\left(t_i\right),\end{array}$$

(24)

where ${\tilde{V}}_k\left(t_i\right),{\zeta }_{V_k}\left(t_i\right)\in \mathbb{R}$ and ${\mathit{v}}_{lander}\left(t_i\right)$, ${\mathit{v}}_{beacon}{\left(t_i\right)}_k\in {\mathbb{R}}^3$. ${\mathit{v}}_{lander}\left(t_i\right)$ and ${\mathit{v}}_{beacon}{\left(t_i\right)}_k$ are the lander and beacon velocity vectors respectively. For the ground-based beacons used in this research, ${\mathit{v}}_{beacon}{\left(t_i\right)}_k=0$. In vector form, we have

$$\begin{array}{cc}\hfill \tilde{\mathit{R}}\left(t_i\right)& =\mathit{R}\left(t_i\right)+{\mathit{b}}_r+{\mathit{\zeta }}_R\left(t_i\right),\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill \tilde{\mathit{V}}\left(t_i\right)& =\mathit{V}\left(t_i\right)+{\mathit{\zeta }}_V\left(t_i\right).\hfill \end{array}$$

(26)

2.2.3. Atmospheric and Aerothermal Sensor Suite

The Mars Entry, Descent, and Landing Instrumentation (MEDLI) used in Mars Science Laboratory (MSL) and its successor MEDLI2 used in Mars 2020 has been utilized to measure atmospheric and aerothermodynamic quantities [40,41,42]. However, these measurements were only used for postflight trajectory and atmospheric reconstruction, as well as aerodynamic performance analysis purposes. The MEDLI2 sensor suite provides information on the forebody and aftbody aeroheating and pressure of the entry vehicle. A similar architecture is utilized here, but in our work, we assume a set of sensors that give us a fairly accurate characterization of the stagnation-point pressure (using forebody pressure sensors) and heating rate (using forebody thermocouples) in real time to inform onboard guidance and control actions. (Measurement accuracy can be contingent upon flight regime; for instance, pressure transducers that are accurate for hypersonic regime may not give sufficient results for supersonic flight, but these details are not considered in this research.) An illustration of the geometry of the sensors on the heat shield is given in Figure 4. We use the location of sensors proposed in [28]. (The authors in that reference considered the location of pressure sensors only; we propose a similar geometry for both pressure and heating rate sensors in the present work. On actual Mars landers, the sensor placement is done systematically using an optimization approach.) The measurement of dynamic pressure (we assumed that an aggregate measurement from all corresponding sensor arrays was used) is given by

$$\begin{array}{c}\hfill {\tilde{q}}_s\left(t_i\right)=\frac{1}{2}\rho \left(t_i\right)v{\left(t_i\right)}^2+{\zeta }_{q_s}\left(t_i\right),\end{array}$$

(27)

where ${\zeta }_{q_s}\left(t_i\right)\in \mathbb{R}$ is the measurement noise. Upon incorporating the relation in (3), we obtain the form

$$\begin{array}{c}\hfill {\tilde{q}}_s\left(t_i\right)=\frac{1}{2}{\rho }_0\left(t_i\right)exp\left(\frac{r_0-r\left(t_i\right)}{h_s}\right)v{\left(t_i\right)}^2+{\zeta }_{q_s}\left(t_i\right).\end{array}$$

(28)

the measured convective heating rate ${\tilde{\dot{Q}}}_s\left(t_i\right)$ at the stagnation point is modeled by the Sutton–Graves relation [43] represented by

$$\begin{array}{cc}\hfill {\tilde{\dot{Q}}}_s\left(t_i\right)& =k{\left(\frac{\rho \left(t\right)v{\left(t_i\right)}^2}{R_n}\right)}^{\frac{1}{2}}v{\left(t_i\right)}^3+{\zeta }_{{\dot{Q}}_s}\left(t_i\right),\hfill \end{array}$$

(29)

where k is a planet-dependent constant, which, for Mars, equals $1.9027\times {10}^{-4}\frac{\sqrt{\mathrm{Kg}}}{\mathrm{m}}$, $R_n$ is the vehicle nose radius, and ${\zeta }_{{\dot{Q}}_s}\left(t_i\right)\in \mathbb{R}$ is the heating-rate measurement noise. Substituting (3) into (29), we get

$$\begin{array}{c}\hfill {\tilde{\dot{Q}}}_s\left(t_i\right)=k{\left(\frac{{\rho }_0\left(t_i\right)exp\left(\frac{r_0-r\left(t_i\right)}{h_s}\right)v{\left(t_i\right)}^2}{R_n}\right)}^{\frac{1}{2}}v{\left(t_i\right)}^3+{\zeta }_{{\dot{Q}}_s}\left(t_i\right).\end{array}$$

(30)

We now combine the measurements from all the sources mentioned so far into an aggregate measurement relation stated as

$$\begin{array}{c}\hfill \mathit{z}\left(t_i\right)=\left[\begin{array}{c}{\tilde{\mathit{a}}}^m\left(t_i\right)\\ \tilde{\mathit{R}}\left(t_i\right)\\ \tilde{\mathit{V}}\left(t_i\right)\\ {\tilde{q}}_s\left(t_i\right)\\ {\tilde{Q}}_s\left(t_i\right)\end{array}\right]=\left[\begin{array}{c}({\mathit{I}}_{3\times 3}+{\mathbf{\Delta }}_a)({\mathit{I}}_{3\times 3}+{\mathit{S}}_a)(T_b^m{\mathit{a}}^b\left(t_i\right)+{\mathit{b}}_a)\\ \mathit{R}\left(t_i\right)+{\mathit{b}}_r\\ \mathit{V}\left(t_i\right)\\ \frac{1}{2}{\rho }_0\left(t_i\right)exp\left(\frac{r_0-r\left(t_i\right)}{h_s}\right)v{\left(t_i\right)}^2\\ k{\left(\frac{{\rho }_0\left(t_i\right)exp\left(\frac{r_0-r\left(t_i\right)}{h_s}\right)v{\left(t_i\right)}^2}{R_n}\right)}^{\frac{1}{2}}v{\left(t_i\right)}^3\end{array}\right]+\left[\begin{array}{c}{\mathit{\zeta }}_a\left(t_i\right))\\ {\mathit{\zeta }}_R\left(t_i\right)\\ {\mathit{\zeta }}_V\left(t_i\right)\\ {\zeta }_{q_s}\left(t_i\right)\\ {\zeta }_{{\dot{Q}}_s}\left(t_i\right)\end{array}\right].\end{array}$$

(31)

Equation (31) is finally cast as a general nonlinear observation equation of the form

$$\begin{array}{c}\hfill \mathit{z}\left(t_i\right)=\mathit{h}(\mathit{x}\left(t_i\right),t_i)+\mathit{\zeta }\left(t_i\right),\end{array}$$

(32)

where h is the surjection, $h:{\mathbb{R}}^9\twoheadrightarrow {\mathbb{R}}^{11}$. $\mathit{x}\left(t_i\right)\in {\mathbb{R}}^9$, $\mathit{z}\left(t_i\right)$, $\mathit{\zeta }\left(t_i\right)$$\in {\mathbb{R}}^{11}$, and the following properties hold

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{\zeta }\left(t_i\right)\right]=0,\mathbb{E}\left[\mathit{\zeta }\left(t_i\right){\mathit{\zeta }}^T\left(t_j\right)\right]={\mathit{R}}_m\left(t_i\right){\delta }_{ij},\end{array}$$

(33)

where ${\mathit{R}}_m\left(t_i\right)\in {\mathbb{R}}^{11\times 11}$ is the measurement noise covariance matrix and ${\delta }_{ij}$ is the Kronecker delta function given by

$$\begin{array}{c}\hfill {\delta }_{ij}=\left\{\begin{array}{c}1,fori=j\hfill \\ 0,fori\ne j\hfill \end{array}\right..\end{array}$$

(34)

moreover, we have

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{\zeta }\left(t_i\right){(\mathit{x}\left(t_0\right)-{\widehat{\mathit{x}}}_0)}^T\right]=0,\mathbb{E}\left[\mathit{w}\left(t\right){\mathit{\zeta }}^T\left(t_i\right)\right]=0,\forall i,t.\end{array}$$

(35)

the key consideration made in this research is that, contrary to that of the process noise $\mathit{w}\left(t\right)$, we do not insist on the measurement noise $\mathit{\zeta }\left(t_i\right)$ being Gaussian. This allows us to model outlier events such as sensor faults and corrupted measurements.

3. Robust Statistical Methods in Estimation

One of the goals in statistical signal processing is estimating the parameters of signals embedded in noise [44,45]. The statistical nature of the noise is usually characterized by a Gaussian distribution. In a large number of applications, due to the central limit theorem, this assumption holds. However, when the Gaussian assumption is no longer valid, the estimation machinery that considers Gaussian noise can perform very poorly. In realistic scenarios, we are usually interested in measurement data that are contaminated by outlier of the type discussed in Section 1. Outliers are data points that are at a significant distance away from the bulk of the observed data and they render the distribution of the observations heavy-tailed [46]. Examples of well-known heavy-tailed distributions are Laplace, Cauchy, and $ϵ$-contaminated Gaussian distribution. Consider a linear measurement model of the form [47]

$$\begin{array}{c}\hfill y_i=\chi +{\nu }_i,i=0,\dots ,n,\end{array}$$

(36)

where $y_i$ is the observation, $\chi$ is the parameter to be estimated, and ${\nu }_i$ is the observation noise. In the Gaussian case, the observation noise density, parameterized by the mean ${\mu }_v$ and variance ${\sigma }_v^2$, takes the form

$$\begin{array}{c}\hfill p_0({\nu }_i;{\mu }_v,{\sigma }_v)=\frac{1}{\sqrt{2\pi {\sigma }_v^2}}exp(-\frac{{({\nu }_i-{\mu }_v)}^2}{2{\sigma }_v^2}).\end{array}$$

(37)

for Laplace and Cauchy densities, the corresponding pdfs are given by

$$\begin{array}{c}\hfill p_0({\nu }_i;{\mu }_v,{\sigma }_v)=\frac{1}{\sqrt{2{\sigma }_v^2}}exp(-\frac{\sqrt{2}|{\nu }_i-{\mu }_v|}{{\sigma }_v}),\end{array}$$

(38)

and

$$\begin{array}{c}\hfill p_0({\nu }_i;{\mu }_v,{\sigma }_v)=\frac{1}{\pi {\sigma }_v}\frac{{\sigma }_v^2}{{({\nu }_i-{\mu }_v)}^2+{\sigma }_v^2},\end{array}$$

(39)

respectively. The $ϵ$-contaminated Gaussian model is a mixture that is given by

$$\begin{array}{c}\hfill p_0({\nu }_i;{\mu }_v,{\lambda }_v)=(1-ϵ)\frac{1}{\sqrt{2\pi {\sigma }_v^2}}exp(-\frac{{({\nu }_i-{\mu }_v)}^2}{2{\sigma }_v^2})+ϵ\frac{1}{\sqrt{2\pi {\tau }_v^2}}exp(-\frac{{({\nu }_i-{\mu }_v)}^2}{2{\tau }_v^2}),\end{array}$$

(40)

where $ϵ$ is the fraction of outliers in the data, ${\tau }_v^2$ is the variance of the contaminating distribution, and ${\lambda }_v$ is the standard deviation of the mixture. The aforementioned non-Gaussian densities are shown in comparison to the Gaussian density in Figure 5. Using the $ϵ$-contaminated model (40), an illustration of the characteristics of outlier-contaminated Gaussian noise, ${\nu }_i$, is shown in Figure 6 for varying percentages of outliers, $ϵ$, in the data. Distributionally robust procedures that can mitigate the increase in estimation variance in the face of heavy-tailed noise are desirable in real-world applications. One such procedure is discussed below.

Once again, considering the measurement model (36) and assuming ${\nu }_i$ are independent and identically distributed, the likelihood function is given by

$$\begin{array}{c}\hfill {\mathcal{L}}_{\mathcal{F}}(y_1,\dots ,y_n;\chi )=\prod _{i=1}^n{p}_0(y_i-\chi ).\end{array}$$

(41)

the maximum likelihood estimator (MLE) of the parameter $\chi$ is the quantity $\widehat{\chi }$ which satisfies

$$\begin{array}{c}\hfill \widehat{\chi }=\widehat{\chi }(y_1,\dots ,y_n)=\underset{\chi }{arg\; max}{\mathcal{L}}_{\mathcal{F}}(y_1,\dots ,y_n;\chi ).\end{array}$$

(42)

from the monotonicity property of the logarithmic function and assuming $p_0$ is positive everywhere, we have

$$\begin{array}{c}\hfill \widehat{\chi }=\underset{\chi }{arg\; min}\sum _{i=1}^n\eta (y_i-\chi ),\end{array}$$

(43)

where $\eta =-ln\left(p_0\right)$. Equation (43) is general in that it works for all distributions satisfying the conditions imposed on $p_0$. For instance, if $p_0$ has a normal distribution

$$\begin{array}{c}\hfill p_0\left(y\right)=\frac{1}{\sqrt{2\pi {\sigma }_v}}exp\left(\frac{-{(y-\chi )}^2}{2{\sigma }_v^2}\right),\end{array}$$

(44)

$\eta =\frac{-{(y-\chi )}^2}{2{\sigma }_v^2}$, then we have (after ignoring constants that do not affect the optimization)

$$\begin{array}{c}\hfill \widehat{\chi }=\underset{\chi }{arg\; min}\sum _{i=1}^n{(y_i-\chi )}^2.\end{array}$$

(45)

if $p_0$ has a Laplace distribution

$$\begin{array}{c}\hfill p_0\left(y\right)=\frac{1}{\sqrt{2}{\sigma }_v}exp\left(\frac{-\sqrt{2}|y-\chi |}{{\sigma }_v}\right),\end{array}$$

(46)

then $\eta =|y-\chi |$, and the MLE for $\chi$ becomes

$$\begin{array}{c}\hfill \widehat{\chi }=\underset{\chi }{arg\; min}\sum _{i=1}^n|y_i-\chi |.\end{array}$$

(47)

taking the derivative of the negative log-likelihood function $\eta$, the MLE $\widehat{\chi }$ satisfies

$$\begin{array}{c}\hfill \sum _{i=1}^n\pi (y_i-\widehat{\chi })=0.\end{array}$$

(48)

where $\pi (·)={\eta }^{\prime }(·)$. (In the statistics literature, $\rho (·)$ and $\psi (·)$ are used as standard symbols instead of $\eta (·)$ and $\pi (·)$ used in this paper. We changed the notation in order to avoid confusion with the symbols for atmospheric density and heading angle used in the equations of motion).

The function $\eta (·)$ may be considered a penalty on measurement residuals (also referred to as a loss function in the literature), while the function $\pi (·)$ gives a measure of the sensitivity of the estimator to outliers in the observation.

A class of estimators called M-estimators can provide outlier-robust estimates without the need for preprocessing the data [48,49]. One such estimator is the Huber M-estimator. For such an estimator, for some real r, $\eta \left(r\right)$ is given by

$$\eta \left(r\right)=\left\{\begin{array}{cc}\frac{1}{2}{r}^2\hfill & \left|r\right|\le \alpha \hfill \\ \alpha \left|r\right|-\frac{1}{2}{\alpha }^2\hfill & \left|r\right|>\alpha .\hfill \end{array}\right.$$

(49)

$\eta \left(r\right)$ is a hybrid of ${\ell }_1$-norm penalty (corresponding to a Laplace observation noise) and ${\ell }_2$-norm penalty (corresponding to a Gaussian observation noise). An illustration of the ${\ell }_1$ and ${\ell }_2$ norm penalties is shown in Figure 7.

Differentiating (49), the sensitivity function $\pi \left(r\right)$ for the Huber penalty becomes

$$\pi \left(r\right)=\left\{\begin{array}{cc}r\hfill & \left|r\right|\le \alpha \hfill \\ \alpha \frac{r}{\left|r\right|}\hfill & \left|r\right|>\alpha ,\hfill \end{array}\right.$$

(50)

where $\alpha$ is a designer-chosen tuning parameter that defines the level of sensitivity of the estimator to outliers. Illustrations of $\eta \left(r\right)$ and $\pi \left(r\right)$ for the Huber M-estimator are shown in Figure 8. Depending on the choice of the parameter $\alpha$, one can design an estimator with varying degrees of response to outliers. Estimators such as the Huber M-estimator with bounded sensitivities for large residuals provide robust performance. In fact, this estimator is robust in the minimax sense [34,35], i.e., it is optimally robust against maximum performance degradation for an $ϵ$-deviation of data from normality assumptions.

For the case of a dynamic estimation in a Kalman filtering setting, the update stage is known to be equivalent to a static estimation problem [50]. Ruckdeschel et al. [35] proposed a Huberization of the state update derived from the Huber penalty discussed in the preceding section. In that formulation, the sensitivity function $\pi (·)$ takes as an argument the Kalman filter innovations sequence. This helps mitigate large measurements (likely outliers) that enter the update equation in an unbounded manner. This seemingly ad hoc procedure has minimax optimal robustness properties. (There are some conditions attached to this result, the reader is referred to [35] for details.) We integrated this concept into an iterated extended Kalman filter to gain the benefits of the reduced linearization errors of the iterated EKF while adding robustness to non-Gaussian measurement noise.

4. Navigation Filter Design

The discrete nonlinear state-space model containing the dynamics and observation relations is given by [51,52]

$$\begin{array}{cc}\hfill \mathit{x}\left(t_{i+1}\right)& =\mathit{f}(\mathit{x}\left(t_i\right),\mathit{u}\left(t_i\right),t_i)+\mathit{w}\left(t_i\right),\hfill \end{array}$$

(51)

$$\begin{array}{cc}\hfill \mathit{z}\left(t_i\right)& =\mathit{h}(\mathit{x}\left(t_i\right),t_i)+\mathit{\zeta }\left(t_i\right),\hfill \end{array}$$

(52)

where $t\in \mathbb{R}$, $\mathit{x}\left(t_i\right),\mathit{w}\left(t_i\right)\in {\mathbb{R}}^n$, $\mathit{u}\left(t_i\right)\in {\mathbb{R}}^p$, and $\mathit{z}\left(t_i\right),\mathit{\zeta }\left(t_i\right)\in {\mathbb{R}}^m$. Furthermore, f and h have the property that $\mathit{f}:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n$ and $\mathit{h}:{\mathbb{R}}^n\twoheadrightarrow {\mathbb{R}}^m$. The statistical properties of the noise random vectors $\mathit{w}\left(t_i\right)$ and $\mathit{\zeta }\left(t_i\right)$ are similar to those stated in Section 2. An important difference in the case of the discretized dynamics (51) is that

$$\begin{array}{c}\hfill \mathbb{E}\left[\mathit{w}\left(t_i\right){\mathit{w}}^T\left(t_j\right)\right]=\mathit{Q}\left(t_i\right){\delta }_{ij},\end{array}$$

(53)

in which case $\mathit{Q}\left(t_i\right)$ is the process noise’s covariance matrix rather than the process noise’s power spectral density. Before explaining the proposed state estimation method, we briefly discuss known techniques used for navigation filtering based on relations (51) and (52).

4.1. Established Methods

4.1.1. Extended Kalman Filter (EKF)

Based on a first-order Taylor series approximation of the dynamic and measurement equations, a cycle of the well-known extended Kalman filter is provided in Algorithm 1.

Algorithm 1 EKF

Step 0: Initialize state estimate and state estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_0^{+}\right),\mathit{P}\left(t_0^{+}\right)\end{array}$$ (54) Step 1: Compute a linearized version of the process equation $$\begin{array}{c}\hfill \mathit{F}\left(t_{i-1}\right)=\frac{\partial \mathit{f}(\mathit{x}\left(t_{i-1}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1}))}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}\left(t_{i-1}^{+}\right)}\end{array}$$ (55) Step 2: Propagation $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{-}\right)& =\mathit{f}(\mathit{x}\left(t_{i-1}^{+}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1})\hfill \end{array}$$ (56) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{-}\right)& =\mathit{F}\left(t_{i-1}\right)\mathit{P}\left(t_{i-1}^{+}\right){\mathit{F}}^T\left(t_{i-1}\right)+\mathit{Q}\left(t_{i-1}\right)\hfill \end{array}$$ (57) Step 3: Compute a linearized version of the measurement equation $$\begin{array}{c}\hfill \mathit{H}\left(t_i\right)=\frac{\partial \mathit{h}(\mathit{x}\left({\mathit{t}}_{\mathit{i}}\right),t_i)}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}\left(t_i^{-}\right)}\end{array}$$ (58) Step 4: Update $$\begin{array}{cc}\hfill \mathit{K}\left(t_i\right)& =\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T\left(t_i\right)(\mathit{H}\left(t_i\right)\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T\left(t_i\right)+{\mathit{R}}_m^{-1}\left(t_i\right))\hfill \end{array}$$ (59) $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{+}\right)& =\widehat{\mathit{x}}\left(t_i^{-}\right)+\mathit{K}\left(t_i\right)(\mathit{z}\left(t_i\right)-\mathit{h}(\widehat{\mathit{x}}\left(t_i^{-}\right),t_i))\hfill \end{array}$$ (60) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{+}\right)& =(I-\mathit{K}\left(t_i\right)\mathit{H}\left(t_i\right))\mathit{P}\left(t_i^{-}\right){(I-\mathit{K}\left(t_i\right)\mathit{H}\left(t_i\right))}^T+\mathit{K}\left(t_i\right){\mathit{R}}_m\left(t_i\right){\mathit{K}}^T\left(t_i\right)\hfill \end{array}$$ (61)

4.1.2. Unscented Kalman Filter (UKF)

The UKF uses $2n+1$ (where n is the dimension of the state space) deterministically chosen points (called sigma points) to be transformed by the nonlinear dynamic and measurement equations [53]. This procedure is capable of capturing higher-order information compared to the linearization approach taken in the EKF. The UKF procedure is shown in Algorithm 2.

Algorithm 2 UKF

Step 0: Initialize state estimate and state estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_0^{+}\right),\mathit{P}\left(t_0^{+}\right)\end{array}$$ (62) step 1: Select sigma points $$\begin{array}{cc}\hfill \widehat{\mathit{x}}{\left(t_{i-1}\right)}^{\left(k\right)}& =\widehat{\mathit{x}}\left(t_{i-1}^{+}\right)+{\tilde{\mathit{x}}}^{\left(k\right)},k=1,\dots ,2n\hfill \end{array}$$ (63) $$\begin{array}{cc}\hfill {\tilde{\mathit{x}}}^{\left(k\right)}& ={\left(\sqrt{(n+\kappa )\mathit{P}(t_{i-1}^{+}})\right)}_k^T,k=1,\dots ,n\hfill \end{array}$$ (64) $$\begin{array}{cc}\hfill {\tilde{\mathit{x}}}^{(n+k)}& =-{\left(\sqrt{(n+\kappa )\mathit{P}(t_{i-1}^{+}})\right)}_k^T,k=1,\dots ,n\hfill \end{array}$$ (65) $$\begin{array}{cc}\hfill W^{\left(0\right)}& =\frac{\kappa }{n+\kappa },\hfill \end{array}$$ (66) $$\begin{array}{cc}\hfill W^{\left(k\right)}& =\frac{1}{2(n+\kappa )},k=1,\dots ,2n\hfill \end{array}$$ (67) $\kappa$ is a tuning parameter that can take on any real number as long as $n+\kappa \ne 0$. Step 2: Transform sigma points using nonlinear process equation $$\begin{array}{c}\hfill \widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)}=\mathit{f}(\widehat{\mathit{x}}{\left(t_{i-1}\right)}^{\left(k\right)},\mathit{u}\left(t_{i-1}\right),t_{i-1})\end{array}$$ (68) Step 3: Form a priori state estimate and covariance $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{-}\right)& =\sum _{k=1}^{2n}{W}^{\left(k\right)}\widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)}\hfill \end{array}$$ (69) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{-}\right)& =\sum _{i=1}^{2n}{W}^{\left(k\right)}(\widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{x}}\left(t_i^{-}\right)){(\widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{x}}\left(t_i^{-}\right))}^T+\mathit{Q}\left(t_{i-1}\right)\hfill \end{array}$$ (70) Step 4: Perform measurement update $$\begin{array}{cc}\hfill \widehat{\mathit{z}}{\left(t_i\right)}^{\left(k\right)}& =\mathit{h}(\widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)},t_i)\hfill \end{array}$$ (71) $$\begin{array}{cc}\hfill \widehat{\mathit{z}}\left(t_i\right)& =\sum _{k=1}^{2n}{W}^{\left(k\right)}\widehat{\mathit{z}}{\left(t_i\right)}^{\left(k\right)}\hfill \end{array}$$ (72) $$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{z}}\left(t_i\right)& =\sum _{i=1}^{2n}{W}^{\left(k\right)}(\widehat{\mathit{z}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{z}}\left(t_i\right)){(\widehat{\mathit{z}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{z}}\left(t_i\right))}^T+{\mathit{R}}_m\left(t_i\right)\hfill \end{array}$$ (73) $$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{xz}}\left(t_i\right)& =\sum _{i=1}^{2n}{W}^{\left(k\right)}(\widehat{\mathit{x}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{x}}\left(t_i^{-}\right)){(\widehat{\mathit{z}}{\left(t_i\right)}^{\left(k\right)}-\widehat{\mathit{z}}\left(t_i\right))}^T\hfill \end{array}$$ (74) $$\begin{array}{cc}\hfill \mathit{K}\left(t_i\right)& ={\mathit{P}}_{\mathit{xz}}\left(t_i\right){\mathit{P}}_{\mathit{z}}^{-1}\left(t_i\right)\hfill \end{array}$$ (75) $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{+}\right)& =\widehat{\mathit{x}}\left(t_i^{-}\right)+\mathit{K}\left(t_i\right)(\mathit{z}\left(t_i\right)-\widehat{\mathit{z}}\left(t_i\right))\hfill \end{array}$$ (76) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{+}\right)& =\mathit{P}\left(t_i^{-}\right)-\mathit{K}\left(t_i\right){\mathit{P}}_{\mathit{z}}\left({\mathit{t}}_{\mathit{i}}\right){\mathit{K}}^T\left(t_i\right)\hfill \end{array}$$ (77)

4.2. M-Estimation-Based Iterated Extended Kalman Filter (MIEKF)

In this subsection, we integrate the ideas of robust penalized estimation discussed in Section 3 into an iterated extended Kalman filter to come up with the MIEKF proposed in the present research. First, we present the iterated EKF algorithm and then add an extension to make it robust. Contrary to the update step of the EKF which linearizes $\mathit{h}(\mathit{x}\left(t_i\right),t_i)$ about the prior estimate $\widehat{x}\left(t_i^{-}\right)$, the IEKF linearizes $\mathit{h}(\mathit{x}\left(t_i\right),t_i)$ about the posterior estimate $\widehat{x}\left(t_i^{+}\right)$, which is a better estimate of the state $\mathit{x}\left(t_i\right)$. A repeated application of this procedure reduces linearization errors. (This improvement usually ceases after a small number of iterations.) Let us define $\mathit{x}{\left(t_i\right)}_k^{+}$ as the posterior estimate of the state after k measurement update iterations, $\mathit{P}{\left(t_i\right)}_k^{+}$ and $\mathit{K}{\left(t_i\right)}_k^{+}$ are the estimation error covariance and the Kalman gain at the kth measurement update iteration, respectively. $\mathit{H}{\left(t_i\right)}_k$ is the Jacobian of the measurement equation evaluated at the state estimate at the kth relinearization. A complete derivation of the IEKF using an optimization approach is provided in Appendix B. A single IEKF cycle is shown in Algorithm 3.

The IEKF generally performs better than the standard EKF due to a reduction in the linearization errors. However, in the presence of measurement outliers, The IEKF, EKF, and UKF, give reduced performances. In order to obtain the proposed MIEKF (Algorithm 4), we made modifications to the update step of the IEKF. Instead of the innovations term $\mathit{z}\left(t_i\right)-\mathit{h}(\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k,t_i)-\mathit{H}{\left(t_i\right)}_k)(\widehat{\mathit{x}}\left(t_i^{-}\right)-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k)$, we used a robust version related to Huber’s M-estimator discussed in Section 3.

Algorithm 3 IEKF

Step 0: Initialize state estimate and state estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_{i-1}^{+}\right),\mathit{P}\left(t_{i-1}^{+}\right)\end{array}$$ (78) Step 1: Compute a linearized version of the process equation $$\begin{array}{c}\hfill \mathit{F}\left(t_{i-1}\right)=\frac{\partial \mathit{f}(\mathit{x}\left(t_{i-1}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1}))}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}\left(t_{i-1}^{+}\right)}\end{array}$$ (79) Step 2: Propagation $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{-}\right)& =\mathit{f}(\mathit{x}\left(t_{i-1}^{+}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1})\hfill \end{array}$$ (80) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{-}\right)& =\mathit{F}\left(t_{i-1}\right)\mathit{P}\left(t_{i-1}^{+}\right){\mathit{F}}^T\left(t_{i-1}\right)+\mathit{Q}\left(t_{i-1}\right)\hfill \end{array}$$ (81) Step 3: Update Step 3.1: Set iteration count to $k=0$ and initialize IEKF with the prior found from the propagation step $$\begin{array}{c}\hfill \widehat{\mathit{x}}{\left(t_i^{+}\right)}_0=\widehat{x}\left(t_i^{-}\right)\end{array}$$ (82) $$\begin{array}{c}\hfill \mathit{P}{\left(t_i^{+}\right)}_0=\mathit{P}\left(t_i^{-}\right)\end{array}$$ (83) Step 3.2: With $k=k+1$, until $\parallel \widehat{\mathit{x}}{\left(t_i^{+}\right)}_{k+1}-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k\parallel <\delta$, perform N times $$\begin{array}{cc}\hfill \mathit{H}{\left(t_i\right)}_k& =\frac{\partial \mathit{h}(\mathit{x}\left({\mathit{t}}_{\mathit{i}}\right),t_i)}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k}\hfill \end{array}$$ (84) $$\begin{array}{cc}\hfill \mathit{K}{\left(t_i\right)}_k& =\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T{\left(t_i\right)}_k{(\mathit{H}{\left(t_i\right)}_k\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T{\left(t_i\right)}_k+{\mathit{R}}_m\left(t_i\right))}^{-1}\hfill \end{array}$$ (85) $$\begin{array}{cc}\hfill \widehat{\mathit{x}}{\left(t_i^{+}\right)}_{k+1}& =\widehat{\mathit{x}}\left(t_i^{-}\right)+\mathit{K}{\left(t_i\right)}_k(\mathit{z}\left(t_i\right)-\mathit{h}(\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k,t_i)-\mathit{H}{\left(t_i\right)}_k)(\widehat{\mathit{x}}\left(t_i^{-}\right)-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k)\hfill \end{array}$$ (86) $$\begin{array}{cc}\hfill \mathit{P}{\left(t_i^{+}\right)}_{k+1}& =(I-\mathit{K}{\left(t_i\right)}_k\mathit{H}{\left(t_i\right)}_k)\mathit{P}\left(t_i^{-}\right){(I-\mathit{K}{\left(t_i\right)}_k\mathit{H}{\left(t_i\right)}_k)}^T+\mathit{K}{\left(t_i\right)}_k{\mathit{R}}_m\left(t_i\right){\mathit{K}}^T{\left(t_i\right)}_k\hfill \end{array}$$ (87) where $\delta$ is a user-specified termination threshold. Step 3.3: Return posterior state estimate and estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_i^{+}\right)=\widehat{\mathit{x}}{\left(t_i^{+}\right)}_{N+1}\end{array}$$ (88) $$\begin{array}{c}\hfill \mathit{P}\left(t_i^{+}\right)=\mathit{P}{\left(t_i^{+}\right)}_{N+1}\end{array}$$ (89)

Algorithm 4 MIEKF

Step 0: Initialize state estimate and state estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_{i-1}^{+}\right),\mathit{P}\left(t_{i-1}^{+}\right)\end{array}$$ (90) Step 1: Compute a linearized version of the process equation $$\begin{array}{c}\hfill \mathit{F}\left(t_{i-1}\right)=\frac{\partial \mathit{f}(\mathit{x}\left(t_{i-1}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1}))}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}\left(t_{i-1}^{+}\right)}\end{array}$$ (91) Step 2: Propagation $$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(t_i^{-}\right)& =\mathit{f}(\mathit{x}\left(t_{i-1}^{+}\right),\mathit{u}\left(t_{i-1}\right),t_{i-1})\hfill \end{array}$$ (92) $$\begin{array}{cc}\hfill \mathit{P}\left(t_i^{-}\right)& =\mathit{F}\left(t_{i-1}\right)\mathit{P}\left(t_{i-1}^{+}\right){\mathit{F}}^T\left(t_{i-1}\right)+\mathit{Q}\left(t_{i-1}\right)\hfill \end{array}$$ (93) Step 3: Update Step 3.1: Set iteration count to $k=0$ and initialize IEKF with the prior found from the propagation step $$\begin{array}{c}\hfill \widehat{\mathit{x}}{\left(t_i^{+}\right)}_0=\widehat{x}\left(t_i^{-}\right)\end{array}$$ (94) $$\begin{array}{c}\hfill \mathit{P}{\left(t_i^{+}\right)}_0=\mathit{P}\left(t_i^{-}\right)\end{array}$$ (95) Step 3.2: With $k=k+1$, until $\parallel \widehat{\mathit{x}}{\left(t_i^{+}\right)}_{k+1}-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k\parallel <\delta$, perform N times $$\begin{array}{cc}\hfill \mathit{H}{\left(t_i\right)}_k& =\frac{\partial \mathit{h}(\mathit{x}\left({\mathit{t}}_{\mathit{i}}\right),t_i)}{\partial \mathit{x}}{|}_{\mathit{x}=\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k}\hfill \end{array}$$ (96) $$\begin{array}{cc}\hfill \mathit{K}{\left(t_i\right)}_k& =\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T{\left(t_i\right)}_k{(\mathit{H}{\left(t_i\right)}_k\mathit{P}\left(t_i^{-}\right){\mathit{H}}^T{\left(t_i\right)}_k+{\mathit{R}}_m\left(t_i\right))}^{-1}\hfill \end{array}$$ (97) $$\begin{array}{cc}\hfill \Delta \mathit{z}\left(t_i\right)& =\mathit{z}\left(t_i\right)-\mathit{h}(\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k,t_i)-\mathit{H}{\left(t_i\right)}_k(\widehat{\mathit{x}}\left(t_i^{-}\right)-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k)\hfill \end{array}$$ (98) $$\begin{array}{cc}\hfill \pi (\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right))& =\left\{\begin{array}{cc}\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)\hfill & |\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|\le \alpha \hfill \\ \alpha \frac{\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)}{|\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|}\hfill & |\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|>\alpha \hfill \end{array}\right.\hfill \end{array}$$ (99) $$\begin{array}{cc}\hfill \widehat{\mathit{x}}{\left(t_i^{+}\right)}_{k+1}& =\widehat{\mathit{x}}\left(t_i^{-}\right)+\pi (\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right))\hfill \end{array}$$ (100) $$\begin{array}{cc}\hfill \mathit{P}{\left(t_i^{+}\right)}_{k+1}& =(I-\mathit{K}{\left(t_i\right)}_k\mathit{H}{\left(t_i\right)}_k)\mathit{P}\left(t_i^{-}\right){(I-\mathit{K}{\left(t_i\right)}_k\mathit{H}{\left(t_i\right)}_k)}^T+\mathit{K}{\left(t_i\right)}_k{\mathit{R}}_m\left(t_i\right){\mathit{K}}^T{\left(t_i\right)}_k\hfill \end{array}$$ (101) where $\delta$ is a user-specified termination threshold. Step 3.3: Return posterior state estimate and estimation error covariance $$\begin{array}{c}\hfill \widehat{\mathit{x}}\left(t_i^{+}\right)=\widehat{\mathit{x}}{\left(t_i^{+}\right)}_{N+1}\end{array}$$ (102) $$\begin{array}{c}\hfill \mathit{P}\left(t_i^{+}\right)=\mathit{P}{\left(t_i^{+}\right)}_{N+1}\end{array}$$ (103)

$$\begin{array}{c}\hfill \Delta \mathit{z}\left(t_i\right)=\mathit{z}\left(t_i\right)-\mathit{h}(\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k,t_i)-\mathit{H}{\left(t_i\right)}_k(\widehat{\mathit{x}}\left(t_i^{-}\right)-\widehat{\mathit{x}}{\left(t_i^{+}\right)}_k).\end{array}$$

(104)

The sensitivity function $\pi$ defined in Section 3 now takes as an argument the product of the Kalman gain $\mathit{K}{\left(t_i\right)}_k$ and the innovations $\Delta \mathit{z}\left(t_i\right)$ at the kth measurement update iteration. The interpretation is that we are applying a robustifying procedure to each component for the vector $\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)$ to mitigate the outliers that enter unbounded through the sensor measurements $\mathit{z}\left(t_i\right)$. According to (50), we have

$$\pi (\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right))=\left\{\begin{array}{cc}\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)\hfill & |\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|\le \alpha \hfill \\ \alpha \frac{\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)}{|\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|}\hfill & |\mathit{K}{\left(t_i\right)}_k\Delta \mathit{z}\left(t_i\right)|>\alpha .\hfill \end{array}\right.$$

(105)

the MIEKF modifies the update iteration (step 3.2) of the IEKF. The rest of the algorithm is the same as that of the IEKF.

The IEKF is a tried and tested method for real-world navigation filter design. The MIEKF, which combines the practicality of the IEKF with the added feature of the robustness of Huber’s M-estimation increases the applicability of the MIEKF to real mission scenarios where the assumptions of Gaussianity on observation noise are not always applicable.

5. Simulation Experiments

Simulation Settings

Simulation experiments were carried out in MATLAB^®. Estimation results were obtained using non-Gaussian measurement noise using an $ϵ$-contaminated model with: $ϵ=0.05$ ($5\%$ contamination), $ϵ=0.20$ ($20\%$ contamination), and a more severe case with $ϵ=0.4$ ($40\%$ contamination). The results of each of the three scenarios are shown in Section 6. Initial conditions are taken from [21] and are shown in

Table 1. True and filter initial conditions for states and parameters used in simulations.

State/Parameter	Initial True State	Initial Filter State
Range (km)	3522.2	3521.2
Velocity ($\frac{\mathrm{m}}{\mathrm{s}}$)	6900	6910
Flight path angle (deg)	−12	−13
Longitude (deg)	0	0.02
Latitude (deg)	1	1.02
Heading angle (deg)	89	90
Inv. ball. coeff. ($\frac{{\mathrm{m}}^2}{\mathrm{kg}}$)	0.016	0.0176
Ref. atmospheric density ($\frac{\mathrm{kg}}{{\mathrm{m}}^3}$)	$2\times {10}^{-4}$	$2.2\times {10}^{-4}$
Lift-to-drag ratio	0.156	0.172

. Although the entry phase of EDL typically takes around 240 to 250 s, we ran our simulations for a longer time span to better visualize filter performance over a wide timeline. Time intervals in which there were outlier-influenced measurements were specified to be: 50–90 s, 170–220 s, 300–340 s, and 380–400 s (assuming outlier contamination throughout the entire entry phase is also possible in the proposed framework).

The bank angle $\sigma$ was taken to be $\frac{\pi }{4}$. The specifications of the IMU were directly taken from

Table 2. UHF transceiver locations on Mars’s surface.

	Longitude (deg.)	Latitude (deg.)
Beacon 1	0	0
Beacon 2	5.7	5.7
Beacon 3	−5.7	5.7

in [7]. A high-end IMU option was selected. The location of radio beacons on Mars’s surface are given in Table 2.

The filter parameters were chosen as follows:

Using the method outlined in (Chapter 9, [54]), the initial state error covariance was taken as

$$\begin{array}{c}\hfill \mathit{P}\left(t_0\right)=\left[\begin{array}{ccccccccc}2\times {10}^6& & & & & & & & \\ & {10}^4& & & & & & & \\ & & 3\times {10}^{-4}& & & & & & \\ & & & 9\times {10}^{-8}& & & & & \\ & & & & 1.23\times {10}^{-7}& & & & \\ & & & & & 9\times {10}^{-8}& & & \\ & & & & & & 2.56\times {10}^{-6}& & \\ & & & & & & & 4\times {10}^{-10}& \\ & & & & & & & & 2.56\times {10}^{-3}\end{array}\right].\end{array}$$

(106)

the process noise covariance matrix $\mathit{Q}\left(t_0\right)$ was taken to be

$$\begin{array}{c}\hfill \mathit{Q}\left(t_0\right)=\left[\begin{array}{ccccccccc}{10}^{-10}& & & & & & & & \\ & {10}^{-1}& & & & & & & \\ & & {10}^{-8}& & & & & & \\ & & & {10}^{-10}& & & & & \\ & & & & 0& & & & \\ & & & & & {10}^{-9}& & & \\ & & & & & & {10}^{-10}& & \\ & & & & & & & {10}^{-10}& \\ & & & & & & & & {10}^{-10}\end{array}\right].\end{array}$$

(107)

the measurement noise covariance ${\mathit{R}}_m\left(t_0\right)$ was taken as (these values were chosen based on actual sensor capabilities reported in [7], where applicable).

$$\begin{array}{c}\hfill {\mathit{R}}_m\left(t_0\right)=\left[\begin{array}{cccccccccc}{10}^2& & & & & & & & & \\ & {10}^2& & & & & & & & \\ & & {10}^2& & & & & & & \\ & & & 2.5\times {10}^{-5}& & & & & & \\ & & & & 2.5\times {10}^{-5}& & & & & \\ & & & & & 2.5\times {10}^{-5}& & & & \\ & & & & & & 4.9\times {10}^{-7}& & & \\ & & & & & & & 4.9\times {10}^{-7}& & \\ & & & & & & & & 4.9\times {10}^{-7}& \\ & & & & & & & & & {10}^{-6}& \\ & & & & & & & & & & {10}^{-6}\end{array}\right].\end{array}$$

(108)

Outliers were generated using an $ϵ$-contaminated model for the kth element of measurement noise vector. The model was given by (varying levels of outliers can be used in this model for each sensor depending on the prior crude knowledge of the measurement environment; here, we considered similar contamination levels for all sensors):

$$\begin{array}{c}\hfill \mathit{\zeta }{\left(t_i\right)}_k\sim (1-ϵ)\mathcal{N}(0,{\varsigma }_k^2)+ϵ\mathcal{N}(0,150)\end{array}$$

(109)

where ${\varsigma }_k^2$ is the measurement variance of each sensor (i.e., the diagonals of ${\mathit{R}}_m\left(t_0\right)$). The results of our simulation studies are given below. In each case, three candidate algorithms were compared: EKF, UKF, and MIEKF.

6. Results and Discussion

Detailed plots showing the estimation performance of the proposed method and comparison with alternative approaches are provided in Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18. Figure 9 shows the estimation performance of the various filters in the presence of a relatively mild outlier level of $5\%$.

The state estimates for the proposed method showed good adherence to the true values while alternative methods deviated from the true values during the time intervals where outlier influences occurred. The absolute values of the estimation errors are depicted in Figure 10. In this figure, one can see the clear performance benefits of the MIEKF-based navigation (shown in a solid black line) in outlier-influenced time intervals (shown in light green). The MIEKF maintained a low estimation error throughout the simulation. We see a similar pattern for the case of $20\%$ outlier levels in Figure 11 and Figure 12. The estimation errors for all filters were correspondingly higher with a higher level of outliers. However, the MIEKF performance degraded only marginally, while the performance degradation in other filters was noticeable. Overall, the MIEKF still maintained the lowest estimation error among all the filters considered in this research.

An outlier level of $40\%$ is considered a high level of contamination. The trend we observed in the previous two cases persisted in that case as well. This is shown in Figure 13 and Figure 14. The MIEKF provided the lowest estimation errors among the alternatives. The filter consistency plots in Figure 15, Figure 16 and Figure 17 show that only the MIEKF-based estimates give a good agreement with the theoretically predicted performance. The estimates based on alternative approaches tend to show divergence within outlier-influenced regions and in some cases, they show hang-off errors.

Plots for the relationship between altitude and atmospheric density, altitude and velocity, as well as flight path angle and velocity are provided in Figure 18. This gives yet another perspective on the enhanced performance of MIEKF. In passing, we note that if the conventional Gaussian assumption for measurement noise (i.e., no outliers) is assumed throughout the simulation duration, the UKF performs the best on a majority of the state estimates.

As a final demonstration of the effectiveness of the proposed method, we provide numerical RMSE values to show what level of performance we can expect from the proposed multisensor navigation scheme. This is shown in

Table 3. Filter performance comparison based on RMSE values.

	$\mathit{ϵ}$ = 0.05 (5% Outlier Level)			$\mathit{ϵ}$ = 0.20 (20% Outlier Level)			$\mathit{ϵ}$ = 0.40 (40% Outlier Level)
State/Param.	EKF	UKF	MIEKF	EKF	UKF	MIEKF	EKF	UKF	MIEKF
Range, r (m)	555.4604	443.6054	$\mathbf{58}.\mathbf{5634}$	1023.6361	616.3866	$\mathbf{96}.\mathbf{6578}$	1883.3481	812.8448	$\mathbf{141}.\mathbf{4939}$
Vel., v ($\frac{\mathrm{m}}{\mathrm{s}}$)	0.9395	0.1408	$\mathbf{0}.\mathbf{0309}$	1.1502	0.2433	$\mathbf{0}.\mathbf{0796}$	3.5284	0.3599	$\mathbf{0}.\mathbf{1619}$
FPA, $\gamma$ (deg)	0.6831	0.2771	$\mathbf{0}.\mathbf{0810}$	1.1463	0.5762	$\mathbf{0}.\mathbf{0834}$	1.9735	0.7493	$\mathbf{0}.\mathbf{1030}$
Long.,$\theta$ (deg)	0.0043	$\mathbf{0}.\mathbf{0013}$	0.0018	0.0068	$\mathbf{0}.\mathbf{0011}$	0.0018	0.01052	$\mathbf{0}.\mathbf{00167}$	0.00169
Lat. $\varphi$ (deg)	0.0110	0.0017	$\mathbf{0}.\mathbf{0009}$	0.0162	0.0033	$\mathbf{0}.\mathbf{0010}$	0.0206	0.0055	$\mathbf{0}.\mathbf{0011}$
Heading., $\psi$ (deg)	0.1947	$\mathbf{0}.\mathbf{1705}$	0.1814	0.2795	0.2908	$\mathbf{0}.\mathbf{1797}$	0.4493	0.3357	$\mathbf{0}.\mathbf{1820}$
Inv. Ball. Coeff., B ($\frac{{\mathrm{m}}^2}{\mathrm{kg}}$)	0.000573	$\mathbf{0}.\mathbf{000532}$	0.000562	0.000580	$\mathbf{0}.\mathbf{000549}$	0.000563	0.000589	0.000578	$\mathbf{0}.\mathbf{000565}$
Ref. atm. density, ${\rho }_0$ ($\frac{\mathrm{kg}}{{\mathrm{m}}^3}$)	0.000016	0.0000011	$\mathbf{0}.\mathbf{000002}$	0.000026	0.000018	$\mathbf{0}.\mathbf{000003}$	0.000042	0.000021	$\mathbf{0}.\mathbf{000004}$
Lift-to-drag ratio, $\frac{L}{D}$	0.008955	0.040039	$\mathbf{0}.\mathbf{008069}$	0.065336	0.113399	$\mathbf{0}.\mathbf{015262}$	0.143800	0.130166	$\mathbf{0}.\mathbf{039634}$

. MIEKF consistently performs better than the alternative approaches. For most states, the margin of improvement is substantial.

7. Conclusions

A statistically robust sensor fusion framework for autonomous entry phase navigation for Mars landing was presented. The method added positioning accuracy and parameter observability by virtue of the multisensor approach and added distributional robustness to non-Gaussian noise due to the M-estimation-based filter employed to fuse the data from the sensors. The method had a similar computational complexity to existing tried and proven filtering methods. The present design can account for sensor faults or external sources of spurious data that lead to outliers. In space, these events can be frequent. Future Mars pinpoint landing missions require that landers touch down within an error ellipse of at most 50 m around the chosen landing site. The main challenge in meeting this requirement has been the limitation of inertial-only navigation during entry. By employing a multimodal approach in a robust sensor fusion architecture, the research in this paper, after additional practical considerations, has the potential to enable a highly accurate entry phase navigation. Combining the advantages of the iterated extended Kalman filter (IEKF) with the robustness properties of the M-estimation, the proposed M-estimation-based IEKF (MIEKF) performed better than alternative approaches. We substantiated our claim with experiments showing the estimation error performance for each candidate method. There are several additional problems that require attention in future work. With eventual onboard implementation in mind, using more stable versions of the covariance propagation using factorization methods is required to avoid potential numerical problems. Accounting for uncertainties in radio beacon location is an important consideration that warrants attention. The assumption of a fairly accurate knowledge of entry conditions is a serious limitation of the present work. The use of X-ray pulsar-based navigation during the approach phase of Mars Entry, Descent, and Landing (EDL) to improve knowledge of entry conditions is a promising technique. A more rigorous derivation of a different version of the MIEKF directly from a maximum likelihood formulation is being developed by the authors for future work. This will avoid some ad hoc assumptions presented in this paper that may reduce estimation efficiency. Applying the distributionally robust sensor fusion method discussed in this work to other phases of EDL, where more navigation sensors such as cameras and radar are available, is yet another interesting avenue to consider.