- Email: [email protected]
Robust three-stage unscented Kalman filter for Mars entry phase navigation
Accepted Manuscript
Robust three-stage unscented Kalman filter for Mars entry phase navigation Yongbo Zhang , Mengli Xiao , Zhihua Wang , Huimin Fu , Yunzhang Wu PII: DOI: Reference:
S1566-2535(18)30098-8 https://doi.org/10.1016/j.inffus.2018.11.003 INFFUS 1041
To appear in:
Information Fusion
Received date: Revised date: Accepted date:
5 February 2018 29 October 2018 6 November 2018
Please cite this article as: Yongbo Zhang , Mengli Xiao , Zhihua Wang , Huimin Fu , Yunzhang Wu , Robust three-stage unscented Kalman filter for Mars entry phase navigation, Information Fusion (2018), doi: https://doi.org/10.1016/j.inffus.2018.11.003
This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.
ACCEPTED MANUSCRIPT
Highlights
Precision Mars entry navigation algorithm under uncertainties and unknown inputs Uncertainties in atmosphere density, ballistic coefficient and lift-to-drag ratio
RThUKF has shown great advantages in state estimation of nonlinear system
Simulation resulting in precision navigation fulfills future pinpoint landing.
AC
CE
PT
ED
M
AN US
CR IP T
1
ACCEPTED MANUSCRIPT
Robust three-stage unscented Kalman filter for Mars entry phase navigation Yongbo Zhang1*, Mengli Xiao1, Zhihua Wang, Huimin Fu1,Yunzhang Wu2 1
Research Center of Small Sample Technology, Beihang University, Beijing, China 100191, 2
Army Aviation Research Institute, Beijing, China 101123
CR IP T
E-mail: [email protected]
Abstract A high-precision Mars entry navigation algorithm under parameter uncertainties and unknown inputs is developed via a robust three-stage unscented Kalman filter. The uncertainties lie in the atmosphere density, ballistic coefficient and
AN US
lift-to-drag ratio are the most important error source of state estimation, which have been investigated in this paper. As the robust three-stage unscented Kalman filter has great advantages in parameter estimation and state estimation for nonlinear system with unknown input, it is proposed to compensate the effect of those parameter
M
uncertainties and unknown inputs and obtain more accurate estimation. In the numerical simulation of Mars entry navigation, this filter algorithm has shown its
ED
advantages by resulting a high precision navigation with position error less than 100m and velocity error less than 2m/s, which fulfills the requirement of future pinpoint
PT
Mars landing missions.
Key words: robust three-stage unscented Kalman filter, nonlinear system, parameter
CE
uncertainties, unknown input, Mars entry navigation
AC
1. Introduction The landing accuracy of Mars vehicle has been improved gradually. Generation 1
Mars landing systems aimed at landing safely without having the freedom to choose the landing site, the landing accuracy of which was more than 35km to the target. By introducing the hypersonic guidance and with advanced navigation capability, Generation 2 delivered the vehicle to within 5-10km of the target
[1, 2]
. But the
pinpoint landing accuracy of future Mars exploration missions was required to be within 100m of a pre-selected target site. To achieve pinpoint landing, high-precision 2
ACCEPTED MANUSCRIPT
navigation, guidance, and control techniques are necessary for Mars entry, descent and landing (EDL) phases [3-6]. According to the existing investigation [1, 7, 8], there are three most significantly important sources leading to Mars landing dispersion during EDL: the first one is position and velocity errors at the Mars entry point, the second one is uncertainties in the Mars atmosphere density and the vehicle aerodynamic characteristics during Mars entry phase, and the last one is drifts caused by strong
CR IP T
winds during the parachute descent phase.
For the initial errors and the uncertainties in Mars entry phase, several algorithms have been developed. Levesque et al. [7] obtained the parameter estimation by treating the uncertain parameters as the augmented state. Ely et al. [9] estimated the Mars [10, 11]
AN US
atmosphere density using Hierarchical mixture of experts. Heyne et al.
developed an adaptive sigma point Kalman filter bank to reduce the navigation errors caused by the atmosphere density uncertainty. In the investigations mentioned above, only the data of IMU outputs was used as the measurements, while it has been proven
Bishop
[12]
M
that IMU-based navigation cannot make all the state variables convergent. Zanetti and investigated the application of a multiple model adaptive estimation
ED
(MMAE) comprising an extended Kalman filter (EKF) bank to address the problem of the state estimation for Mars entry navigation under uncertain atmosphere density. In
PT
MMAE, a bank of filters running in parallel, and the state estimate was obtained by combining the estimates of all parallel filters. This algorithm was efficient in the state
CE
estimation. An extension to the MMAE for Mars entry navigation was proposed by Marschke et al.
[13]
to analyze the effect of the biases and scale factors of IMU on Mars
AC
navigation, in which the adverse effect of the biases and scale factors cannot be eliminated completely. Nevertheless, there was excessive competition between the various sub-models in the MMAE, thus large estimation errors were produced if one model was closer to the actual model than other ones. Li et al [14] proposed a modified MMAE methodology with exponential decay terms to reduce the excessive competition. But Li used EKF for the state estimation, while for complex nonlinear system, using first-order Taylor series expansion may produce truncation errors that lead to the filter divergence
[15, 16]
. Also, it may be difficult to compute the partial 3
ACCEPTED MANUSCRIPT
derivatives. To solve the problems, Xiao et al.
[17]
proposed an integrated navigation
based on multiple model adaptive rank estimation (MMARE) for Mars entry phase. This novel algorithm adopted several Rank filters running in parallel instead of EKFs. MMARE obtain a more superior performance when compared with the modified MMAE. But the numerical underflows may happen as the multiple model adaptive estimation was designed based on the assumption that the actual value of the
CR IP T
uncertain parameters were contained with the assumed values. But neither Li nor Xiao considered the unexpected unknown inputs for radiometric measurements that caused by the ionization of atmosphere. Wu et al
[18]
proposed an extension of robust
three-stage Kalman filter (ERThSKF) to investigate the parameter uncertainties and
AN US
unknown inputs for high-precision entry navigation, but it may encounter implementation problem when the complex nonlinear system cannot be transformed into linear problems or the model partial derivatives are difficult or impossible to compute. Yu et al
[19]
proposed a robust Kalman filter for Mars entry under the
M
atmosphere density uncertainty and unknown inputs. In this filter, a performance index was established by considering the trace of posterior variance matrix and the
ED
transition matrices of atmosphere density uncertainty and unknown inputs. And then the gain matrix was determined by minimizing the performance index. The method
PT
was proven to be more accurate and robustness to noise. But the filter was also an improvement of EKF.
CE
Augmented state Kalman filter (ASKF) was a basic approach to address the problem of state estimation for stochastic discrete-time system with uncertain
AC
parameters and unknown inputs, of which the uncertain parameter and the unknown input vector were augmented as the state vector
[7]
. However, the computational
complexity and memory cost increased with the augmented state dimension, and it becomes serious when the dimensions of the uncertain parameter vector and the unknown input vector were comparable to that of the state vector
[20]
. To improve its
efficiency, Hmida et al. [21] developed an optimal three-stage Kalman filter (OThSKF) for linear discrete-time stochastic system by decoupling the covariance matrices of ASKF via a three-stage U-V transformation, which was proven to be optimal for the 4
ACCEPTED MANUSCRIPT
dynamical evolution models of the uncertain parameters and the unknown inputs were known. Considering the dynamic models were not perfectly known or unknown, a robust ThSKF (RThSKF)
[21]
and an augmented RThSKF
[22]
were developed by
modifying the measurement update equations of the OThSKF. Both RThSKF and ARThSKF can provide unbiased minimum-variance parameter estimation and state estimation for the linear system with unknown inputs. For nonlinear system, Wu et.al [18]
. But as mentioned above,
CR IP T
extended RThSKF (ERThSKF) for state estimation
ERThSKF was an EKF-like nonlinear filter which may encounter difficulties in actual application. Based on the unscented Kalman filter (UKF)
[15, 16]
, a robust three-stage
unscented Kalman filter (RThSUKF) which avoids the calculation of model partial
AN US
derivates is proposed in our investigation [23].
This paper investigates the application of RThSUKF for Mars entry navigation. The uncertain parameters including atmosphere density, ballistic coefficient and lift-to-drag ratio and unknown inputs that may lead to performance degeneration of
M
the traditional Kalman filters are considered. With the adverse effect of all these uncertainties been compensated, RThSUKF could obtain a high-precision estimate for
ED
Mars entry navigation. The paper is organized as follows. Section 2 demonstrates the traditional simplified 3-DOF Mars entry dynamic model and measurement equations.
PT
Section 3 introduces the robust three-stage unscented Kalman filter. Section 4 gives the numerical simulation and the performance comparison between the standard
CE
unscented Kalman filter and RThSUKF. Finally, in Section 5, there is a conclusion of our research.
AC
2. Mars entry dynamic model and measurement equations 2.1. Mars entry dynamic model The Mars entry phase begins at the Mars atmosphere interface (defined as a radius of 3552km from the center of Mars, and an altitude of 125km over the surface) and ends at the first parachute deployment. The vehicle is assumed to fly in a stationary atmosphere of non-rotating spherical planet, thus winds and the centripetal and Coriolis effects due to the Mars rotation are neglected. The accuracy of Mars 5
ACCEPTED MANUSCRIPT
entry navigation is heavily depended on the dynamical model, which is used for predicting the state of vehicle. A widely used simplified 3-DOF dynamic model of the Mars entry defined with respect to the Mars-centered, Mars-fixed coordinate system [7] is given by x f x
(1)
where x r v , r denotes the distance from the center of Mars to the center of mass of the entry vehicle,
CR IP T
T
is the longitude, is the latitude, v is
the velocity of the entry vehicle, is the flight-path-angle (FPA) and is the azimuth angle.
(2)
ED
M
AN US
v sin v cos sin r cos v cos cos f x r D g (r ) sin v g (r ) cos L cos r v v v sin cos tan L sin v cos r
PT
where is the bank angle defined as the angle about the velocity vector from the local vertical plane to the lift vector, which is also considered as the only control
CE
variable. g (r ) is the Mars gravitational acceleration, and an Newton’s inverse square acceleration model giving a roughly accuracy of 99% [7] is given by
AC
g (r ) r 2
(3)
where =42828.29 109 m3 s2 is the Mars gravitational constant. D and L are the aerodynamic drag and lift accelerations, which are defined by D
1 2 v B 2
L L DD
B
CD S m
(4)
where B is called the ballistic coefficient, L D is the lift-drag ratio, C D is the aerodynamic drag coefficient, S is the reference surface area, m is the mass of the 6
ACCEPTED MANUSCRIPT
entry vehicle. As the importance of aerodynamic characteristics in the dynamic model, the uncertainties are one of the most important error sources leading to an inaccurate dynamical state estimate. is the Mars atmospheric density and an exponential Mars atmospheric density
model is assumed as follows =0 exp(r0 r ) hs
CR IP T
(5)
where 0 is the density on the surface of the Mars, r0 3437.2km is the reference radial radius of Mars (40km above the Mars), and hs =7.5km is the constant atmospheric scale height. Since the true Mars atmospheric density varies with the seasons and
AN US
temperature, an accurate Mars atmospheric density is still difficult to be achieved, and the model (5) is only an approximation. Therefore, the deviation of the atmospheric density is considered in our research.
2.2. Measurement equations
M
During the Mars entry phase, a heat shield was wrapped outside protecting the vehicle from the aerodynamic heating. Therefore, among the conventional navigation [12, 13]
ED
sensors, IMU is the only available sensor for Mars entry navigation
. Recently,
the ultra high-frequency (UHF) communication between the vehicle and the orbiter or [24, 25]
PT
the surface beacons was proven to be slightly affected by the ionizing plasma
,
providing an effective way for Mars entry phase navigation. In our investigation, the
CE
IMU/radio beacons integrated navigation is adopted as the navigation scheme.
AC
Accelerometer measurements The accelerometer of IMU is used for measuring the non-gravitational linear
acceleration along three orthogonal axes. The accelerometer measurement in the body-fixed coordinate system can be given by an accelerometer as follows ab ab ba ξa
where ab
is
the
true
acceleration
7
along
(6) the
body-fixed
coordinate
ACCEPTED MANUSCRIPT
T
system, ba bx by bz is the unknown acceleration bias, ξ a is the accelerator stochastic noise modeled as zero-mean, Gaussian distribution white noise. The acceleration along the body-fixed coordinate system ab can be given by ab Tvb a v
(7)
a v D, D L D sin , D L D cos
T
CR IP T
(8)
where a v is the acceleration vector in the velocity coordinate system, Tvb is the coordinate transition matrix for the velocity coordinate system to the body-fixed coordinate system.
AN US
As the dynamical model defined with respect to the Mars-centered, Mars-fixed coordinate system, measurement vectors are needed to transfer from the velocity coordinate system to the Mars-centered, Mars-fixed coordinate system. a m TpmTvp a v
(9)
M
where a m is the output vector of accelerator in the Mars-centered, Mars-fixed coordinate system, Tpm is the coordinate transition matrix for the system of
ED
navigation coordinate to the Mars-centered, Mars-fixed coordinate system, and Tvp is
PT
the coordinate transition matrix of the velocity coordinate system to navigation coordinate system, which are given by
CE
cos cos Tpm cos sin sin
sin cos 0
sin cos sin sin cos
cos Tvp sin cos sin sin
sin cos cos cos sin
0 sin cos
(10)
AC
Radiometric range measurements As illustrated above, the vehicle could communicate with orbiters and surface
beacons via UHF The radiometric measurements between the vehicle and the radio beacons are obtained via radiometric transceivers. By analyzing the ranging shifted signals, the radial range between the vehicle and an orbiter or surface beacon can be modeled as follows R R bR ξ R 8
(11)
ACCEPTED MANUSCRIPT
rl rb rl rb -
R
T
(12)
r cos cos r r cos sin r sin
(13)
where R is the radial range between the entry vehicle and the orbiters or the surface
CR IP T
beacons, bR is the range measurement bias, ξ R is zero-mean, Gaussian distribution white noise, rl is the position vector of the entry vehicle, rb is the position vector of an orbiter or a surface beacon. Both the position vectors mentioned above are defined in the Mars centered, Mars-fixed coordinate system. Doppler measurement
can be modeled as follows
AN US
According to Doppler shifted signals, the change rate of ranging measurement
V V ξV
rl rb vl vb
(14)
T
M
V
R
(15)
ED
where V is the change rate of radial distance, ξV is zero-mean, white noise. v l denotes the velocity vector of the entry vehicle, vb denotes the velocity vector of the
PT
orbiter or the surface beacon. The vectors mentioned above are defined in the Mars centered, Mars-fixed coordinate system.
CE
Since the accelerometer of IMU is used to measure the true aerodynamic
acceleration of the vehicle in Mars entry, it should be noted that the atmosphere
AC
density and aerodynamics coefficients involved in Eq. (8) are true values. Thus the uncertain parameters are also included in the measurement equations. Besides, as the ionization of the atmosphere occurs, it will result in unacceptable unknown inputs of the radiometric measurements. As uncertain parameters and unknown inputs may degrade the performance of the navigation filter, thus how to compensate the effect of the parameter uncertainties and unknown inputs should be considered in the navigation filter design. 9
ACCEPTED MANUSCRIPT
2.3. Observability analysis The observability of the autonomous navigation is an important index representing the state estimation capability of vehicle via measurements in finite time, thus always been emphasized in the navigation system design. In this study, for convenient, the system is firstly linearized by the first-order approximation as follows f f0 F x x0
CR IP T
h h0 H x x0
(16) (17)
where F and H are the first order linearization matrices of the state and measurement equations, respectively. Then the observability matrix of the system [26] is given by T , H n 1F
T
AN US
T T OM H T , HF , H 2 F ,
(18)
A condition number of the observability matrix is used as the performance index to evaluate the observability of the system, which reflects the singularity of the observability matrix. It may be closer to singular when there is a larger condition
M
number [27]. Accordingly, the condition number is defined as max (OM ) min (OM )
(19)
ED
cond (OM )
where max and min are the maximum and minimum singular number of the
PT
observability matrix. The degree of the observability for the system is defined by the
CE
inverse of the condition number of the observability matrix, which is given by
(O ) 1 min M cond (OM ) max (OM )
(20)
The degree of observability holds for the condition of 0 1 . If 0 , it means
AC
that the rank of the observability matrix is less than the state dimension, then the system is locally unobservable. Otherwise, the system is locally weakly observable, and the degree is larger, the system is more observable [27].
3. Robust three-stage unscented Kalman filter According to the Mars entry dynamical model and measurement equations given above, the discrete-time nonlinear system with uncertain parameters and unknown input is given by 10
ACCEPTED MANUSCRIPT
xk 1 f k xk , uk , bk , d k wkx
(21)
yk hk xk , uk , bk , dk vk
(22)
where f () and h() are nonlinear state transformation function and nonlinear measurement function, respectively. xk 1 n is the state vector, yk m is the
CR IP T
observation vector, bk p is the uncertain parameter vector, and dk q is unknown inputs. All matrices have the appropriate dimensions. The noise sequences wkx and v k are zero-mean uncorrelated Gaussian random sequences with w x w x T Q x E k j k vk v j 0
0 kj Rk
AN US
(23)
where Qkx 0, Qkb 0, Qkd 0, Rk 0 and kj is the Kronecker delta, kj 1 if k j , else kj 0 if k j . The initial state x0 , fault b0 and unknown inputs d 0 are assumed
M
to be uncorrelated with the zero-mean white noise sequences wkx , wkb , wkd and v k . Assume that x0 satisfies the following equations xˆ 0 E( x0 ), P0x E ( x0 xˆ 0 )( x0 xˆ 0 )T
ED
(24)
PT
The proposed method is the work from [23], which was designed for dealing with the uncertain parameters and the unknown inputs. Firstly, the matrices of
CE
uncertain parameter and unknown input are identified by the following numerical approximation B B
AC
x k
E E x k
Bky Bky ,1
E E y k
y ,1 k
x ,1 k
x, p k
, B
x,q k
, E
B
x ,1 k
E
x ,i k
x ,i k
Bky , p , Bky ,i
E
y,q k
, E
y ,i k
f k xˆ k k , uk , bˆk k i eip , d k k f k xˆ k k , uk , bˆk k , d k k
(25)
i
f k xˆ k k , uk , bˆk k , d k k i eiq f k xˆ k k , uk , bˆk k , d k k
(26)
i
hk xˆ k k 1 , uk , bˆk k 1 i eip , d k k 1 hk xˆ k k 1 , uk , bˆk k 1 , d k k 1 i
hk xˆ k k 1 , uk , bˆk k 1 , d k k 1 i eiq hk xˆ k k 1 , uk , bˆk k 1 , dk k 1 i 11
(27)
(28)
ACCEPTED MANUSCRIPT
where
e ip and eqi are the i th column of the identity matrices with dimensions p
and q , respectively. i is an appropriate selected small value. Then the nonlinear discrete-time varying system given by (21) and (22) can be approximated as
xk 1 f k* xk , uk , bˆk k , dk k Bkx bk Ekx dk wkx
(29)
yk hk* xk , uk , bˆk k 1 , dk k 1 Bky bk Eky dk vk f k* xk , uk , bk k f k xk , uk , bˆk k , dk k Bkx bˆk k Ekx dk k
where
CR IP T
(30)
hk* xk , uk , bˆk k 1 , dk k 1 hk xk , uk , bˆk k 1 , dk k 1 Bky bˆk k 1 Eky dk k 1
Step 1: Initialization.
.
AN US
At the first stage, an initial V023 should be chosen.
xˆ 0 0 xˆ 0 , Pˆ0x0 P0x ,V023
Step 2: State sub-filter
and
(31)
This sub-filter is designed based on the assumption that the uncertain parameters
M
and the unknown inputs are identically zero. Thus, update the state estimate xk k and its
ED
error covariance matrix Pkxk by unscented Kalman filter as follows χk 1,i xˆ k 1 k 1
xˆ k 1 k 1 (n+ )Pˆkx1 k 1
xˆ k 1 k 1 (n+ )Pˆkx1 k 1
AC
CE
PT
χk*/ k 1,i f k*1 χk 1,i , uk 1 , bˆk 1/ k 1 , dk 1/ k 1 2 nx
(32) (33)
xk k 1 i χ k*/ k 1,i m
(34)
i 0
2n
c Pkx k 1 i χ k* k 1,i xk k 1 χ k* k 1,i xk k 1 i 0
χ k k 1,i xk k 1
T
Q
xk k 1 (n+ )Pkx k 1
xk k 1 (n+ )Pkx k 1
yk / k 1,i hk* χk k 1,i , uk 1 , bˆk 1/ k 1 , dk 1/ k 1 2 nx
yk / k 1 i yk / k 1,i i 0
12
x k
m
(35) (36) (37) (38)
ACCEPTED MANUSCRIPT
2n
c Pxy i χ k k 1,i xk k 1 yk k 1,i yk / k 1 i 0
2n
T
c Pyy i yk k 1,i yk / k 1 yk k 1,i yk / k 1 i 0
K kx Pxy Pyy
T
1
(39)
R
k
(40) (41) (42)
CR IP T
xk k xk k 1 K kx yk yk / k 1
Pkxk Pkxk 1 K kx Pyy K kx
T
(43)
where n is dimension of state vector, 2 (n ) n is a scaling parameter, is a constant determines the spread of the sigma points around mean value, and usually set
AN US
to a small positive value. The constant is a secondary scaling parameter, which is usually set to 0 or 3 n , and is used to incorporate prior knowledge of the distribution of mean value, with the weights i
given by [16]
M
0 m n , 0c n (1 2 ), i m ic 2 n i 1,2, 2n (38)
Step 3: Uncertain parameter sub-filter
ED
This sub-filter is designed based on the assumption that the unknown inputs are
PT
identically zero, giving the unknown input-free parameter estimate bk / k and its error
AC
CE
covariance matrix Pkb/ k by
Uk12 Bkx1
(44)
Sk2 Hk Uk12 Bky
(45)
Pkb/ k Sk2T Pyy1 Sk2
(46)
K kb Pkb/ k Sk2T Pyy1
(47)
bk / k K kb yk yk / k 1
(48)
Step 4: Unknown input sub-filter The filter gives the input estimate dk / k and its error covariance matrix Pkd/ k by Uk23 Vk231 13
(49)
ACCEPTED MANUSCRIPT
Uk13 Ekx1 Uk12Uk23
(50)
Sk3 Hk Uk13 BkyUk23 Eky Pkd/ k S k3T Pyy1 S k3
(51)
(52)
K kd Pkd/ k Sk3T Pyy1αk I Sk2 Sk2
d k / k K kd yk yk / k 1
Step 5: Correction of the state and the fault estimations
(54)
CR IP T
where αk is an arbitrary matrix.
(53)
The optimal state and fault estimates xˆ k k , bˆk k as well as their error covariance
AN US
matrices Pˆkx/ k , Pˆkb/ k are obtained from the estimates of three sub-filters and the coupling
Vk12 Uk12 K kx Sk2
(55)
Vk23 Uk23 K kd Sk3
(56)
M
equations Vk12 ,Vk13 ,Vk23 as shown
CE
PT
ED
Vk13 Uk13 Vk12 Kkd Sk3 Kkx Sk3
(57)
xˆ k k xk k Vk12 bk k Vk13d k k
(58)
bˆk k bk k Vk23dk k
(59)
Pˆkx/ k Pkx/ k Vk12 Pkb/ kVk12T Vk13 Pkd/ kVk13T Pˆkb/ k Pkb/ k Vk23 Pkd/ kVk23T
(60) (61)
AC
RThSUKF is a robust filter designed for predicting the state and parameter of
nonlinear system in presence of unknown input. For the Mars entry navigation problem in this study, if the statistic properties of uncertain parameters and unknown inputs are perfectly known, it can be addressed by augmented state unscented Kalman filter (ASUKF) via treating the uncertain parameters and unknown inputs as the augmented state. However, the computational and memory resource of vehicle is limited, and the costs are increasing with the dimension of the augmented state. What is worse, ASUKF loses its optimality when part of uncertain parameter and unknown 14
ACCEPTED MANUSCRIPT
inputs models are unknown. But RThSUKF could solve this problem by decoupling the covariance matrices of ASUKF via a three-stage U-V transformation and modifying the measurement update equations of uncertain parameter sub-filter and unknown input sub-filter. Therefore, RThSUKF could obtain an optimal estimate in spite of the statistical information of the uncertain parameters and the unknown inputs
4. Numerical simulation and analysis
CR IP T
are partially known or unknown during the Mars entry navigation.
In this section, numerical simulation of Mars entry navigation is carried out to illustrate the performance of the proposed algorithm. The planned entry span is assumed to be 400s， which is equal to the period of the vehicle from the atmospheric
AN US
surface 125km to 10km, and the simulation sample step is set to 1s. The initial values of state variables are listed in Table 1. Also, we adopted 3 orbiting beacons in the course of entry. The initial positions and velocities of the radio beacons are described in Table 2[28]. As the actual values of the atmosphere density and aerodynamics
M
coefficients are still difficult to be obtained. The uncertainties lied in have been investigated by loads of researchers. Based on the previous measurement, several
ED
assumptions have been made. In the experiment of Prince et al.
[4]
, sets of 2000
atmosphere profiles have been developed specifically for the Phoenix landing sites
PT
and seasons incorporating day-to-day variations in temperature and pressure. Theses profiles include regional density, temperature, and pressure characteristics with
CE
respect to altitude. Fig.1 shows the percentage difference of the 2000 individual profiles. Accordingly, Li et al. [5,6], Chen et al. [6,7], Schoenenberger et al. [8], Chen et al. , believed that there is a maximum of 15% deviations in the nominal values from
AC
[9]
exponential Mars atmospheric density model when compared with the true Mars atmosphere density, and the uncertainties roughly follows a normal distribution around the mean values. In our investigation, two cases are compared in the simulation, of which the uncertainties are assumed to follow Normal and Uniform distribution, respectively. And according to the research of [19], the unknown inputs of the radiometric ranging measurement are assumed to be containing a position bias 15
ACCEPTED MANUSCRIPT following Normal distribution of N 10,1002
[19]
.
The simulation results are
compared with conventional unscented Kalman filter (UKF), and the root mean square error (RMSE) is used as measure. The scaling parameters for the proposed filter and UKF are chosen to 0.6, 2, 3 n . Table1 Initial state variables and physical parameters of entry vehicle [7]
CR IP T
***** Table2 Initial position and velocity of radio beacons [28] *****
The observability of the IMU/radio beacons integrated navigation is firstly
AN US
analyzed before the simulation. The degree of obsevability of the integrated navigation is shown in Fig.1. In this figure, the degree of observability increases fast during the first 50 seconds, and reaches the peak of 2.2 108 . But it rapidly reduces to a relatively lower value of 1 109 , and then fluctuates at the mean value of 7.5 109 . In all, it reaches the minimum value 7 1011 at the beginning which is larger
M
than 1 1016 , indicating that the navigation system is observable during the whole
ED
entry phase, though the uncertain parameters and measurement errors are needed to be estimated simultaneously.
PT
*****
Fig.1 Degree of observability for IMU/ integrated navigation
CE
Case 1:
According to the analysis above, the deviation of reference atmosphere
AC
density 0 is assumed to follow a Normal distribution of N 0, 0.152 in the first case. As the reference atmosphere density 0 and ballistic coefficient B are both the parameters of D and L as shown in Eq. (4), we consider the product of 0 B and lift-to-drag ratio L / D as the uncertain parameter vector. The deviations of ballistic coefficient B and lift-to-drag ratio L D are assumed to follow uniform distribution of U 0.1, 0.1 . 16
ACCEPTED MANUSCRIPT
The actual and estimated values of uncertain parameters 0 B and L / D are plotted in subfigures (a) and (b) of Fig.2. As shown, at the first 30 seconds, the RThSUKF are adjusting the initial conditions with a relatively larger deviation. But, after that it rapidly adapts these changes of the parameters, and obtains accurate estimates quickly. And then, it keeps that good performance to the end of the whole
CR IP T
simulation process. In another part, the unknown inputs need to be estimated, and Fig.3 depicts the comparison of these estimates with the actual values. Also, it can be observed that there is still an adaptive adjustment of this algorithm at the first 30 seconds, and after this adjustment, the input estimates are close to the actual values. *****
AN US
Fig.2 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D *****
Fig.3 Actual and estimated values of the unknown inputs
The state estimation was the most important part of this simulation. Fig.4 depicts
M
the estimation of position, longitude, latitude, velocity, FPA, and azimuth in
ED
subfigures (a) to (e) respectively. As there are small differences between the actual values and each state estimate, the RThSUKF exhibits advantages in the Mars entry
PT
navigation with good performances. For further analysis, the comparison between the conventional widely used UKF and RThSUKF are conducted as shown in Fig.5, of
CE
which the subfigures (a)-(e) exhibits the RMSE of estimations in position, longitude, latitude, velocity, FPA, and azimuth under the same condition, respectively. As shown,
AC
UKF represents a relative more accurate state estimation at the initial filter phase of the first 40 seconds. But, after that the state estimation errors increase sharply in a short time and fluctuate at a high value till the end of simulation. For some of the most important state, position (a) and velocity (d), it could reach a peak of 500m and 10m/s, respectively, which is unacceptable. This is fundamentally caused by the uncertain parameters and the unknown inputs, and when they accumulate larger and larger, it becomes more difficult for the conventional UKF to compensate, thus results in the large estimation errors. Therefore, in the simulation of Mars entry navigation, 17
ACCEPTED MANUSCRIPT
UKF is difficult in addressing the system in presence of the uncertain parameters and unknown inputs. However, RThSUKF has the contrary simulation results to the performance of UKF. It has relative larger state estimation errors during the first 40 seconds as there are parameter and input estimation adjustment as shown in Figs.2 and Fig.3. Then the state estimation errors reduced quickly to a very low level with a little fluctuation till
values. *****
CR IP T
the end of simulation, thus the final state estimation are very close to the actual
Fig.4 Actual and estimated values of the state variables of the vehicle
AN US
***** Fig.5 Performance comparison of UKF and RThSUKF with respect to RMSE
Case 2:
In the second case, the deviations of reference atmosphere density are assumed
M
to be subjected to Normal distribution of N 0, 0.152 , and the ballistic coefficient as
ED
well as lift-to-drag ratio also follows the normal distribution of N 0, 0.12 . The actual and estimated values of the uncertain parameters, unknown inputs and state variables
PT
are compared in Figs.6, 7, and 8, respectively. As shown, RThSUKF performs as well as case 1, exhibiting accurate estimations after a period of time adjustment. Fig.9
CE
shows the performance comparison of UKF and RThSUKF with respect to RMSE in state variables. UKF is still very sensitive to the drastic changes of the parameter
AC
uncertainties and the unknown inputs, and the estimation errors fluctuate at a high value. In all, RThSUKF could obtain a superior estimation in presence of uncertainties and unknown inputs whether what the distributions of these uncertainties and unknown inputs subject to. ***** Fig.6 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D ***** 18
ACCEPTED MANUSCRIPT
Fig.7 Actual and estimated values of the unknown inputs ***** Fig.8 Actual and estimated values of the state variables of vehicle ***** Fig.9 Performance comparison of UKF and RThSUKF with respect to RMSE in state variables
Table 3 illustrates the average RMSE of the two algorithms for the two cases,
CR IP T
which is defined over all the time instants in the state variables. As shown, The RMSE of RThSUKF is as much as one order in magnitude less than that of UKF in all the state variables. Especially for the position estimation, the errors of RThSUKF are less than 100m in both cases, but that of UKF are up to 526.2147m and 591.6754m,
AN US
respectively. For comparison, the differences of velocity are not so large between these two filter algorithms. It may be the reason that the Doppler measurements have provided adequate and sufficiently accurate velocity information for the Mars entry vehicle in this simulation. Also, with the comparison of these two cases, it has shown
M
that the average RMSE of UKF becomes larger as the parameter’s distribution varies from case1 to case 2. But RThSUKF keeps an accurate stable state estimation.
ED
Therefore, UKF is really sensitive to the parameter uncertainties and unknown inputs. It furtherly illustrates the advantages of RThSUKF in dealing with the state estimation
PT
of system with uncertain parameters and unknown inputs.
CE
Table3 Performance comparison of UKF and RThSUKF with respect to average RMSE *****
According to the simulation results analysis above, it has shown that RThSUKF
AC
performs well in the state estimation of Mars entry autonomous navigation when the nonlinear system is in presence of uncertain parameters and unknown inputs. The filter can obtain the parameter estimation precisely and compensate the effect of parameter uncertainties and unknown inputs properly. Moreover, it can achieve superior performance with high precision state estimation, which fulfills the Mars entry navigation accuracy requirement of the future Mars pinpoint landing.
19
ACCEPTED MANUSCRIPT
5. Conclusion A robust three-stage unscented Kalman filter (RThSUKF) is introduced to address the problem of sate estimation under the large parameter uncertainties and unknown inputs during Mars entry phase. RThSUKF is a robust filter and performances well at gaining unbiased state and parameter estimation for the
CR IP T
nonlinear system in presence of unknown inputs. With considering the large uncertainties lie in the atmosphere density, ballistic coefficient and lift-to-drag ratio as well as the unknown inputs, numerical simulation of Mars entry based on IMU/radios beacons integrated navigation is conducted. The simulation results show that RThSUKF could compensate the effect of parameter uncertainties and unknown input
AN US
properly. It has represented a superior performance with high precision navigation accuracy when compared with the conventional standard unscented Kalman filter, which is much more sensitive to these uncertainties. RThSUKF algorithm has proved its advantages in future pinpoint Mars landing missions.
M
Reference
AC
CE
PT
ED
[1] Wolf, A. A., Graves, C., Powell, R. W., Johnson, W. (2005). System for pinpoint landing at Mars. Advances in the Astronautical Science, 119, 2677-2696. [2] Yu, Z., Cui, P., Crassidis, J. L. (2017). Design and optimization of navigation and guidance techniques for Mars pinpoint landings: review and prospect. Progress in Aerospace Sciences. 94, 82-94. [3] Brand, T., Fuhrman,L., Geller, D., Hattis, P., Paschall, S., and Tao, Y. C.(2004). GN&C technology needed to achieve pinpoint landing accuracy at Mars. AIAA/AAS Astrodynamics Specialist Conference and Exhibit. AIAA/AAS Astrondynamics Specialist Conference and Exhibit, Providence, Rhode Island, August 2004, 16-19. [4] Chu, C. C. (2006).Development of advanced entry, descent, and landing technologies for future Mars missions. IEEE Proceedings of Aerospace Conference, Big Sky, Montana, May2006, 4-11. [5] Steinfeldt, B. A., Grant, M. J., Matz, D.M., Braun, R. D., and Barton, G. H. (2008). Guidance, navigation, and control technology system trades for Mars pinpoint landing. AIAA Atmospheric Flight Mechanics Conference and Exhibit, Honolulu, Hawaii, August18-21. [6] Prince, J. L., Desai, P. N., Queen, E. M., and Grover, M. R. (2015). Mars Phoenix entry, descent, and landing simulation design and modeling analysis. Journal of Spacecraft and Rockets, 48(48), 756-764. [7] Levesque J F. (2006). Advanced navigation and guidance for high-precision planetary landing on Mars. Ph. D. thesis, Universite de Sherbroke, QC, Canada. [8] Levesque J. F., de Lafontaine J. (2007). Innovative navigation schemes for state and 20
ACCEPTED MANUSCRIPT
[15]
[16] [17] [18] [19] [20] [21]
AC
[22]
CR IP T
[14]
AN US
[13]
M
[12]
ED
[11]
PT
[10]
CE
[9]
parameter estimation during Mars entry. Journal of Guidance, Control, and Dynamics, 30(1), 169-184. Ely, T. A., Bishop, R. H., and Dubois-Matra, O. (2001).Robust entry navigation using hierarchical filter architectures regulated with gating networks. 16th International Symposium on Spaceflight Dynamics Symposium, Pasadena, CA United States. Heyne, M. C, Bishop, R. H. (2006). Spacecraft precision entry navigation using sigma point Kalman filtering. Proceedings of the IEEE/ION Position, Location and Navigation Symposium, San Diego, California. Heyne, M. C. (2007). Spacecraft precision entry navigation using an adaptive sigma point Kalman filter bank. Ph. D thesis. University of Texas, Austin. May. Zanetti, R, Bishop, R. H. (2007). Adaptive entry navigation using inertial measurements. Advances in the Astronautical Sciences, 127, 457-469. Marschke, J. M., Crassidis, J. L. (2008).Multiple model adaptive estimation for inertial navigation during Mars entry. AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Honolulu, Hawaii. Li, S., Jiang, X., Liu, Y. (2014). Innovative Mars entry integrated navigation using modified multiple model adaptive estimation. Aerospace Science and Technology, 39, 403-413. Julier, S. J., Uhlman. J.K.., Durrant, F. H. (2000). A new method for nonlinear transformation of means and covariances in filters and estimates. IEEE Transactions on Automatic Control, 45(3), 477-482. Wan, E. A., Rudolph, V. D. M. (2002). The unscented Kalman filter. Kalman filtering &Neural Networks, 221-280. Xiao, Q., Fu, H., Wang, Z., and Zhang, Y. (2017). Multiple model adaptive Rank estimation for integrated navigation during Mars entry. The Journal of Navigation, 70, 291-308. Wu, Y., Fu, H., Xiao, Q., et al. (2014). Extension of robust three-stage Kalman filter for state estimation during Mars Entry. IET Radar, Sonar and Navigation, 8(8), 895-906. Z. Yu, S. Zhu, P. Cui and L. Wang. (2015). A robust Kalman filter for Mars entry navigation. Proceedings of the 34th Chinese Control Conference. Hangzhou, China July 28-30. Alouani, A. T., Xia, P., Rice, T. R., and Blair, W. D. (1993). On the optimality of two-stage state estimation in the presence of random bias. 38(8), 1279-1282. Hmida, F. B., Khemiri, K., Ragot, J. et al. (2012). Three-stage Kalman filter for state and fault estimation of linear stochastic systems with unknown inputs. Journal of the Franklin Institute-Engineering and Applied Mathematics. 394(7), 2369-2388. Hmida, F. B., Khemiri, K., Ragot, J. et al. (2010).Robust filtering for state and fault estimation of linear stochastic systems with unknown disturbance. Mathematical Problems in Engineering .2010(1024-123x), 629-637. Mengli Xiao, Yongbo Zhang, Huimin Fu. (2017) Three-stage Unscented Kalman Filter for State and Fault Estimation of Nonlinear System with Unknown Input. Journal of the Franklin Institute.354(18). Morabito, D. D. (2002). The spacecraft communications blackout problem encountered during passage or entry of planetary atmospheres. Pasadena, California: Jet Propulsion Laboratory, August. Lightsey, E. G., Mogensen A,Burkhart P D,E Ely T A, and Duncan C. (2008). Real-time navigation for Mars mission using the mars networks. Journal of Spacecraft and Rockets,
[23]
[24]
[25]
21
ACCEPTED MANUSCRIPT
AC
CE
PT
ED
M
AN US
CR IP T
45(3), 519-533. [26] Chen, Z. (1991). Local observability and its application to multiple measurement estimation. IEEE Transactions on Industrial Electronics, 38(6), 491-496. [27] Yu, Z., Cui, P., Zhu, S. (2015). Observability-based beacon configuration optimization for Mars entry navigation. Journal of Guidance, Control, and Dynamics.38, 643-649. [28] Li, S., Jiang, X., Liu, Y. (2014).High-precision Mars entry integrated navigation under large uncertainties. Journal of Navigation.67, 327-342.
22
CR IP T
ACCEPTED MANUSCRIPT
ED
M
AN US
Fig.1 Degree of observability for IMU/ integrated navigation
(a)
(b)
AC
CE
PT
Fig.2 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D
Fig.3 Actual and estimated values of the unknown inputs
23
ACCEPTED MANUSCRIPT
(a)
(b)
AN US
CR IP T
(c)
(d)
(e)
(f)
PT
ED
M
Fig.4 Actual and estimated values of the state variables of the vehicle
(a)
(c)
AC
CE
(b)
(d)
(e)
(f)
Fig.5 Performance comparison of UKF and RThSUKF with respect to RMSE
24
ACCEPTED MANUSCRIPT
(a)
CR IP T
(b)
AN US
Fig.6 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D
CE
PT
ED
M
Fig.7 Actual and estimated values of the unknown inputs
(a)
(c)
AC
(b)
(d)
(e)
Fig.8 Actual and estimated values of the state variables of vehicle 25
(f)
ACCEPTED MANUSCRIPT
(c)
AN US
(b)
CR IP T
(a)
(d)
(e)
(f)
AC
CE
PT
ED
M
Fig.9 Performance comparison of UKF and RThSUKF with respect to RMSE in state variables
26
ACCEPTED MANUSCRIPT
Table1 Initial state variables and physical parameters of entry vehicle [6] True
Initial estimation
Initial altitude h0 /(km)
125
126
Initial longitude 0 /(deg)
0.00
0.02
Initial latitude 0 /(deg)
1.00
1.02
Initial velocity v0 /(m·s-1)
6900
Initial flight path 0 /(deg)
-12
Initial azimuth 0 /(deg)
89
CR IP T
Initial state conditions
6910 -13
AN US
90
Initial position(km)
Initial velocity(m/s)
Orbiting beacon 1
780.44,13276.13,1267.20
317.7, 50,1697.2
Orbiting beacon 2
3927.85, 230.90,374.91
93.6,3120.8, 584.1
3492.70,3659.20,215.11
605.1,97,3232.8
ED
Beacons type
M
Table2 Initial position and velocity of radio beacons [27]
PT
Orbiting beacon3
CE
Table3 Performance comparison of UKF and RThSUKF with respect to average RMSE Average RMSE
State variables
RThSUKF
UKF
RThSUKF
Position/(m)
526.2147
72.2183
591.6754
78.8354
Longitude/(deg)
0.0012
0.0012
0.0013
0.0012
Latitude/(deg)
0.0062
0.0034
0.0066
0.0035
Velocity/(m·s-1)
4.3596
0.9331
4.8583
1.0163
FPA/(deg)
0.0345
0.0110
0.0516
0.0127
Azimuth/(deg)
0.0318
0.0228
0.0472
0.0242
AC
UKF
27
Robust three-stage unscented Kalman filter for Mars entry phase navigation Yongbo Zhang , Mengli Xiao , Zhihua Wang , Huimin Fu , Yunzhang Wu PII: DOI: Reference:
S1566-2535(18)30098-8 https://doi.org/10.1016/j.inffus.2018.11.003 INFFUS 1041
To appear in:
Information Fusion
Received date: Revised date: Accepted date:
5 February 2018 29 October 2018 6 November 2018
Please cite this article as: Yongbo Zhang , Mengli Xiao , Zhihua Wang , Huimin Fu , Yunzhang Wu , Robust three-stage unscented Kalman filter for Mars entry phase navigation, Information Fusion (2018), doi: https://doi.org/10.1016/j.inffus.2018.11.003
This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.
ACCEPTED MANUSCRIPT
Highlights
Precision Mars entry navigation algorithm under uncertainties and unknown inputs Uncertainties in atmosphere density, ballistic coefficient and lift-to-drag ratio
RThUKF has shown great advantages in state estimation of nonlinear system
Simulation resulting in precision navigation fulfills future pinpoint landing.
AC
CE
PT
ED
M
AN US
CR IP T
1
ACCEPTED MANUSCRIPT
Robust three-stage unscented Kalman filter for Mars entry phase navigation Yongbo Zhang1*, Mengli Xiao1, Zhihua Wang, Huimin Fu1,Yunzhang Wu2 1
Research Center of Small Sample Technology, Beihang University, Beijing, China 100191, 2
Army Aviation Research Institute, Beijing, China 101123
CR IP T
E-mail: [email protected]
Abstract A high-precision Mars entry navigation algorithm under parameter uncertainties and unknown inputs is developed via a robust three-stage unscented Kalman filter. The uncertainties lie in the atmosphere density, ballistic coefficient and
AN US
lift-to-drag ratio are the most important error source of state estimation, which have been investigated in this paper. As the robust three-stage unscented Kalman filter has great advantages in parameter estimation and state estimation for nonlinear system with unknown input, it is proposed to compensate the effect of those parameter
M
uncertainties and unknown inputs and obtain more accurate estimation. In the numerical simulation of Mars entry navigation, this filter algorithm has shown its
ED
advantages by resulting a high precision navigation with position error less than 100m and velocity error less than 2m/s, which fulfills the requirement of future pinpoint
PT
Mars landing missions.
Key words: robust three-stage unscented Kalman filter, nonlinear system, parameter
CE
uncertainties, unknown input, Mars entry navigation
AC
1. Introduction The landing accuracy of Mars vehicle has been improved gradually. Generation 1
Mars landing systems aimed at landing safely without having the freedom to choose the landing site, the landing accuracy of which was more than 35km to the target. By introducing the hypersonic guidance and with advanced navigation capability, Generation 2 delivered the vehicle to within 5-10km of the target
[1, 2]
. But the
pinpoint landing accuracy of future Mars exploration missions was required to be within 100m of a pre-selected target site. To achieve pinpoint landing, high-precision 2
ACCEPTED MANUSCRIPT
navigation, guidance, and control techniques are necessary for Mars entry, descent and landing (EDL) phases [3-6]. According to the existing investigation [1, 7, 8], there are three most significantly important sources leading to Mars landing dispersion during EDL: the first one is position and velocity errors at the Mars entry point, the second one is uncertainties in the Mars atmosphere density and the vehicle aerodynamic characteristics during Mars entry phase, and the last one is drifts caused by strong
CR IP T
winds during the parachute descent phase.
For the initial errors and the uncertainties in Mars entry phase, several algorithms have been developed. Levesque et al. [7] obtained the parameter estimation by treating the uncertain parameters as the augmented state. Ely et al. [9] estimated the Mars [10, 11]
AN US
atmosphere density using Hierarchical mixture of experts. Heyne et al.
developed an adaptive sigma point Kalman filter bank to reduce the navigation errors caused by the atmosphere density uncertainty. In the investigations mentioned above, only the data of IMU outputs was used as the measurements, while it has been proven
Bishop
[12]
M
that IMU-based navigation cannot make all the state variables convergent. Zanetti and investigated the application of a multiple model adaptive estimation
ED
(MMAE) comprising an extended Kalman filter (EKF) bank to address the problem of the state estimation for Mars entry navigation under uncertain atmosphere density. In
PT
MMAE, a bank of filters running in parallel, and the state estimate was obtained by combining the estimates of all parallel filters. This algorithm was efficient in the state
CE
estimation. An extension to the MMAE for Mars entry navigation was proposed by Marschke et al.
[13]
to analyze the effect of the biases and scale factors of IMU on Mars
AC
navigation, in which the adverse effect of the biases and scale factors cannot be eliminated completely. Nevertheless, there was excessive competition between the various sub-models in the MMAE, thus large estimation errors were produced if one model was closer to the actual model than other ones. Li et al [14] proposed a modified MMAE methodology with exponential decay terms to reduce the excessive competition. But Li used EKF for the state estimation, while for complex nonlinear system, using first-order Taylor series expansion may produce truncation errors that lead to the filter divergence
[15, 16]
. Also, it may be difficult to compute the partial 3
ACCEPTED MANUSCRIPT
derivatives. To solve the problems, Xiao et al.
[17]
proposed an integrated navigation
based on multiple model adaptive rank estimation (MMARE) for Mars entry phase. This novel algorithm adopted several Rank filters running in parallel instead of EKFs. MMARE obtain a more superior performance when compared with the modified MMAE. But the numerical underflows may happen as the multiple model adaptive estimation was designed based on the assumption that the actual value of the
CR IP T
uncertain parameters were contained with the assumed values. But neither Li nor Xiao considered the unexpected unknown inputs for radiometric measurements that caused by the ionization of atmosphere. Wu et al
[18]
proposed an extension of robust
three-stage Kalman filter (ERThSKF) to investigate the parameter uncertainties and
AN US
unknown inputs for high-precision entry navigation, but it may encounter implementation problem when the complex nonlinear system cannot be transformed into linear problems or the model partial derivatives are difficult or impossible to compute. Yu et al
[19]
proposed a robust Kalman filter for Mars entry under the
M
atmosphere density uncertainty and unknown inputs. In this filter, a performance index was established by considering the trace of posterior variance matrix and the
ED
transition matrices of atmosphere density uncertainty and unknown inputs. And then the gain matrix was determined by minimizing the performance index. The method
PT
was proven to be more accurate and robustness to noise. But the filter was also an improvement of EKF.
CE
Augmented state Kalman filter (ASKF) was a basic approach to address the problem of state estimation for stochastic discrete-time system with uncertain
AC
parameters and unknown inputs, of which the uncertain parameter and the unknown input vector were augmented as the state vector
[7]
. However, the computational
complexity and memory cost increased with the augmented state dimension, and it becomes serious when the dimensions of the uncertain parameter vector and the unknown input vector were comparable to that of the state vector
[20]
. To improve its
efficiency, Hmida et al. [21] developed an optimal three-stage Kalman filter (OThSKF) for linear discrete-time stochastic system by decoupling the covariance matrices of ASKF via a three-stage U-V transformation, which was proven to be optimal for the 4
ACCEPTED MANUSCRIPT
dynamical evolution models of the uncertain parameters and the unknown inputs were known. Considering the dynamic models were not perfectly known or unknown, a robust ThSKF (RThSKF)
[21]
and an augmented RThSKF
[22]
were developed by
modifying the measurement update equations of the OThSKF. Both RThSKF and ARThSKF can provide unbiased minimum-variance parameter estimation and state estimation for the linear system with unknown inputs. For nonlinear system, Wu et.al [18]
. But as mentioned above,
CR IP T
extended RThSKF (ERThSKF) for state estimation
ERThSKF was an EKF-like nonlinear filter which may encounter difficulties in actual application. Based on the unscented Kalman filter (UKF)
[15, 16]
, a robust three-stage
unscented Kalman filter (RThSUKF) which avoids the calculation of model partial
AN US
derivates is proposed in our investigation [23].
This paper investigates the application of RThSUKF for Mars entry navigation. The uncertain parameters including atmosphere density, ballistic coefficient and lift-to-drag ratio and unknown inputs that may lead to performance degeneration of
M
the traditional Kalman filters are considered. With the adverse effect of all these uncertainties been compensated, RThSUKF could obtain a high-precision estimate for
ED
Mars entry navigation. The paper is organized as follows. Section 2 demonstrates the traditional simplified 3-DOF Mars entry dynamic model and measurement equations.
PT
Section 3 introduces the robust three-stage unscented Kalman filter. Section 4 gives the numerical simulation and the performance comparison between the standard
CE
unscented Kalman filter and RThSUKF. Finally, in Section 5, there is a conclusion of our research.
AC
2. Mars entry dynamic model and measurement equations 2.1. Mars entry dynamic model The Mars entry phase begins at the Mars atmosphere interface (defined as a radius of 3552km from the center of Mars, and an altitude of 125km over the surface) and ends at the first parachute deployment. The vehicle is assumed to fly in a stationary atmosphere of non-rotating spherical planet, thus winds and the centripetal and Coriolis effects due to the Mars rotation are neglected. The accuracy of Mars 5
ACCEPTED MANUSCRIPT
entry navigation is heavily depended on the dynamical model, which is used for predicting the state of vehicle. A widely used simplified 3-DOF dynamic model of the Mars entry defined with respect to the Mars-centered, Mars-fixed coordinate system [7] is given by x f x
(1)
where x r v , r denotes the distance from the center of Mars to the center of mass of the entry vehicle,
CR IP T
T
is the longitude, is the latitude, v is
the velocity of the entry vehicle, is the flight-path-angle (FPA) and is the azimuth angle.
(2)
ED
M
AN US
v sin v cos sin r cos v cos cos f x r D g (r ) sin v g (r ) cos L cos r v v v sin cos tan L sin v cos r
PT
where is the bank angle defined as the angle about the velocity vector from the local vertical plane to the lift vector, which is also considered as the only control
CE
variable. g (r ) is the Mars gravitational acceleration, and an Newton’s inverse square acceleration model giving a roughly accuracy of 99% [7] is given by
AC
g (r ) r 2
(3)
where =42828.29 109 m3 s2 is the Mars gravitational constant. D and L are the aerodynamic drag and lift accelerations, which are defined by D
1 2 v B 2
L L DD
B
CD S m
(4)
where B is called the ballistic coefficient, L D is the lift-drag ratio, C D is the aerodynamic drag coefficient, S is the reference surface area, m is the mass of the 6
ACCEPTED MANUSCRIPT
entry vehicle. As the importance of aerodynamic characteristics in the dynamic model, the uncertainties are one of the most important error sources leading to an inaccurate dynamical state estimate. is the Mars atmospheric density and an exponential Mars atmospheric density
model is assumed as follows =0 exp(r0 r ) hs
CR IP T
(5)
where 0 is the density on the surface of the Mars, r0 3437.2km is the reference radial radius of Mars (40km above the Mars), and hs =7.5km is the constant atmospheric scale height. Since the true Mars atmospheric density varies with the seasons and
AN US
temperature, an accurate Mars atmospheric density is still difficult to be achieved, and the model (5) is only an approximation. Therefore, the deviation of the atmospheric density is considered in our research.
2.2. Measurement equations
M
During the Mars entry phase, a heat shield was wrapped outside protecting the vehicle from the aerodynamic heating. Therefore, among the conventional navigation [12, 13]
ED
sensors, IMU is the only available sensor for Mars entry navigation
. Recently,
the ultra high-frequency (UHF) communication between the vehicle and the orbiter or [24, 25]
PT
the surface beacons was proven to be slightly affected by the ionizing plasma
,
providing an effective way for Mars entry phase navigation. In our investigation, the
CE
IMU/radio beacons integrated navigation is adopted as the navigation scheme.
AC
Accelerometer measurements The accelerometer of IMU is used for measuring the non-gravitational linear
acceleration along three orthogonal axes. The accelerometer measurement in the body-fixed coordinate system can be given by an accelerometer as follows ab ab ba ξa
where ab
is
the
true
acceleration
7
along
(6) the
body-fixed
coordinate
ACCEPTED MANUSCRIPT
T
system, ba bx by bz is the unknown acceleration bias, ξ a is the accelerator stochastic noise modeled as zero-mean, Gaussian distribution white noise. The acceleration along the body-fixed coordinate system ab can be given by ab Tvb a v
(7)
a v D, D L D sin , D L D cos
T
CR IP T
(8)
where a v is the acceleration vector in the velocity coordinate system, Tvb is the coordinate transition matrix for the velocity coordinate system to the body-fixed coordinate system.
AN US
As the dynamical model defined with respect to the Mars-centered, Mars-fixed coordinate system, measurement vectors are needed to transfer from the velocity coordinate system to the Mars-centered, Mars-fixed coordinate system. a m TpmTvp a v
(9)
M
where a m is the output vector of accelerator in the Mars-centered, Mars-fixed coordinate system, Tpm is the coordinate transition matrix for the system of
ED
navigation coordinate to the Mars-centered, Mars-fixed coordinate system, and Tvp is
PT
the coordinate transition matrix of the velocity coordinate system to navigation coordinate system, which are given by
CE
cos cos Tpm cos sin sin
sin cos 0
sin cos sin sin cos
cos Tvp sin cos sin sin
sin cos cos cos sin
0 sin cos
(10)
AC
Radiometric range measurements As illustrated above, the vehicle could communicate with orbiters and surface
beacons via UHF The radiometric measurements between the vehicle and the radio beacons are obtained via radiometric transceivers. By analyzing the ranging shifted signals, the radial range between the vehicle and an orbiter or surface beacon can be modeled as follows R R bR ξ R 8
(11)
ACCEPTED MANUSCRIPT
rl rb rl rb -
R
T
(12)
r cos cos r r cos sin r sin
(13)
where R is the radial range between the entry vehicle and the orbiters or the surface
CR IP T
beacons, bR is the range measurement bias, ξ R is zero-mean, Gaussian distribution white noise, rl is the position vector of the entry vehicle, rb is the position vector of an orbiter or a surface beacon. Both the position vectors mentioned above are defined in the Mars centered, Mars-fixed coordinate system. Doppler measurement
can be modeled as follows
AN US
According to Doppler shifted signals, the change rate of ranging measurement
V V ξV
rl rb vl vb
(14)
T
M
V
R
(15)
ED
where V is the change rate of radial distance, ξV is zero-mean, white noise. v l denotes the velocity vector of the entry vehicle, vb denotes the velocity vector of the
PT
orbiter or the surface beacon. The vectors mentioned above are defined in the Mars centered, Mars-fixed coordinate system.
CE
Since the accelerometer of IMU is used to measure the true aerodynamic
acceleration of the vehicle in Mars entry, it should be noted that the atmosphere
AC
density and aerodynamics coefficients involved in Eq. (8) are true values. Thus the uncertain parameters are also included in the measurement equations. Besides, as the ionization of the atmosphere occurs, it will result in unacceptable unknown inputs of the radiometric measurements. As uncertain parameters and unknown inputs may degrade the performance of the navigation filter, thus how to compensate the effect of the parameter uncertainties and unknown inputs should be considered in the navigation filter design. 9
ACCEPTED MANUSCRIPT
2.3. Observability analysis The observability of the autonomous navigation is an important index representing the state estimation capability of vehicle via measurements in finite time, thus always been emphasized in the navigation system design. In this study, for convenient, the system is firstly linearized by the first-order approximation as follows f f0 F x x0
CR IP T
h h0 H x x0
(16) (17)
where F and H are the first order linearization matrices of the state and measurement equations, respectively. Then the observability matrix of the system [26] is given by T , H n 1F
T
AN US
T T OM H T , HF , H 2 F ,
(18)
A condition number of the observability matrix is used as the performance index to evaluate the observability of the system, which reflects the singularity of the observability matrix. It may be closer to singular when there is a larger condition
M
number [27]. Accordingly, the condition number is defined as max (OM ) min (OM )
(19)
ED
cond (OM )
where max and min are the maximum and minimum singular number of the
PT
observability matrix. The degree of the observability for the system is defined by the
CE
inverse of the condition number of the observability matrix, which is given by
(O ) 1 min M cond (OM ) max (OM )
(20)
The degree of observability holds for the condition of 0 1 . If 0 , it means
AC
that the rank of the observability matrix is less than the state dimension, then the system is locally unobservable. Otherwise, the system is locally weakly observable, and the degree is larger, the system is more observable [27].
3. Robust three-stage unscented Kalman filter According to the Mars entry dynamical model and measurement equations given above, the discrete-time nonlinear system with uncertain parameters and unknown input is given by 10
ACCEPTED MANUSCRIPT
xk 1 f k xk , uk , bk , d k wkx
(21)
yk hk xk , uk , bk , dk vk
(22)
where f () and h() are nonlinear state transformation function and nonlinear measurement function, respectively. xk 1 n is the state vector, yk m is the
CR IP T
observation vector, bk p is the uncertain parameter vector, and dk q is unknown inputs. All matrices have the appropriate dimensions. The noise sequences wkx and v k are zero-mean uncorrelated Gaussian random sequences with w x w x T Q x E k j k vk v j 0
0 kj Rk
AN US
(23)
where Qkx 0, Qkb 0, Qkd 0, Rk 0 and kj is the Kronecker delta, kj 1 if k j , else kj 0 if k j . The initial state x0 , fault b0 and unknown inputs d 0 are assumed
M
to be uncorrelated with the zero-mean white noise sequences wkx , wkb , wkd and v k . Assume that x0 satisfies the following equations xˆ 0 E( x0 ), P0x E ( x0 xˆ 0 )( x0 xˆ 0 )T
ED
(24)
PT
The proposed method is the work from [23], which was designed for dealing with the uncertain parameters and the unknown inputs. Firstly, the matrices of
CE
uncertain parameter and unknown input are identified by the following numerical approximation B B
AC
x k
E E x k
Bky Bky ,1
E E y k
y ,1 k
x ,1 k
x, p k
, B
x,q k
, E
B
x ,1 k
E
x ,i k
x ,i k
Bky , p , Bky ,i
E
y,q k
, E
y ,i k
f k xˆ k k , uk , bˆk k i eip , d k k f k xˆ k k , uk , bˆk k , d k k
(25)
i
f k xˆ k k , uk , bˆk k , d k k i eiq f k xˆ k k , uk , bˆk k , d k k
(26)
i
hk xˆ k k 1 , uk , bˆk k 1 i eip , d k k 1 hk xˆ k k 1 , uk , bˆk k 1 , d k k 1 i
hk xˆ k k 1 , uk , bˆk k 1 , d k k 1 i eiq hk xˆ k k 1 , uk , bˆk k 1 , dk k 1 i 11
(27)
(28)
ACCEPTED MANUSCRIPT
where
e ip and eqi are the i th column of the identity matrices with dimensions p
and q , respectively. i is an appropriate selected small value. Then the nonlinear discrete-time varying system given by (21) and (22) can be approximated as
xk 1 f k* xk , uk , bˆk k , dk k Bkx bk Ekx dk wkx
(29)
yk hk* xk , uk , bˆk k 1 , dk k 1 Bky bk Eky dk vk f k* xk , uk , bk k f k xk , uk , bˆk k , dk k Bkx bˆk k Ekx dk k
where
CR IP T
(30)
hk* xk , uk , bˆk k 1 , dk k 1 hk xk , uk , bˆk k 1 , dk k 1 Bky bˆk k 1 Eky dk k 1
Step 1: Initialization.
.
AN US
At the first stage, an initial V023 should be chosen.
xˆ 0 0 xˆ 0 , Pˆ0x0 P0x ,V023
Step 2: State sub-filter
and
(31)
This sub-filter is designed based on the assumption that the uncertain parameters
M
and the unknown inputs are identically zero. Thus, update the state estimate xk k and its
ED
error covariance matrix Pkxk by unscented Kalman filter as follows χk 1,i xˆ k 1 k 1
xˆ k 1 k 1 (n+ )Pˆkx1 k 1
xˆ k 1 k 1 (n+ )Pˆkx1 k 1
AC
CE
PT
χk*/ k 1,i f k*1 χk 1,i , uk 1 , bˆk 1/ k 1 , dk 1/ k 1 2 nx
(32) (33)
xk k 1 i χ k*/ k 1,i m
(34)
i 0
2n
c Pkx k 1 i χ k* k 1,i xk k 1 χ k* k 1,i xk k 1 i 0
χ k k 1,i xk k 1
T
Q
xk k 1 (n+ )Pkx k 1
xk k 1 (n+ )Pkx k 1
yk / k 1,i hk* χk k 1,i , uk 1 , bˆk 1/ k 1 , dk 1/ k 1 2 nx
yk / k 1 i yk / k 1,i i 0
12
x k
m
(35) (36) (37) (38)
ACCEPTED MANUSCRIPT
2n
c Pxy i χ k k 1,i xk k 1 yk k 1,i yk / k 1 i 0
2n
T
c Pyy i yk k 1,i yk / k 1 yk k 1,i yk / k 1 i 0
K kx Pxy Pyy
T
1
(39)
R
k
(40) (41) (42)
CR IP T
xk k xk k 1 K kx yk yk / k 1
Pkxk Pkxk 1 K kx Pyy K kx
T
(43)
where n is dimension of state vector, 2 (n ) n is a scaling parameter, is a constant determines the spread of the sigma points around mean value, and usually set
AN US
to a small positive value. The constant is a secondary scaling parameter, which is usually set to 0 or 3 n , and is used to incorporate prior knowledge of the distribution of mean value, with the weights i
given by [16]
M
0 m n , 0c n (1 2 ), i m ic 2 n i 1,2, 2n (38)
Step 3: Uncertain parameter sub-filter
ED
This sub-filter is designed based on the assumption that the unknown inputs are
PT
identically zero, giving the unknown input-free parameter estimate bk / k and its error
AC
CE
covariance matrix Pkb/ k by
Uk12 Bkx1
(44)
Sk2 Hk Uk12 Bky
(45)
Pkb/ k Sk2T Pyy1 Sk2
(46)
K kb Pkb/ k Sk2T Pyy1
(47)
bk / k K kb yk yk / k 1
(48)
Step 4: Unknown input sub-filter The filter gives the input estimate dk / k and its error covariance matrix Pkd/ k by Uk23 Vk231 13
(49)
ACCEPTED MANUSCRIPT
Uk13 Ekx1 Uk12Uk23
(50)
Sk3 Hk Uk13 BkyUk23 Eky Pkd/ k S k3T Pyy1 S k3
(51)
(52)
K kd Pkd/ k Sk3T Pyy1αk I Sk2 Sk2
d k / k K kd yk yk / k 1
Step 5: Correction of the state and the fault estimations
(54)
CR IP T
where αk is an arbitrary matrix.
(53)
The optimal state and fault estimates xˆ k k , bˆk k as well as their error covariance
AN US
matrices Pˆkx/ k , Pˆkb/ k are obtained from the estimates of three sub-filters and the coupling
Vk12 Uk12 K kx Sk2
(55)
Vk23 Uk23 K kd Sk3
(56)
M
equations Vk12 ,Vk13 ,Vk23 as shown
CE
PT
ED
Vk13 Uk13 Vk12 Kkd Sk3 Kkx Sk3
(57)
xˆ k k xk k Vk12 bk k Vk13d k k
(58)
bˆk k bk k Vk23dk k
(59)
Pˆkx/ k Pkx/ k Vk12 Pkb/ kVk12T Vk13 Pkd/ kVk13T Pˆkb/ k Pkb/ k Vk23 Pkd/ kVk23T
(60) (61)
AC
RThSUKF is a robust filter designed for predicting the state and parameter of
nonlinear system in presence of unknown input. For the Mars entry navigation problem in this study, if the statistic properties of uncertain parameters and unknown inputs are perfectly known, it can be addressed by augmented state unscented Kalman filter (ASUKF) via treating the uncertain parameters and unknown inputs as the augmented state. However, the computational and memory resource of vehicle is limited, and the costs are increasing with the dimension of the augmented state. What is worse, ASUKF loses its optimality when part of uncertain parameter and unknown 14
ACCEPTED MANUSCRIPT
inputs models are unknown. But RThSUKF could solve this problem by decoupling the covariance matrices of ASUKF via a three-stage U-V transformation and modifying the measurement update equations of uncertain parameter sub-filter and unknown input sub-filter. Therefore, RThSUKF could obtain an optimal estimate in spite of the statistical information of the uncertain parameters and the unknown inputs
4. Numerical simulation and analysis
CR IP T
are partially known or unknown during the Mars entry navigation.
In this section, numerical simulation of Mars entry navigation is carried out to illustrate the performance of the proposed algorithm. The planned entry span is assumed to be 400s， which is equal to the period of the vehicle from the atmospheric
AN US
surface 125km to 10km, and the simulation sample step is set to 1s. The initial values of state variables are listed in Table 1. Also, we adopted 3 orbiting beacons in the course of entry. The initial positions and velocities of the radio beacons are described in Table 2[28]. As the actual values of the atmosphere density and aerodynamics
M
coefficients are still difficult to be obtained. The uncertainties lied in have been investigated by loads of researchers. Based on the previous measurement, several
ED
assumptions have been made. In the experiment of Prince et al.
[4]
, sets of 2000
atmosphere profiles have been developed specifically for the Phoenix landing sites
PT
and seasons incorporating day-to-day variations in temperature and pressure. Theses profiles include regional density, temperature, and pressure characteristics with
CE
respect to altitude. Fig.1 shows the percentage difference of the 2000 individual profiles. Accordingly, Li et al. [5,6], Chen et al. [6,7], Schoenenberger et al. [8], Chen et al. , believed that there is a maximum of 15% deviations in the nominal values from
AC
[9]
exponential Mars atmospheric density model when compared with the true Mars atmosphere density, and the uncertainties roughly follows a normal distribution around the mean values. In our investigation, two cases are compared in the simulation, of which the uncertainties are assumed to follow Normal and Uniform distribution, respectively. And according to the research of [19], the unknown inputs of the radiometric ranging measurement are assumed to be containing a position bias 15
ACCEPTED MANUSCRIPT following Normal distribution of N 10,1002
[19]
.
The simulation results are
compared with conventional unscented Kalman filter (UKF), and the root mean square error (RMSE) is used as measure. The scaling parameters for the proposed filter and UKF are chosen to 0.6, 2, 3 n . Table1 Initial state variables and physical parameters of entry vehicle [7]
CR IP T
***** Table2 Initial position and velocity of radio beacons [28] *****
The observability of the IMU/radio beacons integrated navigation is firstly
AN US
analyzed before the simulation. The degree of obsevability of the integrated navigation is shown in Fig.1. In this figure, the degree of observability increases fast during the first 50 seconds, and reaches the peak of 2.2 108 . But it rapidly reduces to a relatively lower value of 1 109 , and then fluctuates at the mean value of 7.5 109 . In all, it reaches the minimum value 7 1011 at the beginning which is larger
M
than 1 1016 , indicating that the navigation system is observable during the whole
ED
entry phase, though the uncertain parameters and measurement errors are needed to be estimated simultaneously.
PT
*****
Fig.1 Degree of observability for IMU/ integrated navigation
CE
Case 1:
According to the analysis above, the deviation of reference atmosphere
AC
density 0 is assumed to follow a Normal distribution of N 0, 0.152 in the first case. As the reference atmosphere density 0 and ballistic coefficient B are both the parameters of D and L as shown in Eq. (4), we consider the product of 0 B and lift-to-drag ratio L / D as the uncertain parameter vector. The deviations of ballistic coefficient B and lift-to-drag ratio L D are assumed to follow uniform distribution of U 0.1, 0.1 . 16
ACCEPTED MANUSCRIPT
The actual and estimated values of uncertain parameters 0 B and L / D are plotted in subfigures (a) and (b) of Fig.2. As shown, at the first 30 seconds, the RThSUKF are adjusting the initial conditions with a relatively larger deviation. But, after that it rapidly adapts these changes of the parameters, and obtains accurate estimates quickly. And then, it keeps that good performance to the end of the whole
CR IP T
simulation process. In another part, the unknown inputs need to be estimated, and Fig.3 depicts the comparison of these estimates with the actual values. Also, it can be observed that there is still an adaptive adjustment of this algorithm at the first 30 seconds, and after this adjustment, the input estimates are close to the actual values. *****
AN US
Fig.2 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D *****
Fig.3 Actual and estimated values of the unknown inputs
The state estimation was the most important part of this simulation. Fig.4 depicts
M
the estimation of position, longitude, latitude, velocity, FPA, and azimuth in
ED
subfigures (a) to (e) respectively. As there are small differences between the actual values and each state estimate, the RThSUKF exhibits advantages in the Mars entry
PT
navigation with good performances. For further analysis, the comparison between the conventional widely used UKF and RThSUKF are conducted as shown in Fig.5, of
CE
which the subfigures (a)-(e) exhibits the RMSE of estimations in position, longitude, latitude, velocity, FPA, and azimuth under the same condition, respectively. As shown,
AC
UKF represents a relative more accurate state estimation at the initial filter phase of the first 40 seconds. But, after that the state estimation errors increase sharply in a short time and fluctuate at a high value till the end of simulation. For some of the most important state, position (a) and velocity (d), it could reach a peak of 500m and 10m/s, respectively, which is unacceptable. This is fundamentally caused by the uncertain parameters and the unknown inputs, and when they accumulate larger and larger, it becomes more difficult for the conventional UKF to compensate, thus results in the large estimation errors. Therefore, in the simulation of Mars entry navigation, 17
ACCEPTED MANUSCRIPT
UKF is difficult in addressing the system in presence of the uncertain parameters and unknown inputs. However, RThSUKF has the contrary simulation results to the performance of UKF. It has relative larger state estimation errors during the first 40 seconds as there are parameter and input estimation adjustment as shown in Figs.2 and Fig.3. Then the state estimation errors reduced quickly to a very low level with a little fluctuation till
values. *****
CR IP T
the end of simulation, thus the final state estimation are very close to the actual
Fig.4 Actual and estimated values of the state variables of the vehicle
AN US
***** Fig.5 Performance comparison of UKF and RThSUKF with respect to RMSE
Case 2:
In the second case, the deviations of reference atmosphere density are assumed
M
to be subjected to Normal distribution of N 0, 0.152 , and the ballistic coefficient as
ED
well as lift-to-drag ratio also follows the normal distribution of N 0, 0.12 . The actual and estimated values of the uncertain parameters, unknown inputs and state variables
PT
are compared in Figs.6, 7, and 8, respectively. As shown, RThSUKF performs as well as case 1, exhibiting accurate estimations after a period of time adjustment. Fig.9
CE
shows the performance comparison of UKF and RThSUKF with respect to RMSE in state variables. UKF is still very sensitive to the drastic changes of the parameter
AC
uncertainties and the unknown inputs, and the estimation errors fluctuate at a high value. In all, RThSUKF could obtain a superior estimation in presence of uncertainties and unknown inputs whether what the distributions of these uncertainties and unknown inputs subject to. ***** Fig.6 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D ***** 18
ACCEPTED MANUSCRIPT
Fig.7 Actual and estimated values of the unknown inputs ***** Fig.8 Actual and estimated values of the state variables of vehicle ***** Fig.9 Performance comparison of UKF and RThSUKF with respect to RMSE in state variables
Table 3 illustrates the average RMSE of the two algorithms for the two cases,
CR IP T
which is defined over all the time instants in the state variables. As shown, The RMSE of RThSUKF is as much as one order in magnitude less than that of UKF in all the state variables. Especially for the position estimation, the errors of RThSUKF are less than 100m in both cases, but that of UKF are up to 526.2147m and 591.6754m,
AN US
respectively. For comparison, the differences of velocity are not so large between these two filter algorithms. It may be the reason that the Doppler measurements have provided adequate and sufficiently accurate velocity information for the Mars entry vehicle in this simulation. Also, with the comparison of these two cases, it has shown
M
that the average RMSE of UKF becomes larger as the parameter’s distribution varies from case1 to case 2. But RThSUKF keeps an accurate stable state estimation.
ED
Therefore, UKF is really sensitive to the parameter uncertainties and unknown inputs. It furtherly illustrates the advantages of RThSUKF in dealing with the state estimation
PT
of system with uncertain parameters and unknown inputs.
CE
Table3 Performance comparison of UKF and RThSUKF with respect to average RMSE *****
According to the simulation results analysis above, it has shown that RThSUKF
AC
performs well in the state estimation of Mars entry autonomous navigation when the nonlinear system is in presence of uncertain parameters and unknown inputs. The filter can obtain the parameter estimation precisely and compensate the effect of parameter uncertainties and unknown inputs properly. Moreover, it can achieve superior performance with high precision state estimation, which fulfills the Mars entry navigation accuracy requirement of the future Mars pinpoint landing.
19
ACCEPTED MANUSCRIPT
5. Conclusion A robust three-stage unscented Kalman filter (RThSUKF) is introduced to address the problem of sate estimation under the large parameter uncertainties and unknown inputs during Mars entry phase. RThSUKF is a robust filter and performances well at gaining unbiased state and parameter estimation for the
CR IP T
nonlinear system in presence of unknown inputs. With considering the large uncertainties lie in the atmosphere density, ballistic coefficient and lift-to-drag ratio as well as the unknown inputs, numerical simulation of Mars entry based on IMU/radios beacons integrated navigation is conducted. The simulation results show that RThSUKF could compensate the effect of parameter uncertainties and unknown input
AN US
properly. It has represented a superior performance with high precision navigation accuracy when compared with the conventional standard unscented Kalman filter, which is much more sensitive to these uncertainties. RThSUKF algorithm has proved its advantages in future pinpoint Mars landing missions.
M
Reference
AC
CE
PT
ED
[1] Wolf, A. A., Graves, C., Powell, R. W., Johnson, W. (2005). System for pinpoint landing at Mars. Advances in the Astronautical Science, 119, 2677-2696. [2] Yu, Z., Cui, P., Crassidis, J. L. (2017). Design and optimization of navigation and guidance techniques for Mars pinpoint landings: review and prospect. Progress in Aerospace Sciences. 94, 82-94. [3] Brand, T., Fuhrman,L., Geller, D., Hattis, P., Paschall, S., and Tao, Y. C.(2004). GN&C technology needed to achieve pinpoint landing accuracy at Mars. AIAA/AAS Astrodynamics Specialist Conference and Exhibit. AIAA/AAS Astrondynamics Specialist Conference and Exhibit, Providence, Rhode Island, August 2004, 16-19. [4] Chu, C. C. (2006).Development of advanced entry, descent, and landing technologies for future Mars missions. IEEE Proceedings of Aerospace Conference, Big Sky, Montana, May2006, 4-11. [5] Steinfeldt, B. A., Grant, M. J., Matz, D.M., Braun, R. D., and Barton, G. H. (2008). Guidance, navigation, and control technology system trades for Mars pinpoint landing. AIAA Atmospheric Flight Mechanics Conference and Exhibit, Honolulu, Hawaii, August18-21. [6] Prince, J. L., Desai, P. N., Queen, E. M., and Grover, M. R. (2015). Mars Phoenix entry, descent, and landing simulation design and modeling analysis. Journal of Spacecraft and Rockets, 48(48), 756-764. [7] Levesque J F. (2006). Advanced navigation and guidance for high-precision planetary landing on Mars. Ph. D. thesis, Universite de Sherbroke, QC, Canada. [8] Levesque J. F., de Lafontaine J. (2007). Innovative navigation schemes for state and 20
ACCEPTED MANUSCRIPT
[15]
[16] [17] [18] [19] [20] [21]
AC
[22]
CR IP T
[14]
AN US
[13]
M
[12]
ED
[11]
PT
[10]
CE
[9]
parameter estimation during Mars entry. Journal of Guidance, Control, and Dynamics, 30(1), 169-184. Ely, T. A., Bishop, R. H., and Dubois-Matra, O. (2001).Robust entry navigation using hierarchical filter architectures regulated with gating networks. 16th International Symposium on Spaceflight Dynamics Symposium, Pasadena, CA United States. Heyne, M. C, Bishop, R. H. (2006). Spacecraft precision entry navigation using sigma point Kalman filtering. Proceedings of the IEEE/ION Position, Location and Navigation Symposium, San Diego, California. Heyne, M. C. (2007). Spacecraft precision entry navigation using an adaptive sigma point Kalman filter bank. Ph. D thesis. University of Texas, Austin. May. Zanetti, R, Bishop, R. H. (2007). Adaptive entry navigation using inertial measurements. Advances in the Astronautical Sciences, 127, 457-469. Marschke, J. M., Crassidis, J. L. (2008).Multiple model adaptive estimation for inertial navigation during Mars entry. AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Honolulu, Hawaii. Li, S., Jiang, X., Liu, Y. (2014). Innovative Mars entry integrated navigation using modified multiple model adaptive estimation. Aerospace Science and Technology, 39, 403-413. Julier, S. J., Uhlman. J.K.., Durrant, F. H. (2000). A new method for nonlinear transformation of means and covariances in filters and estimates. IEEE Transactions on Automatic Control, 45(3), 477-482. Wan, E. A., Rudolph, V. D. M. (2002). The unscented Kalman filter. Kalman filtering &Neural Networks, 221-280. Xiao, Q., Fu, H., Wang, Z., and Zhang, Y. (2017). Multiple model adaptive Rank estimation for integrated navigation during Mars entry. The Journal of Navigation, 70, 291-308. Wu, Y., Fu, H., Xiao, Q., et al. (2014). Extension of robust three-stage Kalman filter for state estimation during Mars Entry. IET Radar, Sonar and Navigation, 8(8), 895-906. Z. Yu, S. Zhu, P. Cui and L. Wang. (2015). A robust Kalman filter for Mars entry navigation. Proceedings of the 34th Chinese Control Conference. Hangzhou, China July 28-30. Alouani, A. T., Xia, P., Rice, T. R., and Blair, W. D. (1993). On the optimality of two-stage state estimation in the presence of random bias. 38(8), 1279-1282. Hmida, F. B., Khemiri, K., Ragot, J. et al. (2012). Three-stage Kalman filter for state and fault estimation of linear stochastic systems with unknown inputs. Journal of the Franklin Institute-Engineering and Applied Mathematics. 394(7), 2369-2388. Hmida, F. B., Khemiri, K., Ragot, J. et al. (2010).Robust filtering for state and fault estimation of linear stochastic systems with unknown disturbance. Mathematical Problems in Engineering .2010(1024-123x), 629-637. Mengli Xiao, Yongbo Zhang, Huimin Fu. (2017) Three-stage Unscented Kalman Filter for State and Fault Estimation of Nonlinear System with Unknown Input. Journal of the Franklin Institute.354(18). Morabito, D. D. (2002). The spacecraft communications blackout problem encountered during passage or entry of planetary atmospheres. Pasadena, California: Jet Propulsion Laboratory, August. Lightsey, E. G., Mogensen A,Burkhart P D,E Ely T A, and Duncan C. (2008). Real-time navigation for Mars mission using the mars networks. Journal of Spacecraft and Rockets,
[23]
[24]
[25]
21
ACCEPTED MANUSCRIPT
AC
CE
PT
ED
M
AN US
CR IP T
45(3), 519-533. [26] Chen, Z. (1991). Local observability and its application to multiple measurement estimation. IEEE Transactions on Industrial Electronics, 38(6), 491-496. [27] Yu, Z., Cui, P., Zhu, S. (2015). Observability-based beacon configuration optimization for Mars entry navigation. Journal of Guidance, Control, and Dynamics.38, 643-649. [28] Li, S., Jiang, X., Liu, Y. (2014).High-precision Mars entry integrated navigation under large uncertainties. Journal of Navigation.67, 327-342.
22
CR IP T
ACCEPTED MANUSCRIPT
ED
M
AN US
Fig.1 Degree of observability for IMU/ integrated navigation
(a)
(b)
AC
CE
PT
Fig.2 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D
Fig.3 Actual and estimated values of the unknown inputs
23
ACCEPTED MANUSCRIPT
(a)
(b)
AN US
CR IP T
(c)
(d)
(e)
(f)
PT
ED
M
Fig.4 Actual and estimated values of the state variables of the vehicle
(a)
(c)
AC
CE
(b)
(d)
(e)
(f)
Fig.5 Performance comparison of UKF and RThSUKF with respect to RMSE
24
ACCEPTED MANUSCRIPT
(a)
CR IP T
(b)
AN US
Fig.6 Actual and estimated values of the uncertain parameters (a) 0 B and (b) L / D
CE
PT
ED
M
Fig.7 Actual and estimated values of the unknown inputs
(a)
(c)
AC
(b)
(d)
(e)
Fig.8 Actual and estimated values of the state variables of vehicle 25
(f)
ACCEPTED MANUSCRIPT
(c)
AN US
(b)
CR IP T
(a)
(d)
(e)
(f)
AC
CE
PT
ED
M
Fig.9 Performance comparison of UKF and RThSUKF with respect to RMSE in state variables
26
ACCEPTED MANUSCRIPT
Table1 Initial state variables and physical parameters of entry vehicle [6] True
Initial estimation
Initial altitude h0 /(km)
125
126
Initial longitude 0 /(deg)
0.00
0.02
Initial latitude 0 /(deg)
1.00
1.02
Initial velocity v0 /(m·s-1)
6900
Initial flight path 0 /(deg)
-12
Initial azimuth 0 /(deg)
89
CR IP T
Initial state conditions
6910 -13
AN US
90
Initial position(km)
Initial velocity(m/s)
Orbiting beacon 1
780.44,13276.13,1267.20
317.7, 50,1697.2
Orbiting beacon 2
3927.85, 230.90,374.91
93.6,3120.8, 584.1
3492.70,3659.20,215.11
605.1,97,3232.8
ED
Beacons type
M
Table2 Initial position and velocity of radio beacons [27]
PT
Orbiting beacon3
CE
Table3 Performance comparison of UKF and RThSUKF with respect to average RMSE Average RMSE
State variables
RThSUKF
UKF
RThSUKF
Position/(m)
526.2147
72.2183
591.6754
78.8354
Longitude/(deg)
0.0012
0.0012
0.0013
0.0012
Latitude/(deg)
0.0062
0.0034
0.0066
0.0035
Velocity/(m·s-1)
4.3596
0.9331
4.8583
1.0163
FPA/(deg)
0.0345
0.0110
0.0516
0.0127
Azimuth/(deg)
0.0318
0.0228
0.0472
0.0242
AC
UKF
27