Radio integrated navigation for Mars powered descent via robust desensitized central difference Kalman filter

Available online at www.sciencedirect.com

ScienceDirect Advances in Space Research 59 (2017) 457–471 www.elsevier.com/locate/asr

MACV/Radio integrated navigation for Mars powered descent via robust desensitized central difference Kalman filter Taishan Lou a, Jie Liu b,⇑, Pan Jin c, Yan Wang a a b

School of Electric and Information Engineering, Zhengzhou University of Light Industry, Zhengzhou 450002, China School of Mechanical and Electrical Engineering, Zhengzhou University of Light Industry, Zhengzhou 450002, China c Zhengzhou Institute of Aeronautical Industry Management, Zhengzhou 450046, China Received 30 April 2016; received in revised form 27 September 2016; accepted 1 October 2016 Available online 11 October 2016

Abstract An innovative integrated navigation scheme based on MCAV/Radio measurement information during Mars powered descent phase, and a robust desensitized central difference Kalman filter (DCDKF) for systems with uncertain parameters or biases are proposed to improve the navigation descent accuracy. Based on the altitude and velocity information of the Miniature Coherent Altimeter and Velocimeter (MCAV), the radio-range information is added into the integrated navigation system to correct the horizontal position error of the vehicle during the Mars powered descent phase. Based the central difference transform, the sensitivity propagation of the state estimate errors in the DCDKF is described, and a designed desensitized cost function is minimized to obtain the gain matrix of the DCDKF. The performances of the innovative navigation scheme and the proposed DCDKF are all demonstrated by two Monte Carlo simulations with the Inertial Measurement Unit biases during the Mars power descent phase. Ó 2016 COSPAR. Published by Elsevier Ltd. All rights reserved.

Keywords: Mars powered descent; Uncertain parameter; Desensitized Kalman filter; Central difference Kalman filter; Sensitivity matrix

1. Introduction Pinpoint landing on a preselected site to continue the search for evidence of life on Mars is a primary mission for the Mars Science Laboratory (MSL). Many new technologies, such as high-precision Entry, Descent and Landing (EDL) navigation technologies, advanced guidance and control techniques, and innovative landing schemes, are required for pinpoint landing on a higher scientific value site due to many uncertainties in dynamical models, atmospheric model, wind drift and map-tie error. The powered descent phase, which starts at parachute cutoff and ignition of the descent engines, is an important phase during EDL, ⇑ Corresponding author.

E-mail addresses: [email protected] (T. Lou), [email protected] (J. Liu). http://dx.doi.org/10.1016/j.asr.2016.10.001 0273-1177/Ó 2016 COSPAR. Published by Elsevier Ltd. All rights reserved.

because the accumulated wind drift of up to several kilometers in the parachute phase must be compensated for during the Mars powered descent phase (Wolf et al., 2004). To achieve the pinpoint landing requirement, advanced navigation, guidance and control techniques must be designed to improve their performance. Further, the accuracy of the vehicle state, coming from the navigation system, will affect the performance of the guidance and control systems. So, providing accurate vehicle state information for the Mars pinpoint landing becomes necessary and stringent. Generally, there are two ways to improve the performance of the navigation system. One is to add new measurements to correct the inertial drift and the vehicle horizontal position under innovative navigation scheme. Li et al. proposed a Miniature Coherent Altimeter and Velocimeter (MCAV) and Inertial Measurement Unit

458

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

(IMU) integrated navigation scheme for the powered descent phase to correct the inertial bias and drift by augmenting them into the state vector (Li et al., 2010). Qin et al. adopted the measurement information of the integrated Doppler radar and the distance and velocity information between the Mars obiter and the vehicle to correct the vehicle horizontal position (Qin et al., 2014). Some other inertial navigation schemes based on the Doppler radar, the onboard light detection and ranging or the vision-aided optical sensors are tested to improve the performance of the spacecraft during the powered descent phase and to support the NASA’s autonomous pinpoint landing project (Janschek et al., 2006; Busnardo et al., 2011; Amzajerdian et al., 2012). Another way is to use new navigation, guidance and control technologies to reduce the negative effects of the model parameter uncertainties or biases under some necessary constraints. In general, the inertial biases coming from the IMU can be augmented into the state vector to estimate with the state, and some good results can be obtained (Li et al., 2010; Qin et al., 2014). Lou et al. considered IMU biases, position biases of the radio beacons and the range biases for the entry vehicle and the onboard satellite during Mars entry by incorporating statistics of the biases into the system formulation, instead of only estimating them (Lou et al., 2015a), and then considered the unobservable uncertain parameters during Mars entry by using consider Kalman filter (Lou et al., 2015b). However, one requirement of augmenting the biases into the state vector or incorporating statistics of the biases is to known the dynamic equation and the statistics of the bias, and this requirement is hard to meet for some conditions during powered descent phase. Some robust and adaptive filtering technologies can be able to solve this problem in the presence of the model biases (Simon, 2006). A desensitized optimal control (DOC) methodology is considered to reduce the sensitivities of state estimate errors respect to the biases or the parameter uncertainties (Shen et al., 2008). Karlgaard and Shen extended the DOC methodology to a robust filter design problem that reducing the performance sensitivity of the filters respected to uncertain model parameters (Karlgaard and Shen, 2013a). The desensitized Kalman filter (DKF) is designed by penalizing the cost function consisting of the posterior covariance trace by using a weighted norm of the state error sensitivities, and minimizing this new cost function to obtain the desensitized state estimates (Karlgaard and Shen, 2013a). Subsequently, the desensitized divided difference filtering (Karlgaard and Shen, 2013b) and the desensitized unscented Kalman filtering (DUKF) (Shen and Karlgaard, 2015) are presented and are applied in motor state estimate problem (Karlgaard and Shen, 2013b) and Mars entry navigation (Li et al., 2014; Lou and Zhao, 2016). Shen and Karlgaard skillfully designed a unique way to propagate the sensitivities of the state estimate error and the priori/posteriori covariance matrices for the DUKF, because a new set of sigma points is always resampled at the next iteration and there is no

continuous propagation for the sensitivities of the sigma points between the iterations (Shen and Karlgaard, 2015). The central difference Kalman filter (CDKF) is considered with the DOC methodology the navigation algorithm in this work (Ito and Xiong, 2000). Based the central difference transform, the CDKF approximates the nonlinear function by using the Stirling’s interpolation formula by replacing the Taylor polynomial formula. This paper proposes an innovative integrated navigation scheme based on MCAV/Radio measurement information during powered descent phase, and then introduces the DOC methodology into the CDKF to propose a robust desensitized CDKF (DCDKF) for systems with uncertain parameters or biases. Section 2 briefly introduces the dynamic model and measurement models during the Mars powered descent phase. An innovative MCAV/Radio measurement integrated navigation scheme (MRMINS) is proposed by introducing the radio range measurement between the vehicle and an orbiting radio satellite or a surface radio beacon. The propagation of the sensitivities of sigma-points respected to the uncertain parameter is introduced and the DCDKF is proposed based on the DOC methodology in Section 3. Section 4 analyzes results of two simulations. One is about two navigation schemes; another is about two filtering algorithms. The conclusions are summarized in Section 5. 2. Mars powered descent navigation system In this section, the dynamic model of the vehicle during the Mars powered descent phase is introduced based on the IMU outputs. The MRMINS is proposed based on two types of measurement information coming from the MCAV and the radio range. 2.1. Dynamic model The Mars PD phase begins when the heat shield separates from the vehicle after discarding the parachute. At this time, the vehicle position from the Mars surface is about kilometers, and the vehicle velocity is about hundred meters per second. Here, the accelerometer, and gyro of the IMU provide the linear acceleration and the angular velocity of the vehicle to construct the kinematics equations of the vehicle, and also the biases of IMU are all brought into the dynamic model. The dynamic equations are represented in the target-fixed coordinate system, which is a local inertial frame with its origin located at the preselected landing target, z-axis pointed to the zenith, y-axis pointed to the north, and x-axis pointed to the east. The powered descent dynamic equations of the Mars vehicle are given by Li and Liu (2009) and Li et al. (2010) r_ ¼ v v_ ¼ C lb ð~a  ba  na Þ þ g  2xM  v  xM  ðxM  rÞ _ ¼ KðXÞðx ~  bx  nx Þ X

ð1Þ

T. Lou et al. / Advances in Space Research 59 (2017) 457–471 T

where r ¼ ½rx ; ry ; rz  is the vehicle position vector, T T v ¼ ½vx ; vy ; vz  is the vehicle velocity vector, X ¼ ½/; h; w is the triaxial Euler angle vector, in which /; h; w are the triaxial attitude angles. ~ a is the accelerometer output along ~ is the gyro output body axes, ba is the acceleration bias, x along body axes, bx is the angular rate bias, na and nx are the zero-mean white Gaussian noises. g is the gravitational acceleration, xM is the Mars rotation angular speed, 1

C lb ¼ ðC bl Þ is the coordinate transformation matrix from the body coordinate system to the target-fixed coordinate system. The matrix C bl is given by 2

1

0

6 C bl ¼ 4 0 cos /

0

32

cos h 0  sin h

76 sin / 54 0

0  sin / cos /

32

cos w

sinw 0

3

76 7 54  sinw cos w 0 5 sinh 0 cos h 0 0 1 1

0

ð2Þ

Matrix KðXÞ is defined as follows 2 3 1 tan h sin / tan h cos / 6 7 KðXÞ ¼ 4 0 cos /  sin / 5 0

sec h sin /

ð3Þ

sec h cos /

2.2. Measurement model In order to correct the IMU biases and the horizontal position, an innovative integrated navigation scheme, in which there are the altitude and velocity information from the MCAV and the distance information between the vehicle and one Mars orbiting radio satellite or predeployed surface radio beacon. The MCAV/Radio measurement integrated navigation scheme with the target-fixed coordinate system for Mars powered descent phase is shown in Fig. 1. 2.2.1. Miniature coherent altimeter and velocimeter The MCAV developed by the Jet Propulsion Laboratory is an advanced sensor to meet the engineering needs

459

of future Mars vehicle (Chu et al., 2005; Li et al., 2010). The MCAV provides the high-precision altitude and three triaxial line-of-sight velocities by using laser. The outputs of the MCAV are given by yMCAV ¼ ½ rz

ð4Þ

T

vc 

where vc is the triaxial velocity vector in the body coordinate system, and satisfies  T ð5Þ vc ¼ vcx vcy vcz ¼ C bl v

2.2.2. Radio-range measurement The radio communication between the vehicle and an orbiting radio satellite or a surface radio beacon (such as the previous landers on the Mars) within sight will measure the distance between the vehicle and a Mars radio satellite or a surface radio beacon, which is called the two-way range measurement (Boehmer, 1999). The two-way range measurement is reconstructed by qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T Rk ¼ ðr  rk Þ ðr  rk Þ þ fR ð6Þ where rk is the position vector of the orbiting radio satellite (k ¼ o) or the surface radio beacon (k ¼ b); and fR is the range noise with zero-mean white Gaussian noise. 2.3. Integrated navigation system model based on the MCAV/Radio measurement During Mars PD phase, the state vector of the vehicle is T

defined as x ¼ ½rT ; vT ; XT  , and the dynamic model is reconstructed as follows 2

3 v 6 7 x_ ¼ f ðx;c;wÞ ¼ 4 C lb ð~ a  ba  na Þ þ g  2xM  v  xM  ðxM  rÞ 5 ~  bx  nx Þ KðXÞðx

Fig. 1. Diagram of MCAV/Radio measurement integrated navigation.

ð7Þ

460

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

Table 1 Sensitivity propagation algorithm of sigma-points. Time sensitivity update (1) Compute the sensitivities sigma-points at step k  1 ði ¼ 1; 2; . . . ; ‘Þ

8 @vþ 0;k1 > ¼ sþ > i;k1 > @ci > > > < þ pffiffiffiffiffiffiffi @vj;k1 @ Pþ þ k1 ¼ s þ h nj ; j ¼ 1; 2; . . . ; 2n i;k1 @ci @ci > > > > p ffiffiffiffiffiffiffi > @vþ > @ Pþ : i;j;k1 k1 ¼ sþ ðei þ ej Þ; 1 6 i < j 6 n i;k1 þ h @ci @ci

ð20Þ

(2) Propagate the sensitivity of the sigma-points

8 @v @f ðvþ ;c;u Þ k1 j;k1 > < @cj;ki ¼ @vþ

@vþ j;k1 @ci

j;k1

> :

@v i;j;k @ci

¼

@f ðvþ ;c;uk1 Þ i;j;k1 @vþ i;j;k1

þ

@vþ i;j;k1 @ci

@f ðvþ ;c;uk1 Þ j;k1 @ci

þ

;

@f ðvþ ;c;uk1 Þ i;j;k1 @ci

j ¼ 0; 1; 2; . . . ; 2n ;

16i
ð21Þ

(3) Compute the sensitivities of the a prior state estimate

s i;k ¼

 2n X @v @~ x j;k ðmÞ @v0;k k ¼ x0 þ xðmÞ @ci @ci @ci j¼1

ð22Þ

(4) Compute the sensitivities of the a prior covariance

(    T ) n n n o X X @v @v @vj;k @v @P j;k nþj;k nþj;k ð1Þ ð1ÞT T ðc1 Þ     k ðvj;k  vnþj;k Þ þ ðvj;k  vnþj;k Þ ¼x   lj;k þ lj;k þ xðc2 Þ @ci @ci @ci @ci @ci j¼1 j¼1 þ 2xðc2 Þ

n n n o X X ð1Þ ð1ÞT lði1;j1;i2;j2Þ;k þ lði2;j2;i1;j1Þ;k

ð23Þ

i2;j2¼1 i1;j1¼1 i2
ð1Þ

lj;k

where    @vj;k @v @v nþj;k 0;k   T ðv ¼ þ 2 j;k þ vnþj;k  2v0;k Þ @ci @ci @ci    @vi1;j1;k @v @v @v nþj1;k nþi1;k 0;k ð1Þ    T ðv   þ lði1;j1;i2;j2Þ;k ¼ i2;j2;k  vnþi2;k  vnþj2;k þ v0;k Þ @ci @ci @ci @ci

ð24Þ ð25Þ

Measurement sensitivity update (5) Compute the sensitivities of the redrawn sigma-points

8 @v 0;k  > > > @ci ¼ si;k > > > pffiffiffiffiffi <  @vj;k @ P k  ¼ s þ h nj ; j ¼ 1; 2; . . . ; 2n i;k @ci @ci > > > > p ffiffiffiffi ffi > > : @vi;j;k ¼ s þ h @ Pk ðe þ e Þ; 1 6 i < j 6 n i j i;k @ci @ci

ð26Þ

(6) Compute the sensitivity of the a prior measurement sigma-points

8  @hðv ;c;uk Þ @Z j;k j;k > > < @ci ¼ @vj;k

@v j;k

 > @hðv ;c;uk Þ > i;j;k : @Z i;j;k ¼ @ci @v i;j;k

@ci

þ

@v i;j;k @ci

@hðv ;c;uk Þ j;k @ci

þ

;

@hðv ;c;uk Þ i;j;k @ci

j ¼ 0; 1; 2; . . . ; 2n ð27Þ ;

16i
(7) Compute the sensitivities of the a prior measurement

ci;k ¼

 2n X @Z  @~z j;k ðmÞ @Z 0;k k ¼ x0 þ xðmÞ @ci @ci @ci j¼1

ð28Þ (continued on next page)

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

461

Table 1 (continued) (8) Compute the sensitivities of the innovation covariance

(     T ) n n n o X X @Z j;k @Z  @Z j;k @Z  @P nþj;k nþj;k zz;k ð2Þ ð2ÞT T    ¼ xðc1 Þ   Z Þ þ ðZ  Z Þ  lj;k þ lj;k þ xðc2 Þ ðZ  j;k nþj;k j;k nþj;k @ci @ci @ci @ci @ci j¼1 j¼1 n n n X X

þ 2xðc2 Þ

ð2Þ

ð2ÞT

lði1;j1;i2;j2Þ;k þ lði2;j2;i1;j1Þ;k

o

ð29Þ

i2;j2¼1 i1;j1¼1 i2
where ð2Þ

lj;k ¼

   @Z j;k @Z  @Z  nþj;k 0;k   T ðZ  þ 2 j;k þ Z nþj;k  2Z 0;k Þ @ci @ci @ci

ð2Þ

lði1;j1;i2;j2Þ;k ¼

ð30Þ

   @Z i1;j1;k @Z  @Z  @Z  nþj1;k nþi1;k 0;k    T ðZ    þ i2;j2;k  Z nþi2;k  Z nþj2;k þ Z 0;k Þ @ci @ci @ci @ci

ð31Þ

(9) Compute the sensitivities of the cross-covariance

( pffiffiffiffiffiffi   T ) n pffiffiffiffiffiffiffiffiffiX pffiffiffiffiffiffi @Z j;k @Z  @P @ P nþj;k xz;k k    ðc Þ ¼ x 1 nj ðZ j;k  Z nþj;k Þ þ Pk nj  @ci @ci @ci @ci j¼1

ð32Þ

(10) Compute the sensitivities of the a posterior state estimate  sþ i;k ¼ si;k  K k ci;k

ð33Þ

(11) Compute the sensitivities of the a posterior covariance matrix 

T



@Pxz;k T @Pxz;k @Pzz;k T @Pþ @P k k ¼  Kk  Kk þ Kk Kk @ci @ci @ci @ci @ci

ð34Þ

Table 2 DCDKF algorithm. Initialization ^0 , and the auxiliary matrix, P0 , and the sensitivity parameters, s0 and @P0 =@ci ; i ¼ 1; 2; . . . ; ‘. (1) Set h P 1; Initialize the state vector, x Time update (2) Factorize the covariance at time tk1 using Eq. (A.10). (3) Compute sigma-points using Eq. (A.11), and the sensitivity using Eq. (20). (4) Compute the propagated sigma-points using Eq. (A.12), and the sensitivity using Eq. (21).  ^ (5) Estimate the a priori state, x k , using Eq. (A.13), and the a prior sensitivity, si;k , using Eq. (22). (6) Estimate the a priori covariance matrix, P , using Eq. (A.14), and the sensitivity, @P k k =@ci , using Eq. (23). Measurement update (7) Factorize the a priori covariance using Eq. (A.15). (8) Redrawn the sigma-points using Eq. (A.16), and the sensitivity using Eq. (26). (9) Propagate the sigma-points of measurement using Eq. (A.17), and the sensitivity using Eq. (27). (10) Estimate the a priori measurement, ^z k , using Eq. (A.18), and the sensitivity, ci;k , using Eq. (28).  (11) Estimate the innovation covariance, P zz;k , using Eq. (A.19), and the sensitivity, @Pzz;k =@ci , using Eq. (29).   (12) Estimate the cross-covariance, Pxz;k , using Eq. (A.20), and the sensitivity, @Pxz;k =@ci , using Eq. (32). (13) Solve the gain matrix, K k ,using Eq. (19). þ ^þ (14) Estimate the a posterior state, x k , using Eq. (A.22), and the a posterior sensitivity, si;k , using Eq. (33). þ (15) Estimate the a posterior covariance, Pþ , using Eq. (A.23), and the sensitivity, @P =@c i , using Eq. (34). k k

T

where c ¼ ½ba ; bx  is the IMU biases, and w is the noise. The measurement model with a MCAV, one orbiting radio satellite and one surface radio beacon is assumed to be modeled by

 z ¼ hðxÞ þ v ¼ rz

vcT

Ro

Rb

T

þv

ð8Þ

where v is the sensor noise with zero-mean white Gaussian process.

462

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

Fig. 2. MATLAB /Simulink block diagram.

Table 3 Initial conditions for integrated navigation. Parameters

True

Estimated

(210, 530, 2300) (4, 14, 85) (0.2618, 0.1745, 0.1396)

Position r (m) Velocity v (m/s) Attitude angle X (rad)

Table 4 Acceleration and gyro error specifications for IMU. Bias Acceleration x-axis y-axis z-axis

Noise (std.)

Bias noise (std.)

1.0  103 1.0  103 1.0  103

1.0  108 1.0  108 1.0  108

1.0  103 1.0  103 1.0  103

1.0  108 1.0  108 1.0  108

2

error (m/s ) 0.3cos(t/100) 0.3cos(t/200) 0.3sin(t/100)

Gyro error (rad/s) x-axis 0.0349sin(t/100) y-axis 0.0349sin(t/200) z-axis 0.0349cos(t/100)

In this section, a robust DCDKF is presented by using the DOC technology and the CDKF method in Appendix A. Here, we consider a discrete-time nonlinear system models with additive noises given by ð9Þ

zk ¼ hðxk ; c; uk Þ þ vk

ð10Þ

where xk is the n dimension state vector, and zk is the m dimension measurement vector. f ðÞ is the nonlinear dynamic function, hðÞ is the nonlinear measurement function. c is referred to as the ‘ dimension uncertain parameter vector. uk is the known control input vector. wk and vk are independent zero-mean Gaussian noise processes with the covariance Qk and Rk , respectively, and they satisfy E½wk wTj  ¼ Qk dij ;

E½vk vTj  ¼ Rk dij ;

E½wk vTj  ¼ 0

where dkj is the Kronecker delta function.

Standard deviation (std)

(210, 530, 2300) (4, 14, 85) (0.2618, 0.1745, 0.1396)

(20, 50, 200) (1, 3, 10) (0.005, 0.005, 0.005)

Under the fundamental assumptions of the Kalman filter, which includes no model and parameter uncertainties, and no color noises, the state estimates are unbiased. It implies that the optimal estimation errors of the Kalman filter satisfy E½~ x k  ¼ 0;

3. Desensitized central difference Kalman filter

xk ¼ f ðxk1 ; c; uk1 Þ þ wk1

Mean

ð11Þ

E½~ xþ k  ¼ 0

ð12Þ

^ ^ ~ where x k ¼ x k  xk is the a priori estimation error and x k þ þ ~k ¼ x ^k  xk is the a posteriori estiis the a prior estimate, x ^þ mation error and x k is the a posteriori estimate. The subscript symbols ‘‘” and ‘‘+” denote the a priori and the a posteriori, respectively. However, the state estimate may be biased or even divergence when the parameter has uncertainty. It is to say that the state estimate error is sensitive to the uncertainty of the parameter. So, we can define the state estimate error sensitivities respected to the parameter, ci , as (Tapley et al., 2004; Karlgaard and Shen, 2013a) s i;k ¼

@~ x k @ci

ð13Þ

sþ i;k ¼

@~ xþ k @ci

ð14Þ

where ci is the ith component of the parameter c. For the CDKF in Appendix A, the propagation equations of the sensitivity respected to the uncertain parameter are s i;k ¼

 2n X @v @^ x j;k ðmÞ @v0;k k ¼ x0 þ xðmÞ @ci @ci @c i j¼1

ð15Þ

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

463

Fig. 3. State estimate RMSEs of the DCDKF under MNS and MRMINS.

Fig. 4. Total position RMSEs of the DCDKF under MNS and MRMINS.

sþ i;k

@^ xþ ¼ k ¼ s i;k  K k ci;k @ci

ð16Þ

where the sensitivity of the measurement function respected to the uncertain parameter is given by ci;k ¼

 2n X @Z  @^z j;k ðmÞ @Z 0;k k ¼ x0 þ xðmÞ @ci @ci @c i j¼1

ð17Þ

Fig. 5. Total velocity RMSEs of the DCDKF under MNS and MRMINS.

We assume that @K=@ci ¼ 0 in Eq. (16), because any @K=@ci – 0 implies that the solution for the optimal gain is a function of the residual, which violates the basis for the linear update equation given in Eq. (A.23) in Appendix A (Karlgaard and Shen, 2013a; Shen and Karlgaard, 2015). For the KF, the Kalman optimal gain K k is obtained by minimizing the cost function J KF ¼ TrðPþ k Þ (Crassidis and Junkins, 2012). Here, the sensitivities of the state estimate error respected to the uncertain parameters are considered, and a penalty function with a weighted norm of the a

464

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

posterior sensitivity is added to penalize the cost function. A new desensitized cost function is defined as J d ¼ TrðPþ k Þþ

‘ X þ sþT i;k W i;k si;k

ð18Þ

i¼1

Substituting Eq. (A.23) in Appendix A and the sensitivities of the posterior state estimate in Eq. (16), taking partial derivative of J d with respected to the gain matrix K k , and then setting the partial derivative @J d =@K k ¼ 0 gives K k P zz;k

‘ ‘ X X T þ W i;k K k ci;k cTi;k ¼ P þ W i;k s xz;k i;k ci;k i¼1

ð19Þ

i¼1

Note that the gain K k is obtained by algebraically solving the linear equation in Eq. (19). In Eqs. (15) and (17), the sensitivities of the sigma-points respected to the uncertain parameter are not given yet. Because the sigma-points of the CDKF are always redrawn step by step in filtering process, so the sensitivities of the sigma-points respected to the uncertain parameter must be recomputed in each step, and there is no transitive property from a step to the next step. In the next subsection, the propagation of the sensitivity respected to the uncertain parameter will be introduced as in the literature (Shen and Karlgaard, 2015).

In Eqs. (20) and (26), the sensitivity of the root square pffiffiffiffi matrix P should be solved before the sensitivities of the sigma-points are calculated. Taking partial derivative of pffiffiffiffi pffiffiffiffi T equation P ¼ Pð PÞ respected to the uncertain parameter ci , gives pffiffiffiffi pffiffiffiffi!T @P @ P pffiffiffiffiT pffiffiffiffi @ P P þ P ¼ ð35Þ @ci @ci @ci Solving Eq. (35) gives the solution (Hodges, 1957) (   )1 pffiffiffiffi T @ P @P 1 1 T W ¼W W C HT ð36Þ @ci 2 @ci where C is an arbitrary n  n skew symmetric matrix with equation CT ¼ C, W and H are non-singular matrix such pffiffiffiffi that W PH ¼ I. 3.2. DCDKF algorithm Based on the desensitized technology and the sensitivity propagation equations of the sigma-points in the above subsections, the complete DCDKF algorithm is summarized in Table 2. 4. Simulation results

3.1. Sensitivity propagation of sigma-points By taking partial derivative of the sigma-point generated equation, such as Eq. (A.1) in Appendix A, the sensitivity of sigma-points respected to the uncertain parameter will be obtained. Then, the sensitivity propagation equations can be obtained by taking partial derivative of the propagation equations of the CDKF, such as Eqs. (A.12) and (A.17) in Appendix A. Finally, the sensitivity propagation algorithm of the sigma-points is summarized in Table 1.

Fig. 6. Landing footprints and 95% confidence error ellipses under MNS.

To analyze the performance of the proposed DCDKF algorithm and the validity of the MRMINS during the Mars powered descent phase, two MATLAB/Simulink simulations have been carried out and the MATLAB/ Simulink block is in Fig. 2. One is comparison of the navigation schemes with MRMINS and MCAV navigation scheme (MNS); another is comparison of the CDKF and the proposed DCDKF under the MRMINS.

Fig. 7. Landing footprints and 95% confidence error ellipses under MRMINS.

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

465

Fig. 8. State estimate errors and 3r bounds of the CDKF under MRMINS.

Fig. 9. State estimate errors and 3r bounds of the DCDKF under MRMINS.

The simulation conditions and parameters, in which the truth and the statistics of the initial values are all listed, are summarized in Table 3. The landing simulation is terminated roughly after 60 s when the next phase starts condition (in this simulation the altitude is 18.6 m, the vertical

descent velocity is 0.75 m/s and horizontal velocity is 0 m/s). In this work, the landing target is assumed to locate at (3,396,000 m, 0 m, 0 m) in the Mars inertial frame, whose longitude and latitude are all zeros. The position of the

466

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

Fig. 10. Mean cost functions of the CDKF and the DCDKF under MRMINS.

Fig. 12. Total position RMSEs of the CDKF and the DCDKF under MRMINS.

surface beacon is assumed as (3,396,000 m, 877,800 m, 177,700 m) and its velocity is assumed to zero in the Mars inertial frame. The initial position of orbiting beacon is optimally preselected based on the observability analysis (Qin et al., 2014). The Mars gravity acceleration is assumed to be a constant g ¼ ½0; 0; 3:69T m=s2 , and the Mars rotation angular speed is assumed to be xM ¼ 7:0882  105 rad=s. The acceleration and gyro error specifications for IMU are listed in Table 4. The reference

acceleration bias and gyro bias of the IMU are assumed to ba ¼ ½0:3 m=s2 ; 0:3 m=s2 ; 0T and respectively be bx ¼ ½0; 0; 0:0349 rad=sT . The altitude error of the MCAV is 0.5 m, and the velocity errors are all 0.03 m/s. For the radio range measurement, the range errors are 50 m. The sensitivity-weighting matrix in the proposed DCDKF is set as W 1 ¼ 1:2  103 I, W 2 ¼ 4:06  106 I. 10,000 simulations have run, and the root mean squared error (RMSE) is averaged, landing footprints and error ellipses with 95% confidence level are computed.

Fig. 11. State estimate RMSEs of the CDKF and the DCDKF under MRMINS.

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

467

In this section, only the MRMINS is considered in simulation. The CDKF is employed to compare with the performance of the proposed DCDKF algorithm. In the CDKF the true value of the uncertain parameter c is unknown, and only the reference value c, coming from the previous experience, is known.

Figs. 8 and 9 show the state estimate errors and 3r bounds of the CDKF and the DCDKF under MRMINS. Fig. 8 shows that the state errors of the CDKF are not captured by their 3r covariance bounds when the biases coming from the IMU are not considered in the navigation algorithm. Fig. 9 shows that all six state errors of the DCDKF are captured by their 3r covariance bounds. Fig. 10 shows the mean cost functions of two filters with logarithmic scales. Here, the cost function of the CDKF and the DCDKF is computed as J d in Eq. (18). From Fig. 10, it can be seen that the mean cost function of the DCDKF is smaller than that of the CDKF. Compared with the CDKF, the DCDKF reduces the sensitivities of state estimate errors respect to the biases, and minimizes the corresponding cost function. Fig. 11 shows that the state estimate RMSEs of the CDKF and the DCDKF with logarithmic scales under the MRMINS in 10,000 times simulations when the IMU biases are applied to uniform distributions. Figs. 12 and 13 show the total position estimate RMSE and the total velocity estimate RMSE of the vehicle with logarithmic scales under the MRMINS. It can be seen that the RMSEs of the proposed DCDKF smaller than that of the CDKF. When the IMU biases are present, the CDKF brings worse estimates than the DCDKF. The proposed DCDKF is able to reduce the sensitivities of the state estimate errors to the IMU biases, and provides more accuracy state estimate than the CDKF. Fig. 14 shows the landing footprints and 95% confidence error ellipses of the CDKF under the MRMINS. Compared with the landing accuracy of the DCDKF in Fig. 7, the CDKF in Fig. 14 has a low landing accuracy under the MRMINS. This is because the IMU biases are not corrected and propagate in the navigation process, and greatly affect the landing accuracy of the CDKF.

Fig. 13. Total velocity RMSEs of the CDKF and the DCDKF under MRMINS.

Fig. 14. Landing footprints and 95% confidence error ellipses of the CDKF under MRMINS.

4.1. Comparison of the MNS and the MRMINS To verify the performance of the MRMINS, the proposed DCDKF is selected as the navigation algorithm, and the MNS is set as a comparison. In simulation, the measurement model of the MNS is given by  T ð39Þ z ¼ hM ðxÞ þ v ¼ rz vcT þ v Fig. 3 shows that the RMSEs of the DCDKF under two navigation schemes and Figs. 4 and 5 show the total position error and the total velocity error of the vehicle under two navigation schemes. From Figs. 3–5, the RMSEs of the MRMINS are smaller than that of the MNS. Especially, it can be seen that the RMSEs of xaxis and y-axis positions have constant biases under the MNS, because the MCAV only provides the altitude information of the vehicle and the vehicle velocity, without the horizontal position information. Figs. 6 and 7 show the landing footprints and 95% confidence error ellipses under the MNS and the MRMINS, respectively. From Figs. 6 and 7, it can be seen that the landing accuracy under the MNS is much lower than that of the MRMINS. The MRMINS provides two range measurements, which contain the horizontal position information of the vehicle, to improve the navigation accuracy as in Fig. 3. 4.2. Comparison of the CDKF and the DCDKF

468

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

Table A.1 CDKF algorithm. Dynamic and measurement equations



xk ¼ f ðxk1 ; uk1 Þ þ wk1 zk ¼ hðxk ; uk Þ þ vk

ðA:9Þ

Initialization ^0 , and the auxiliary matrix, P0 . (1) Set h P 1; Initialize the state vector, x Time update (2) Factorize the covariance at time tk1

Pþ k1

qffiffiffiffiffiffiffiffiffiffiqffiffiffiffiffiffiffiffiffiffiT ¼ Pþ Pþ k1 k1

ðA:10Þ

þ ^þ (3) Compute the sigma-points based on the state estimate x k1 and covariance P k1 , as

8 þ ^þ v ¼x > k1 > < 0;k1 pffiffiffiffiffiffiffiffiffi ffi þ ^ vþ ¼ x Pþ j ¼ 1; 2; . . . ; 2n j;k1 k1 þ h k1 nj ; > pffiffiffiffiffiffiffiffiffi ffi > : vþ þ þ ^k1 þ h Pk1 ðei þ ej Þ; 1 6 i < j 6 n i;j;k1 ¼ x

ðA:11Þ

(4) Compute the propagated sigma-points through the nonlinear function as

(

þ v j ¼ 0; 1; 2; . . . ; 2n j;k ¼ f ðvj;k1 ; uk1 Þ;  vi;j;k ¼ f ðvþ 16i
ðA:12Þ

^ (5) Estimate the a priori state, x k , as ðmÞ  ðmÞ ^ x k ¼ x0 v0;k þ x

2n X v j;k

ðA:13Þ

j¼1

(6) Estimate the a priori covariance matrix, P k , as ðc1 Þ P k ¼ x

n n n n X X X X T    ðc2 Þ      T ðc2 Þ ðv ðv ðv j;k  vnþj;k Þðvj;k  vnþj;k Þ þ x j;k þ vnþj;k  2v0;k Þðvj;k þ vnþj;k  2v0;k Þ þ 2x i1;j1;k j¼1

j¼1

      T  v nþi1;k  vnþj1;k þ v0;k Þðvi2;j2;k  vnþi2;k  vnþj2;k þ v0;k Þ þ Q k1

i2;j2¼1 i1;j1¼1 i2
ðA:14Þ

Measurement update (7) Factorize the a priori covariance

P k ¼

pffiffiffiffiffiffi pffiffiffiffiffiffi

T P P k k

ðA:15Þ

(8) Redrawn the sigma-points as

8  ^ v ¼x k > < 0;k pffiffiffiffiffiffi ^ v P j ¼ 1; 2; . . . ; 2n k nj ; j;k ¼ x k þh > p ffiffiffiffiffiffi :   ^ ðe þ h P þ ej Þ; 1 6 i < j 6 n vi;j;k ¼ x i k k

ðA:16Þ

(9) Propagate the sigma-points of measurement as

(

 Z j ¼ 0; 1; 2; . . . ; 2n j;k ¼ hðvj;k ; uk Þ;  Z i;j;k ¼ hðv ; u Þ; 16i
ðA:17Þ

(10) Estimate the a priori measurement, ^z k , as ðmÞ  ðmÞ ^z k ¼ x0 Z 0;k þ x

2n X Z j;k

ðA:18Þ

j¼1

(continued on next page)

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

469

Table A.1 (continued) (11) Estimate the innovation covariance matrix, P zz;k , as ðc1 Þ P zz;k ¼ x

n n X X T         T ðc2 Þ ðZ  ðZ  j;k  Z nþj;k ÞðZ j;k  Z nþj;k Þ þ x j;k þ Z nþj;k  2Z 0;k ÞðZ j;k þ Z nþj;k  2Z 0;k Þ j¼1

þ 2xðc2 Þ

j¼1 n n X X

       T ðZ  i1;j1;k  Z nþi1;k  Z nþj1;k þ Z 0;k ÞðZ i2;j2;k  Z nþi2;k  Z nþj2;k þ Z 0;k Þ þ Rk

ðA:19Þ

i2;j2¼1 i1;j1¼1 i2
(12) Estimate the cross-covariance, P xz;k , as

P xz;k ¼

n pffiffiffiffiffiffi pffiffiffiffiffiffiffiffiffiX T   xðc1 Þ P k nj ðZ j;k  Z nþj;k Þ

ðA:20Þ

j¼1

(13) Obtain the gain matrix, K k , as  1 K k ¼ P xz;k ðP zz;k Þ

ðA:21Þ

^þ (14) Estimate the a posterior state, x k , as

^þ ^ x z k ¼ x k þ K k ðzk  ^ k Þ

ðA:22Þ

(15) Estimate the a posterior covariance, Pþ k , as   T T  T Pþ k ¼ Pk  P xz;k K k  K k Pxz;k þ K k P zz;k K k

These results demonstrate that the DCDKF is able to reduce the sensitivities of state estimate errors respect to the biases, and have a better performance to estimate the vehicle state and improve the navigation accuracy during the Mars powered descent phase than the CDKF. 5. Conclusions In this paper, an innovative integrated navigation scheme based on MCAV/Radio measurement information during Mars powered descent phase is proposed. The IMU outputs are adopted to construct the kinematics equations of the vehicle. Only the altitude and velocity information of the MCAV cannot correct the horizontal position of the vehicle. The radio-range information, which includes the vehicle horizontal position, is introduced to improve the navigation accuracy during the powered descent phase. Two radio-range measurements are the distances between the descent vehicle and an orbiting radio satellite and a surface radio beacon within sight, respectively. The desensitized optimal control methodology is introduced into the CDKF to propose a robust DCDKF for systems with uncertain parameters or biases. Based on the cost function of the CDKF, a desensitized cost function of the DCDKF is designed by penalizing the trace of posterior covariance using a sensitivity-weighting sum of the posteriori sensitivities. The corresponding gain matrix of the DCDKF is obtained by minimizing the above desensitized cost function. Based the central difference transform, the sensitivity propagation of the state estimate errors in the DCDKF is described.

ðA:23Þ

Finally, the simulation results of the two navigation schemes imply that the MRMINS corrects the horizontal position errors and greatly improves the navigation accuracy during Mars powered descent phase. From the comparison of the CDKF and the DCDKF in Monte Carlo simulation, the DCDKF is able to reduce the sensitivities of state estimate errors respect to the biases, and have a better performance to estimate the vehicle state than the CDKF and improve the navigation accuracy during the Mars powered descent phase. Acknowledgements The work described in this paper was supported by the National Science Foundation of China (Grant Nos. 61603346, 61603347) and the Doctor Startup Funds Foundation of Zhengzhou University of Light Industry (Grant No. 2015BSJJ026). The author fully appreciates the financial supports. Appendix A. Central difference Kalman filter Based the central difference transform, the CDKF approximates the nonlinear function by using the Stirling’s interpolation formula (Ito and Xiong, 2000). Compared with the EKF, it doesn’t need to compute the Jacobi matrix, and only need some finite sigma-points to capture the characteristics of the nonlinear function. Assume the  and covariance n dimension state vector x has the mean x Px , these sigma-points will be generated as follows:

470

T. Lou et al. / Advances in Space Research 59 (2017) 457–471

8  > < v0 ¼ x pffiffiffiffiffi  þ h Px nj ; j ¼ 1; 2; . . . ; 2n vj ¼ x > pffiffiffiffiffi :  þ h Px ðei þ ej Þ; 1 6 i < j 6 n vi;j ¼ x

ðA:1Þ

where nj is the jth element of n sigma-points, and its value comes from the following set fe1 ; e2 ; . . . ; en ; e1 ; e2 ; . . . ; en g

ðA:2Þ

where ej , j ¼ 1; 2; . . . ; n is the unit vector, in which the jth element is one and others are zeros, T pffiffiffiffiffi ej ¼ ½0; . . . ; 0; 1; 0; . . . ; 0 . Px is the Cholesky factor of pffiffiffiffiffi pffiffiffiffiffi T the covariance Px , and satisfies Px ¼ Px Px . h denotes a selected interval length, and it’s optimal value is the Kurtosis of the distribution (Norgaard et al., 2000). For the pffiffiffi Gaussian distribution, it’s optimal value is h ¼ 3. The weights of the above sigma-points is given by

P ¼

n X

f 0CD;j ð xÞf 0CD;j ð x ÞT þ

j¼1

 can be and the second-order derivative of f ðxÞ at x ¼ x approximated by a second-order central difference pffiffiffiffiffi

pffiffiffiffiffi

 þ h P x ej þ f x   h Px ej  2f ð f x xÞ 00 xÞ ¼ ; f CD;jj ð 2 h j ¼ 1; 2; . . . ; n ðA:5Þ f 00CD;ij ð xÞ ¼



pffiffiffiffiffi pffiffiffiffiffi

pffiffiffiffiffi

  h Px ej þ f ð   h Px ðei þ ej Þ  f x   h P x ei  f x xÞ f x h2

16i
;

ðA:6Þ

By using the above central difference approximations and similarly to the extended Kalman filter, the multidimensional state prediction is given as (Wu et al., 2006; Wang et al., 2012; Haug, 2012) n n   X pffiffiffiffiffi  pffiffiffiffiffi o ðmÞ ^ ¼ xðmÞ   x f ð x Þ þ x f x þ h P e þ f x  h P x ej x j 0 j¼1

ðA:7Þ The expression for the multidimensional predictive state error covariance is

n n X n X 1X f 00CD;jj ð xÞf 00CD;jj ð xÞT þ f 00CD;ij ð xÞf 00CD;kl ð xÞT 2 j¼1 k;l¼1 i;j¼1 k
i
n n    X pffiffiffiffiffi  pffiffiffiffiffi on  pffiffiffiffiffi  pffiffiffiffiffi oT  þ h P x ej  f x   h P x ej  þ h P x ej  f x   h P x ej ¼ xðc1 Þ f x f x j¼1

þ xðc2 Þ

n n   on   oT X pffiffiffiffiffi  pffiffiffiffiffi  pffiffiffiffiffi  pffiffiffiffiffi   þ h P x ej þ f x   h Px ej  2f ð  þ h P x ej þ f x   h Px ej  2f ð f x xÞ f x xÞ j¼1

þ 2xðc2 Þ

n X n n X k;l¼1 k
f ð xh

  o pffiffiffiffiffi pffiffiffiffiffi  pffiffiffiffiffi    h P x ei  f x   h Px ej þ f ð Px ðei þ ej ÞÞ  f x xÞ

i;j¼1 i
 n    oT pffiffiffiffiffi pffiffiffiffiffi  pffiffiffiffiffi    h P x ek  f x   h Px ðek þ el Þ  f x   h Px el þ f ð  f x xÞ

8 ðmÞ h2 n > > > x0 ¼ h2 > > < xðmÞ ¼ 1 2h2 ðc1 Þ > x ¼ 4h12 > > > > : xðc2 Þ ¼ 1

ðA:3Þ

2h4

Now, we give some approximation formula of the CDKF. For the multidimensional nonlinear function  can be f ðxÞ, the first-order derivative of f ðxÞ at x ¼ x approximated by an first-order central difference pffiffiffiffiffi

pffiffiffiffiffi

 þ h P x ej  f x   h P x ej f x 0 f CD;j ð ; j ¼ 1; 2; . . . ; n xÞ ¼ 2h ðA:4Þ

ðA:8Þ

Based on the above central difference transform, the estimate of mean and covariance is derived, and the CDKF algorithm with the system model (A.1) is given in Table A.1 (Norgaard et al., 2000; Ito and Xiong, 2000; Haug, 2012). References Amzajerdian, F., Petway, L., Hines, G. et al., 2012. Doppler lidar sensor for precision landing on the Moon and Mars. In: 2012 IEEE Aerospace Conference, Big Sky, MT, pp. 1–7. Boehmer, R.A., 1999. Navigation Analysis and Design for Mars Entry (Doctoral dissertation). Massachusetts Institute of Technology, Massachusetts. Busnardo, D.M., Aitken, M.L.., Tolson, R.H., et al., 2011. Lidar-aided inertial navigation with extended Kalman filtering for pinpoint landing

T. Lou et al. / Advances in Space Research 59 (2017) 457–471 over rough terrain. In: 49th AIAA Aerospace Sciences Meeting including the New Horizons Forum and Aerospace Exposition, AIAA 2011-428, Orlando, Florida. Chu, C., Hayati, S.A., Udomkesmalee, S., et al., 2005. Mars base technology program overview. In: Space 2005, AIAA 2005-6744, Long Beach, California. Crassidis, J.L., Junkins, J.L., 2012. Optimal Estimation of Dynamic Systems, second ed. Chapman & Hall/CRC Press, Boca Raton, FL, pp. 231–238. Haug, A.J., 2012. Bayesian Estimation and Tracking: A Practical Guide. John Wiley & Sons, New Jersey. Hodges, J.H., 1957. Some matrix equations over a finite field. Annali di Matematica Pura ed Applicata, Series 4 (44), 245–250. Ito, K., Xiong, K., 2000. Gaussian filters for nonlinear filtering problems. IEEE Trans. Autom. Control 45, 910–927. Janschek, K., Tchernykh, V., Beck, M., 2006. Performance analysis for visual planetary landing navigation using optical flow and DEM matching. In: Proceedings of the AIAA Guidance, Navigation and Control Conference, AIAA 2006–6706. Keystone, Colorado, pp. 21– 24. Karlgaard, C.D., Shen, H.J., 2013a. Desensitized Kalman filtering. IET Radar, Sonar Nav. 7, 2–9. Karlgaard, C.D., Shen, H.J., 2013b. Robust state estimation using desensitized divided difference filter. ISA Trans. 52, 629–637. Li, S., Liu, Z., 2009. Autonomous navigation and guidance scheme for precise and safe planetary landing. Aircr. Eng. Aerosp. Technol.: Int. J. 81, 516–521. Li, S., Jiang, X.Q., Liu, Y.F., 2014. High-precision Mars entry integrated navigation under large uncertainties. J. Nav. 67, 327–342. Li, S., Peng, Y., Lu, Y., et al., 2010. MCAV/IMU integrated navigation for the powered descent phase of Mars EDL. Adv. Space Res. 46, 557– 570.

471

Lou, T., Fu, H., Wang, Z., et al., 2015a. Schmidt-Kalman filter for navigation biases mitigation during Mars entry. J. Aerosp. Eng. 28, 04014101. Lou, T., Fu, H., Zhang, Y., et al., 2015b. Consider unobservable uncertain parameters using radio beacon navigation during Mars entry. Adv. Space Res. 55, 1038–1050. Lou, T., Zhao, L., 2016. Robust Mars atmospheric entry integrated navigation based on parameter sensitivity. Acta Astronaut. 192, 60–70. Norgaard, M., Poulsen, N.K., Ravn, O., 2000. New developments in state estimation for nonlinear systems. Automatica 36, 1627–1638. Qin, T., Zhu, S., Cui, P., et al., 2014. An innovative navigation scheme of powered descent phase for mars pinpoint landing. Adv. Space Res. 54, 1888–1900. Shen, H., Karlgaard, C.D., 2015. Sensitivity reduction of unscented Kalman filter about parameter uncertainties. IET Radar, Sonar Nav. 9, 374–383. Shen, H.J., Seywald, H., Powell, R., 2008. Desensitizing the pin-point landing trajectory on Mars. In: AIAA/AAS Astrodynamics Specialist Conference and Exhibit, AIAA 2008-6934, Honolulu, Hawaii. Simon, D., 2006. Optimal State Estimation: Kalman, H infinity, and Nonlinear Approaches. Wiley-Interscience, New Jersey. Tapley, B.D., Schutz, B.E., Born, G.H., 2004. Statistical Orbit Determination. Academic Press, New York, pp. 387–438. Wang, Y., Sun, F., Zhang, Y., et al., 2012. Central difference particle filter applied to transfer alignment for SINS on missiles. IEEE Trans. Aerosp. Elec. Sys. 48, 375–387. Wolf, A.A., Graves, C., Powell, R., et al., 2004. Systems for pinpoint landing at Mars. In: 14th AAS/AIAA Space Flight Mechanics Conference, Paper 04-272, Maui, HI. Wu, Y., Hu, D., Wu, M., et al., 2006. A numerical-integration perspective on Gaussian filters. IEEE Trans. Signal Process. 54, 2910–2921.