# Dual-EKF-Based Real-Time Celestial Navigation for Lunar Rover

Authors: Li Xie; Peng Yang; Thomas Yang; Ming Li
Journal: Mathematical Problems in Engineering
 (2012)

---

## Abstract

A key requirement of lunar rover autonomous navigation is to acquire state information accurately in real-time during its motion and set up a gradual parameter-based nonlinear kinematics model for the rover. In this paper, we propose a dual-extended-Kalman-filter- (dual-EKF-) based real-time celestial navigation (RCN) method. The proposed method considers the rover position and velocity on the lunar surface as the system parameters and establishes a constant velocity (CV) model. In addition, the attitude quaternion is considered as the system state, and the quaternion differential equation is established as the state equation, which incorporates the output of angular rate gyroscope. Therefore, the measurement equation can be established with sun direction vector from the sun sensor and speed observation from the speedometer. The gyro continuous output ensures the algorithm real-time operation. Finally, we use the dual-EKF method to solve the system equations. Simulation results show that the proposed method can acquire the rover position and heading information in real time and greatly improve the navigation accuracy. Our method overcomes the disadvantage of the cumulative error in inertial navigation.


---

## Body

## 1. Introduction

In order to conduct scientific exploration on the lunar surface, lunar rover must have the ability to execute tasks in unstructured environment. Its navigation system must have a high degree of autonomy and the capabilities of high-accuracy real-time positioning and orientation. On lunar surface, some commonly used navigation methods on the earth are not applicable. There is no GPS system on the moon. If we use radio navigation, the rover control may fail because of the two-way communication delay. The moon rotation is very slow, so we cannot use north seeking gyro. Also, lunar magnetic field is very weak, so magnetic sensor-based methods are ineffective.Lunar rover navigation techniques mainly include absolute positioning and relative positioning. For absolute positioning, such as autonomous celestial navigation [1], position and heading errors are bounded and do not accumulate over time, and the output is discrete. The initial positioning is generally absolute positioning, and its accuracy directly affects relative positioning accuracy. Relative positioning, such as inertial navigation, achieves high accuracy of position and heading in short time, but the errors accumulate over time (which may lead to divergence), and the output is continuous. The current trend for lunar rover navigation is integrated navigation, which combines the advantages of celestial navigation and inertial navigation.The earliest researchers [2–4] carried out celestial navigation by the altitude difference method through observing the sun, earth, and fixed stars. Kuroda et al. [5] utilized celestial navigation and dead-reckoning-based integrated navigation method to obtain lunar rover’s absolute position and heading, which is achieved by observing the altitude and azimuth of the sun and the earth. However, on the moon, the time period during which the sun and the earth appear simultaneously is very short. Therefore, the application of this method is limited. Altitude difference method is very sensitive to measurement noise, and positioning accuracy [6] is low. Vision-based navigation is often used in robotics (Chen 2012, see [7, 8]), but it has difficulty in determining the absolute location and attitude.Recent researchers use vector-observations-based quaternion estimation (QUEST) to get the rover heading angle [9–12]. Ashitey proposed an absolute heading detection method for the field integrated, design and operation (FIDO) rover [9]. When stopped, it uses sun sensor and accelerometer to sense the sun orientation and the local gravity orientation, supply absolute heading for rover with QUEST, and correct the gyroscope cumulative error. Ali described that the US Mars rovers (the “Hope” and the “Spirit”) utilized this method to self-correct the heading information [10]. Some recent technologies used in robotics can be found in the works of Chen et al. [13]. Methods in Chinese literature are similar to the method by Ashitey and they also calculate the heading through QUEST [11, 12]. Thein analyzed the relationship between lunar rover positioning accuracy and astronomical instrument measurement noise [14]. If we want to limit the position error within 50 m, measurement noise should be less than 5.93 arcsec.The above celestial navigation methods (except [5]) do not combine celestial positioning with orientation and cannot get the absolute heading and location information in real time. Ning established a position and attitude determination method based on celestial observations [15], but its reference frame is moon fixed coordinate system rather than local level coordinate system, and it does not consider the impact of the position and speed changes on the gyro angular rate output. Ning proposed a lunar rover kinematics model-based augmented unscented particle filter (ASUPF) as a new autonomous celestial navigation method for dealing with systematic errors and measurement noise [16]. However, the altitude angle measurements in this method are based on the local level provided by the inertial measurement unit (IMU), assuming the rover is keeping static or in constant motion. When the rover is moving, it needs the support of attitude update algorithm in inertial navigation, because the gyro accumulates error and the local level precision is low. Pei proposed a strapdown inertial navigation and celestial-navigation-based integrated method for lunar rovers [17].Since lunar rover position change on the lunar surface is very slow, in order to reduce the dimension of the system, we can set position as a gradual system parameter to estimate the rover state, that is, heading and attitude. To correct the position of the lunar rover, the velocity observation is introduced. Meanwhile, in order to obtain real-time navigation output, the output of the gyro is needed in integrated navigation. In this paper, a method of real-time celestial navigation is proposed, in which positioning and orientation are simultaneous. Also, the error bounded sun sensor output and high accuracy rate gyro output are fused, which ensures the navigation output to be both real-time and of higher accuracy when the rover is moving. Because there is no accelerometer in the system, the impact of the accelerometer error and gravity anomaly on navigation is avoided.The organization of the paper is as follows. Section1 describes the principles of celestial navigation and attitude quaternion kinematics. Section 2 describes the dual EKF-based real-time celestial navigation method. Section 3 presents the results of computer simulations and compares the accuracy of the results obtained with and without velocity observation. Section 4 presents conclusions and discussions.


## 2. Celestial Navigation Principle and Attitude Kinematics

### 2.1. Principle of Celestial Navigation

Set the selenocenter celestial coordinate system as the inertial coordinate system (i), the moon fixed coordinate system asm, geographic coordinate system (NED) as n, the lunar rover body coordinate system as b, the local level coordinate system as l, and the sun sensor coordinate system as c. After installation, the sensor coordinate systems b and c coincide with each other.Celestial navigation system can detect the rover geographical position and heading provided that the local gravity datum (level posture) is known. The outputs are lunar rover position (latitude and longitude) on the moon and the attitude, including the heading, pitch, and roll. State vector of the systemx=[λ,L,An] is used to describe the lunar rover position and heading information, in which (λ,L) is the lunar rover longitude and astronomical latitude and An is just heading relative to North Pole of the moon.In Figure1, the moon fixed coordinate system, after rotating λ (east longitude is positive) around the Z axis, becomes coordinate system O-x1y1z1. After further rotating by -L-π/2 (north latitude is positive) around the Oy1 axis, the navigation coordinate w is obtained. The attitude matrix about the latitude and longitude is as follows [18]:(2.1)Rmn=Ry(-L-π2)Rz(λ).
The attitude matrix about the navigation coordinate system and the lunar body coordinate system is:(2.2)Rnb=Rx(φ)Ry(ψ)Rz(θ).
Here, θ is the heading, ψ is the pitch angle, and φ is the roll. To prevent the risk of rollover, lunar rover pitch and roll should between ±45°. The attitude matrix about the moon fixed coordinate system and the local level coordinate system is A=ℜml (called target matrix here). Substituting ℜz(θ),ℜy(-L-π/2),ℜz(λ) into the above formula, we get the following equation:(2.3)Rml=Rz(θ)Ry(-L-π2)Rz(λ).Figure 1
Moon-fixed (m) and navigation (n) coordinate system.


### 2.2. Quaternion Attitude Kinematics

Attitude can be expressed in several mathematical parameters: quaternion, attitude matrix, Euler angles, Rodrigues parameters, and so on. The attitude matrix contains a total of nine parameters, but because it is orthogonal matrix, only three components are independent. One of the most useful parameters is the attitude quaternion, which is a four-dimensional vector, defined asq=[ρTq4]T, where ρ=[q1q2q3]T=êsin(ϑ/2) and q4=cos(ϑ/2). Here, ê is the rotation axis and ϑ is the rotation angle. When using a four-dimensional vector to describe the three-dimensional rotation, the four parameters of quaternion are not independent, and they are subject to the constraint qTq=1. The relationship between the attitude matrix and the quaternion from the inertial coordinate system i to the body coordinate system b is(2.4)A(q)ib=ΞT(q)Ψ(q),where(2.5)Ξ(q)≡[q4I3×3+[ρ×]-ρT],Ψ(q)≡[q4I3×3-[ρ×]-ρT]⋅Ξ(q)≡[q4I3×3+[ρ×]-ρT],Ψ(q)≡[q4I3×3-[ρ×]-ρT].
Here [ρ×] is the cross-product matrix, defined as [ρ×]=[0-q3q2q30-q1-q2q10]. One advantage of using quaternion is that the attitude matrix is quadratic equation of the parameter and thus does not include any transcendental function. For small angles, the vector part of the quaternion is approximately half of the rotation angle, and therefore ρ≈α/2, q4≈1, where the 3-dimensional vector α includes roll, pitch, and heading. Therefore, the attitude matrix can be approximated as Aib≈I3×3-[α×], which is effective in the first-order approximation.The attitude kinematics equation is(2.6)Ȧib=-[ωibb×]Aib.
Here, ωibb is the angular velocity of the b frame relative to i frame expressed in b coordinates. The quaternion differential equation is(2.7)q̇=12Ξ(q)ωibb=12Ω(ωibb)q,
where
(2.8)Ω(ωibb)≡[-[ωibb×]ωibb-(ωibb)T0].The main advantage of using the quaternion is that the kinematics equation is linear and there is no singularity. Another advantage is that continuous rotation of coordinate frames can be expressed as the quaternion multiplication. Suppose a continuous rotation can be expressed as(2.9)A(q′)A(q)=A(q′⊗q).The composition of the quaternion is bilinear, with(2.10)q′⊗q=[Ψ(q′)q′]q=[Ξ(q)q]q′,
and the inverse quaternion is defined by q-1=[-ρq4]. Note that q⊗q-1=[0001]T is the identity quaternion.


## 2.1. Principle of Celestial Navigation

Set the selenocenter celestial coordinate system as the inertial coordinate system (i), the moon fixed coordinate system asm, geographic coordinate system (NED) as n, the lunar rover body coordinate system as b, the local level coordinate system as l, and the sun sensor coordinate system as c. After installation, the sensor coordinate systems b and c coincide with each other.Celestial navigation system can detect the rover geographical position and heading provided that the local gravity datum (level posture) is known. The outputs are lunar rover position (latitude and longitude) on the moon and the attitude, including the heading, pitch, and roll. State vector of the systemx=[λ,L,An] is used to describe the lunar rover position and heading information, in which (λ,L) is the lunar rover longitude and astronomical latitude and An is just heading relative to North Pole of the moon.In Figure1, the moon fixed coordinate system, after rotating λ (east longitude is positive) around the Z axis, becomes coordinate system O-x1y1z1. After further rotating by -L-π/2 (north latitude is positive) around the Oy1 axis, the navigation coordinate w is obtained. The attitude matrix about the latitude and longitude is as follows [18]:(2.1)Rmn=Ry(-L-π2)Rz(λ).
The attitude matrix about the navigation coordinate system and the lunar body coordinate system is:(2.2)Rnb=Rx(φ)Ry(ψ)Rz(θ).
Here, θ is the heading, ψ is the pitch angle, and φ is the roll. To prevent the risk of rollover, lunar rover pitch and roll should between ±45°. The attitude matrix about the moon fixed coordinate system and the local level coordinate system is A=ℜml (called target matrix here). Substituting ℜz(θ),ℜy(-L-π/2),ℜz(λ) into the above formula, we get the following equation:(2.3)Rml=Rz(θ)Ry(-L-π2)Rz(λ).Figure 1
Moon-fixed (m) and navigation (n) coordinate system.


## 2.2. Quaternion Attitude Kinematics

Attitude can be expressed in several mathematical parameters: quaternion, attitude matrix, Euler angles, Rodrigues parameters, and so on. The attitude matrix contains a total of nine parameters, but because it is orthogonal matrix, only three components are independent. One of the most useful parameters is the attitude quaternion, which is a four-dimensional vector, defined asq=[ρTq4]T, where ρ=[q1q2q3]T=êsin(ϑ/2) and q4=cos(ϑ/2). Here, ê is the rotation axis and ϑ is the rotation angle. When using a four-dimensional vector to describe the three-dimensional rotation, the four parameters of quaternion are not independent, and they are subject to the constraint qTq=1. The relationship between the attitude matrix and the quaternion from the inertial coordinate system i to the body coordinate system b is(2.4)A(q)ib=ΞT(q)Ψ(q),where(2.5)Ξ(q)≡[q4I3×3+[ρ×]-ρT],Ψ(q)≡[q4I3×3-[ρ×]-ρT]⋅Ξ(q)≡[q4I3×3+[ρ×]-ρT],Ψ(q)≡[q4I3×3-[ρ×]-ρT].
Here [ρ×] is the cross-product matrix, defined as [ρ×]=[0-q3q2q30-q1-q2q10]. One advantage of using quaternion is that the attitude matrix is quadratic equation of the parameter and thus does not include any transcendental function. For small angles, the vector part of the quaternion is approximately half of the rotation angle, and therefore ρ≈α/2, q4≈1, where the 3-dimensional vector α includes roll, pitch, and heading. Therefore, the attitude matrix can be approximated as Aib≈I3×3-[α×], which is effective in the first-order approximation.The attitude kinematics equation is(2.6)Ȧib=-[ωibb×]Aib.
Here, ωibb is the angular velocity of the b frame relative to i frame expressed in b coordinates. The quaternion differential equation is(2.7)q̇=12Ξ(q)ωibb=12Ω(ωibb)q,
where
(2.8)Ω(ωibb)≡[-[ωibb×]ωibb-(ωibb)T0].The main advantage of using the quaternion is that the kinematics equation is linear and there is no singularity. Another advantage is that continuous rotation of coordinate frames can be expressed as the quaternion multiplication. Suppose a continuous rotation can be expressed as(2.9)A(q′)A(q)=A(q′⊗q).The composition of the quaternion is bilinear, with(2.10)q′⊗q=[Ψ(q′)q′]q=[Ξ(q)q]q′,
and the inverse quaternion is defined by q-1=[-ρq4]. Note that q⊗q-1=[0001]T is the identity quaternion.


## 3. Dual-EKF-Based RCPO Method

Assume the state vector of the navigation system isxs, the system parameter vector is xp, and the observation vector is ỹk. According to the problem, a continuous-discrete nonlinear state space model can be derived:(3.1)ẋs(t)=f{t,xs(t),u(t),xp(t)}+w(t),ỹk=hk(xs,k,xp,k)+vk,
where f(·),h(·) are implicit vector functions, w(t) is the continuous process noise, and vk is the discrete measurement noise. In the state vector xs=[qT,βT]T, q is the heading and attitude quaternion in the navigation frame (w) for the lunar rover and β is the constant bias for gyro. In parameter vector xp=[pT,vT]T, p=[Lλ]T is the rover position, which is the latitude and longitude; V=[vLvλ]T is the north speed and east speed on the lunar surface.


### 3.1. System Parameter and State Equations

The lunar rover position and velocity equations constitute the system parameter equations:(3.2)ẋp=Fpxp+wp
with the parameter vector xp=[pTVT]T, the state transition matrix Fp=[0010000100000000], the parameters process noise wp=[00wLwλ], and the noise covariance Qp=diag[00σL2σλ2].The lunar rover attitude constitutes the system state equations, and the quaternion differential equation is expressed by(3.3)q̇2=12Ω(ωnbb)q2.
Here, ωnbb is the angular velocity of the b frame relative to n frame expressed in b coordinates. The gyro measurement model is(3.4)ω̃ibb=ωibb+β+ηv,β̇=ηu.
Here, ωibb is the angular velocity of the b frame relative to i frame expressed in b coordinates. β is the constant bias of the gyro, ηv and ηu are zero mean Gaussian white noise processs, and their spectral density functions are σv2I3×3 and σu2I3×3, respectively.Because the selenocenter celestial coordinate system is the inertial coordinate system here, so(3.5)ωnbb=ωibb-Anb(q2)ωinn.
Also, ωinn is the angular velocity of the n frame relative to i frame expressed in n coordinates(3.6)ωinn=ωim[cos⁡L0-sin⁡L]+[VER-VNR-VEtanLR],VE=vλRcos⁡L,VN=vLR.
In (3.6), ωim is the angular velocity of the m frame relative to i frame, and the second expression on the right side is the angular velocity of the n frame relative to m frame. The angular velocity of the m frame relative to i frame ωim is(3.7)ωim=ωgz+Aωzzim.
In (3.7), ωgz is the revolution angular velocity of the moon around the earth, ωzz is the moon spin velocity, and Aim is the attitude matrix from the inertial reference frame i to the moon fixed frame m, which can be calculated after querying ephemeris [18].


### 3.2. Celestial and Speed Observation Equations

The measurement principle of vector observation attitude sensor can be expressed asb̃i=A(q)ri+vi,i=1,…,n. If n celestial bodies are observable simultaneously, we can get n vector pairs, so the measurement equation at time k is(3.8)b̃k=[A(q2)A(p1)r1A(q2)A(p1)r2⋮A(q2)A(p1)rn]|tk+[v1v2⋮vn]|tk,
where A(q2)=Anb,A(p1)=Amn=ℜy(-L-π/2)ℜz(λ).Setvk=[v1v2⋮vn]|tk, its variance is R=diag[σ12I3×3,σ22I3×3,…,σn2I3×3], where diag[···] is the diagonal matrix. In this paper, n=1, r=si,b=sb, where si is the sun unit vector in inertial frame and sb is the sun unit vector in the body frame.The speed observation equation of the speedometer is(3.9)Ṽk=Vk+uk,
where Ṽk is speed measurement at time k, uk is the measurement noise, and its covariance matrix is Ru=σu2I2×2.


### 3.3. Dual Continuous-Discrete EKF

Dual-EKF algorithm uses two mutual coupling extended Kalman filters working in parallel and a state estimator working between the system parameter time update process and the measurement update process [19]. Dual-EKF can estimate the system state and parameter online. Using the above model, a continuous-discrete extended Kalman filter can be derived (Chen 2012, [20]). The process equation about the system parameter is a continuous linear equation, which can be discretized directly. The process equations about the system state are nonlinear equations, and the Jacobian matrix needs to be calculated. Finally, we get the discrete linear state space model (without considering the control input uk):(3.10)xs,k+1=f{xs,k,xp,k}+wk,ỹk=hk(xs,k,xp,k)+vk.


#### 3.3.1. Linearization of State Process Equations

In order to maintain the quaternion normalization constraint, we use the multiplicative error quaternion in the body frame to express the attitude error:(3.11)δq=q⊗q̂-1,
where q̂-1 is the inverse of the quaternion estimate and δq≡[δρTδq4]T. If the error quaternion δq is very small, we can use the small angle approximation. After a series of derivation, the linear kinematic model of the attitude error [21] is obtained:(3.12)δα̇=-[ω̂nbb×]δα+δωibb-A(q̂2)δωinn,δq̇4=0,
where δωibb=ωibb-ω̂ibb and δωinn=ωinn-ω̂inn=0. Also, δωibb=-(Δβ+ηv) is available by the above gyro model, in which Δβ≡β-β̂. So the above formula becomes(3.13)δα̇=-[ω̂nbb×]δα-(Δβ+ηv).The remaining error equation can be obtained by similar methods. The state vector, the state error vector, and the process noise vector and covariance in this EKF are defined as(3.14)xs≡[qβ],Δxs≡[δαΔβ],ws≡[ηvηu],Qs=[σv2I3×303×303×3σu2I3×3].The error dynamics of time update in the EKF isΔẋ=FΔx+Gw. Here, the state transition matrix F and the noise coefficient matrix G are(3.15)F≡[-[ω̂nbb×]-I3×303×303×3],G≡[-I3×303×303×3I3×3]S.


#### 3.3.2. Linearization of Measurement Equations

Next we determine the sensitive matrixHs(x̂-s) of the system state observation equation. The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.16)b=A(q2)A(p̂1-)r,b̂-=A(q̂2-)A(p̂1-)r.According to (2.6),(3.17)A(q2)=A(δq2)A(q̂2-)=(I3×3-[δα2×])A(q̂2-).From (3.16), we have(3.18)Δb=b-b̂-=[A(q̂2-)A(p̂1-)r×]δα2.Note thatHsq=[A(q̂2-)A(p̂1-)r×], so the sensitivity matrix for all measurements is(3.19)Hs(x̂-s)=[Hsq103×3Hsq203×3⋮⋮Hsqn03×3]|tk.Next we determine the sensitive matrixHp(x̂-p) of the system parameter observation equation.The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.20)b=A(q̂2-)A(p)r,b̂-=A(q̂2-)A(p̂-)r.FunctionA(p) is expanded as a Taylor series, which is(3.21)A(p)≈A(p̂-)+∑j=12Aj-Δpj,
where A1-=∂A/∂L|L̂-,A2-=∂A/∂L|λ̂-.Finally, we have(3.22)Δb=b-b̂-=∑j=12A(q̂2-)Aj-rΔpj.Note thatHp=[A(q̂2-)A1-rA(q̂2-)A2-r]. Combined with the speed observations, the sensitivity matrix of all measurements is(3.23)Hp(x̂-p)=[Hp103×2Hp203×2⋮⋮Hpn03×202×2I2×2]|tk.


#### 3.3.3. Dual-EKF Algorithm

Finally the proposed algorithm of dual-EKF is shown in Table1.Table 1
Dual-EKF algorithm.
InitializationParameter:x̂p(t0)=x̂p,0, Pp(t0)=Pp,0 State: x̂s(t0)=x̂s,0, Ps(t0)=Ps,0State measurement updateKs,k=Ps,k-Hs,kT[Hs,kPs,k-Hs,kT+R]-1εk=b̃s,k-hk(x̂s,k-,x̂p,k-)Δx̂s,k+=Ks,kεkq̂2,k+=q̂2,k-+12Ξ(q̂2,k-)δα̂2,k+, normalizationβ̂k+=β̂k-+Δβ̂k+Ps,k+=[I-Ks,kHs,k]Ps,k-Parameter measurement updateKp,k=Pp,k-Hp,kT[Hp,kPp,k-Hp,kT+R′]-1R′=diag([R,Ru])x̂p,k+=x̂p,k-+Kp,k[εk;(Ṽk-V̂k-)]Pp,k+=[I-Kp,kHp,k]Pp,k+1-Parameter time updatex̂p,k+1-=Φpx̂p,k+Pp,k+1-=ΦpPp,k+ΦpT+QpState time updateω̂nbb=(ω̃ibb-β̂k+)-A(q̂2,k+)ωinn(x̂p,k+1-)q̂̇2=12Ω(ω̂nbb)q̂2β̂̇=0Ṗs=FsPs+PsFsT+GQsGT


## 3.1. System Parameter and State Equations

The lunar rover position and velocity equations constitute the system parameter equations:(3.2)ẋp=Fpxp+wp
with the parameter vector xp=[pTVT]T, the state transition matrix Fp=[0010000100000000], the parameters process noise wp=[00wLwλ], and the noise covariance Qp=diag[00σL2σλ2].The lunar rover attitude constitutes the system state equations, and the quaternion differential equation is expressed by(3.3)q̇2=12Ω(ωnbb)q2.
Here, ωnbb is the angular velocity of the b frame relative to n frame expressed in b coordinates. The gyro measurement model is(3.4)ω̃ibb=ωibb+β+ηv,β̇=ηu.
Here, ωibb is the angular velocity of the b frame relative to i frame expressed in b coordinates. β is the constant bias of the gyro, ηv and ηu are zero mean Gaussian white noise processs, and their spectral density functions are σv2I3×3 and σu2I3×3, respectively.Because the selenocenter celestial coordinate system is the inertial coordinate system here, so(3.5)ωnbb=ωibb-Anb(q2)ωinn.
Also, ωinn is the angular velocity of the n frame relative to i frame expressed in n coordinates(3.6)ωinn=ωim[cos⁡L0-sin⁡L]+[VER-VNR-VEtanLR],VE=vλRcos⁡L,VN=vLR.
In (3.6), ωim is the angular velocity of the m frame relative to i frame, and the second expression on the right side is the angular velocity of the n frame relative to m frame. The angular velocity of the m frame relative to i frame ωim is(3.7)ωim=ωgz+Aωzzim.
In (3.7), ωgz is the revolution angular velocity of the moon around the earth, ωzz is the moon spin velocity, and Aim is the attitude matrix from the inertial reference frame i to the moon fixed frame m, which can be calculated after querying ephemeris [18].


## 3.2. Celestial and Speed Observation Equations

The measurement principle of vector observation attitude sensor can be expressed asb̃i=A(q)ri+vi,i=1,…,n. If n celestial bodies are observable simultaneously, we can get n vector pairs, so the measurement equation at time k is(3.8)b̃k=[A(q2)A(p1)r1A(q2)A(p1)r2⋮A(q2)A(p1)rn]|tk+[v1v2⋮vn]|tk,
where A(q2)=Anb,A(p1)=Amn=ℜy(-L-π/2)ℜz(λ).Setvk=[v1v2⋮vn]|tk, its variance is R=diag[σ12I3×3,σ22I3×3,…,σn2I3×3], where diag[···] is the diagonal matrix. In this paper, n=1, r=si,b=sb, where si is the sun unit vector in inertial frame and sb is the sun unit vector in the body frame.The speed observation equation of the speedometer is(3.9)Ṽk=Vk+uk,
where Ṽk is speed measurement at time k, uk is the measurement noise, and its covariance matrix is Ru=σu2I2×2.


## 3.3. Dual Continuous-Discrete EKF

Dual-EKF algorithm uses two mutual coupling extended Kalman filters working in parallel and a state estimator working between the system parameter time update process and the measurement update process [19]. Dual-EKF can estimate the system state and parameter online. Using the above model, a continuous-discrete extended Kalman filter can be derived (Chen 2012, [20]). The process equation about the system parameter is a continuous linear equation, which can be discretized directly. The process equations about the system state are nonlinear equations, and the Jacobian matrix needs to be calculated. Finally, we get the discrete linear state space model (without considering the control input uk):(3.10)xs,k+1=f{xs,k,xp,k}+wk,ỹk=hk(xs,k,xp,k)+vk.


### 3.3.1. Linearization of State Process Equations

In order to maintain the quaternion normalization constraint, we use the multiplicative error quaternion in the body frame to express the attitude error:(3.11)δq=q⊗q̂-1,
where q̂-1 is the inverse of the quaternion estimate and δq≡[δρTδq4]T. If the error quaternion δq is very small, we can use the small angle approximation. After a series of derivation, the linear kinematic model of the attitude error [21] is obtained:(3.12)δα̇=-[ω̂nbb×]δα+δωibb-A(q̂2)δωinn,δq̇4=0,
where δωibb=ωibb-ω̂ibb and δωinn=ωinn-ω̂inn=0. Also, δωibb=-(Δβ+ηv) is available by the above gyro model, in which Δβ≡β-β̂. So the above formula becomes(3.13)δα̇=-[ω̂nbb×]δα-(Δβ+ηv).The remaining error equation can be obtained by similar methods. The state vector, the state error vector, and the process noise vector and covariance in this EKF are defined as(3.14)xs≡[qβ],Δxs≡[δαΔβ],ws≡[ηvηu],Qs=[σv2I3×303×303×3σu2I3×3].The error dynamics of time update in the EKF isΔẋ=FΔx+Gw. Here, the state transition matrix F and the noise coefficient matrix G are(3.15)F≡[-[ω̂nbb×]-I3×303×303×3],G≡[-I3×303×303×3I3×3]S.


### 3.3.2. Linearization of Measurement Equations

Next we determine the sensitive matrixHs(x̂-s) of the system state observation equation. The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.16)b=A(q2)A(p̂1-)r,b̂-=A(q̂2-)A(p̂1-)r.According to (2.6),(3.17)A(q2)=A(δq2)A(q̂2-)=(I3×3-[δα2×])A(q̂2-).From (3.16), we have(3.18)Δb=b-b̂-=[A(q̂2-)A(p̂1-)r×]δα2.Note thatHsq=[A(q̂2-)A(p̂1-)r×], so the sensitivity matrix for all measurements is(3.19)Hs(x̂-s)=[Hsq103×3Hsq203×3⋮⋮Hsqn03×3]|tk.Next we determine the sensitive matrixHp(x̂-p) of the system parameter observation equation.The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.20)b=A(q̂2-)A(p)r,b̂-=A(q̂2-)A(p̂-)r.FunctionA(p) is expanded as a Taylor series, which is(3.21)A(p)≈A(p̂-)+∑j=12Aj-Δpj,
where A1-=∂A/∂L|L̂-,A2-=∂A/∂L|λ̂-.Finally, we have(3.22)Δb=b-b̂-=∑j=12A(q̂2-)Aj-rΔpj.Note thatHp=[A(q̂2-)A1-rA(q̂2-)A2-r]. Combined with the speed observations, the sensitivity matrix of all measurements is(3.23)Hp(x̂-p)=[Hp103×2Hp203×2⋮⋮Hpn03×202×2I2×2]|tk.


### 3.3.3. Dual-EKF Algorithm

Finally the proposed algorithm of dual-EKF is shown in Table1.Table 1
Dual-EKF algorithm.
InitializationParameter:x̂p(t0)=x̂p,0, Pp(t0)=Pp,0 State: x̂s(t0)=x̂s,0, Ps(t0)=Ps,0State measurement updateKs,k=Ps,k-Hs,kT[Hs,kPs,k-Hs,kT+R]-1εk=b̃s,k-hk(x̂s,k-,x̂p,k-)Δx̂s,k+=Ks,kεkq̂2,k+=q̂2,k-+12Ξ(q̂2,k-)δα̂2,k+, normalizationβ̂k+=β̂k-+Δβ̂k+Ps,k+=[I-Ks,kHs,k]Ps,k-Parameter measurement updateKp,k=Pp,k-Hp,kT[Hp,kPp,k-Hp,kT+R′]-1R′=diag([R,Ru])x̂p,k+=x̂p,k-+Kp,k[εk;(Ṽk-V̂k-)]Pp,k+=[I-Kp,kHp,k]Pp,k+1-Parameter time updatex̂p,k+1-=Φpx̂p,k+Pp,k+1-=ΦpPp,k+ΦpT+QpState time updateω̂nbb=(ω̃ibb-β̂k+)-A(q̂2,k+)ωinn(x̂p,k+1-)q̂̇2=12Ω(ω̂nbb)q̂2β̂̇=0Ṗs=FsPs+PsFsT+GQsGT


## 3.3.1. Linearization of State Process Equations

In order to maintain the quaternion normalization constraint, we use the multiplicative error quaternion in the body frame to express the attitude error:(3.11)δq=q⊗q̂-1,
where q̂-1 is the inverse of the quaternion estimate and δq≡[δρTδq4]T. If the error quaternion δq is very small, we can use the small angle approximation. After a series of derivation, the linear kinematic model of the attitude error [21] is obtained:(3.12)δα̇=-[ω̂nbb×]δα+δωibb-A(q̂2)δωinn,δq̇4=0,
where δωibb=ωibb-ω̂ibb and δωinn=ωinn-ω̂inn=0. Also, δωibb=-(Δβ+ηv) is available by the above gyro model, in which Δβ≡β-β̂. So the above formula becomes(3.13)δα̇=-[ω̂nbb×]δα-(Δβ+ηv).The remaining error equation can be obtained by similar methods. The state vector, the state error vector, and the process noise vector and covariance in this EKF are defined as(3.14)xs≡[qβ],Δxs≡[δαΔβ],ws≡[ηvηu],Qs=[σv2I3×303×303×3σu2I3×3].The error dynamics of time update in the EKF isΔẋ=FΔx+Gw. Here, the state transition matrix F and the noise coefficient matrix G are(3.15)F≡[-[ω̂nbb×]-I3×303×303×3],G≡[-I3×303×303×3I3×3]S.


## 3.3.2. Linearization of Measurement Equations

Next we determine the sensitive matrixHs(x̂-s) of the system state observation equation. The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.16)b=A(q2)A(p̂1-)r,b̂-=A(q̂2-)A(p̂1-)r.According to (2.6),(3.17)A(q2)=A(δq2)A(q̂2-)=(I3×3-[δα2×])A(q̂2-).From (3.16), we have(3.18)Δb=b-b̂-=[A(q̂2-)A(p̂1-)r×]δα2.Note thatHsq=[A(q̂2-)A(p̂1-)r×], so the sensitivity matrix for all measurements is(3.19)Hs(x̂-s)=[Hsq103×3Hsq203×3⋮⋮Hsqn03×3]|tk.Next we determine the sensitive matrixHp(x̂-p) of the system parameter observation equation.The true value and the estimate of the celestial bodies vector in the body coordinate system are(3.20)b=A(q̂2-)A(p)r,b̂-=A(q̂2-)A(p̂-)r.FunctionA(p) is expanded as a Taylor series, which is(3.21)A(p)≈A(p̂-)+∑j=12Aj-Δpj,
where A1-=∂A/∂L|L̂-,A2-=∂A/∂L|λ̂-.Finally, we have(3.22)Δb=b-b̂-=∑j=12A(q̂2-)Aj-rΔpj.Note thatHp=[A(q̂2-)A1-rA(q̂2-)A2-r]. Combined with the speed observations, the sensitivity matrix of all measurements is(3.23)Hp(x̂-p)=[Hp103×2Hp203×2⋮⋮Hpn03×202×2I2×2]|tk.


## 3.3.3. Dual-EKF Algorithm

Finally the proposed algorithm of dual-EKF is shown in Table1.Table 1
Dual-EKF algorithm.
InitializationParameter:x̂p(t0)=x̂p,0, Pp(t0)=Pp,0 State: x̂s(t0)=x̂s,0, Ps(t0)=Ps,0State measurement updateKs,k=Ps,k-Hs,kT[Hs,kPs,k-Hs,kT+R]-1εk=b̃s,k-hk(x̂s,k-,x̂p,k-)Δx̂s,k+=Ks,kεkq̂2,k+=q̂2,k-+12Ξ(q̂2,k-)δα̂2,k+, normalizationβ̂k+=β̂k-+Δβ̂k+Ps,k+=[I-Ks,kHs,k]Ps,k-Parameter measurement updateKp,k=Pp,k-Hp,kT[Hp,kPp,k-Hp,kT+R′]-1R′=diag([R,Ru])x̂p,k+=x̂p,k-+Kp,k[εk;(Ṽk-V̂k-)]Pp,k+=[I-Kp,kHp,k]Pp,k+1-Parameter time updatex̂p,k+1-=Φpx̂p,k+Pp,k+1-=ΦpPp,k+ΦpT+QpState time updateω̂nbb=(ω̃ibb-β̂k+)-A(q̂2,k+)ωinn(x̂p,k+1-)q̂̇2=12Ω(ω̂nbb)q̂2β̂̇=0Ṗs=FsPs+PsFsT+GQsGT


## 4. Simulations and Discussions

### 4.1. Simulation Conditions

Specific simulation parameters are shown in Table2.Table 2
Simulation parameters.
Beginning time2011-01-01 00:00:00Sampling intervalΔt=1sInitial originλ(t0)=0°,L(t0)=0°Initial velocityvL=-0.1m/(s·R),vλ=0.1m/(s·R)Initial attitudeq(t0)=[0001]TGyro biasesβ(t0)=0.1[111]Tdeg/hrInitial covarianceP0p=0.052deg2P0V=0.12(m/s)2P0α=0.12deg2P0β=0.22(deg/hr)2Gyro noise (Qs)σgv=10×10-7rad/s1/2σgu=10×10-10rad/s3/2CV model (Qp)σL=σL=0.0001m/(s·R)(R: moon radius, the same below)Sun sensor (R)1′(3σs)Velocity sensor (Ru)σu=0.001m/(s·R)


### 4.2. Simulation of Moving Lunar Rover

In this paper, we carried out lunar rover simulation under various moving conditions described in Table3, and navigation accuracy with and without the speed observation is compared. The lunar rover movement includes rotational and translational movements, where the former can be sensed by the gyro angular velocity and the latter can be measured by the speedometer line speed.Table 3
Lunar rover motion.
MotionTime (s)Angular velocity (∘/s)Linear velocity (m/(s·R))(1) Static1~10000(2) Rotation101~200ωz=10(3) Uniform motion201~3000vL=0.2vλ=-0.25(4) Rotation and uniform motion301~500ωz=1vL=0.2vλ=-0.25(5) Static again501~60000The simulation results of the lunar rover are shown in Figure2, with the left diagram on each figure representing the simulation result without speed observation and the right diagram representing the simulation result with speed observation.Position error and3σ boundary.
(a)(b)(c)(d)Figure2 shows the position error and its 3σ boundary, and we see the latitude and longitude errors in the left diagram diverge at last. After the uniform motion error expands, we mainly have the lunar rover speed changes, so the constant velocity (CV) model is no longer applicable. The navigation error in the right diagram is kept within the 3σ boundary, and it does not diverge. Because of the speedometer line speeds information, the absolute position of the rover can be adjusted in real time. The mean of the latitude error is 3.97″, and the standard deviation is 0.83″; the mean of longitude error is 1.07″, and the standard deviation is 1.42″. Converted into the line error according to the lunar radius, the error is 35.51 m.Figure3 shows the speed error and its 3σ boundary. The initial velocity is not accurate. In the left diagram, when uniform motion speed changes cannot be sensed any longer, the error shape exhibits phase steps. While the speed observation is available, the navigation system can sense it after the speed change. We see the two speed changes in the lunar rover movement are in zigzag fashions on the speed error figure and then quickly disappear.The speed error and3σ boundary.
(a)(b)(c)(d)Figure4 shows the attitude, heading error, and its 3σ boundary, but the heading information is of main interest in the navigation. The mean of the heading error in the left diagram is -5.55" with a standard deviation of 3.03". The mean of the heading error in the right diagram is 1.71", with a standard deviation of 3.53".Attitude, heading error, and3σ boundary.
(a)(b)(c)(d)(e)(f)Figure5 shows the constant gyro bias error and its 3σ boundary. As can be seen from the graph, the 3-channel constant bias basically converges in the Motion 1 stage, that is, static, and completes the initial alignment of the gyroscope.The constant gyro bias error and3σ boundary.
(a)(b)(c)(d)(e)(f)


### 4.3. Discussions and Remarks

From the above analysis and simulation, it can be seen that the significance of this work is to combine celestial and inertial sensor data to obtain the attitude and heading information for the real-time navigation of the lunar rover. The simulation results indicate that the dual-EKF method is valid in this field. To obtain better results, the following two properties are worth of being further investigated in the future work on navigation.Computational accuracy: the technology of imaging processing plays a role in the celestial navigation. The performance of noise filtering and feature extraction for the astronomical images will affect the navigation precision directly (Liao et al., see [22, 23]; Yang et al., see [24, 25]). In addition, the nonlinear properties, such as fractals [26, 27], in the astronomical images can affect the navigation effect also.Computational complexity: though the Kalman filter is the most widely used attitude estimation algorithm for navigation and it offers the optimal recursive solution to the nonlinear estimation problem, the implementation efficiency of the recursive Kalman estimator has been an issue. Correlation is a useful technique in the field. Real-time navigation may use it to help in Kalman filtering [28, 29].


## 4.1. Simulation Conditions

Specific simulation parameters are shown in Table2.Table 2
Simulation parameters.
Beginning time2011-01-01 00:00:00Sampling intervalΔt=1sInitial originλ(t0)=0°,L(t0)=0°Initial velocityvL=-0.1m/(s·R),vλ=0.1m/(s·R)Initial attitudeq(t0)=[0001]TGyro biasesβ(t0)=0.1[111]Tdeg/hrInitial covarianceP0p=0.052deg2P0V=0.12(m/s)2P0α=0.12deg2P0β=0.22(deg/hr)2Gyro noise (Qs)σgv=10×10-7rad/s1/2σgu=10×10-10rad/s3/2CV model (Qp)σL=σL=0.0001m/(s·R)(R: moon radius, the same below)Sun sensor (R)1′(3σs)Velocity sensor (Ru)σu=0.001m/(s·R)


## 4.2. Simulation of Moving Lunar Rover

In this paper, we carried out lunar rover simulation under various moving conditions described in Table3, and navigation accuracy with and without the speed observation is compared. The lunar rover movement includes rotational and translational movements, where the former can be sensed by the gyro angular velocity and the latter can be measured by the speedometer line speed.Table 3
Lunar rover motion.
MotionTime (s)Angular velocity (∘/s)Linear velocity (m/(s·R))(1) Static1~10000(2) Rotation101~200ωz=10(3) Uniform motion201~3000vL=0.2vλ=-0.25(4) Rotation and uniform motion301~500ωz=1vL=0.2vλ=-0.25(5) Static again501~60000The simulation results of the lunar rover are shown in Figure2, with the left diagram on each figure representing the simulation result without speed observation and the right diagram representing the simulation result with speed observation.Position error and3σ boundary.
(a)(b)(c)(d)Figure2 shows the position error and its 3σ boundary, and we see the latitude and longitude errors in the left diagram diverge at last. After the uniform motion error expands, we mainly have the lunar rover speed changes, so the constant velocity (CV) model is no longer applicable. The navigation error in the right diagram is kept within the 3σ boundary, and it does not diverge. Because of the speedometer line speeds information, the absolute position of the rover can be adjusted in real time. The mean of the latitude error is 3.97″, and the standard deviation is 0.83″; the mean of longitude error is 1.07″, and the standard deviation is 1.42″. Converted into the line error according to the lunar radius, the error is 35.51 m.Figure3 shows the speed error and its 3σ boundary. The initial velocity is not accurate. In the left diagram, when uniform motion speed changes cannot be sensed any longer, the error shape exhibits phase steps. While the speed observation is available, the navigation system can sense it after the speed change. We see the two speed changes in the lunar rover movement are in zigzag fashions on the speed error figure and then quickly disappear.The speed error and3σ boundary.
(a)(b)(c)(d)Figure4 shows the attitude, heading error, and its 3σ boundary, but the heading information is of main interest in the navigation. The mean of the heading error in the left diagram is -5.55" with a standard deviation of 3.03". The mean of the heading error in the right diagram is 1.71", with a standard deviation of 3.53".Attitude, heading error, and3σ boundary.
(a)(b)(c)(d)(e)(f)Figure5 shows the constant gyro bias error and its 3σ boundary. As can be seen from the graph, the 3-channel constant bias basically converges in the Motion 1 stage, that is, static, and completes the initial alignment of the gyroscope.The constant gyro bias error and3σ boundary.
(a)(b)(c)(d)(e)(f)


## 4.3. Discussions and Remarks

From the above analysis and simulation, it can be seen that the significance of this work is to combine celestial and inertial sensor data to obtain the attitude and heading information for the real-time navigation of the lunar rover. The simulation results indicate that the dual-EKF method is valid in this field. To obtain better results, the following two properties are worth of being further investigated in the future work on navigation.Computational accuracy: the technology of imaging processing plays a role in the celestial navigation. The performance of noise filtering and feature extraction for the astronomical images will affect the navigation precision directly (Liao et al., see [22, 23]; Yang et al., see [24, 25]). In addition, the nonlinear properties, such as fractals [26, 27], in the astronomical images can affect the navigation effect also.Computational complexity: though the Kalman filter is the most widely used attitude estimation algorithm for navigation and it offers the optimal recursive solution to the nonlinear estimation problem, the implementation efficiency of the recursive Kalman estimator has been an issue. Correlation is a useful technique in the field. Real-time navigation may use it to help in Kalman filtering [28, 29].


## 5. Discussion and Conclusions

In this paper, a sun-orientation-and-speed-observations-based lunar rover real-time celestial navigation method is proposed, using dual-EKF to estimate system parameters and state. The method treats the position and velocity as system parameters and establishes a position, velocity differential equation. Further, the rover attitude quaternion is treated as the system state, and the quaternion differential equation is established as the state equation. To establish the measurement equation, the sun direction vector is obtained from the sun sensor and the speed observation is obtained from the speedometer. Finally, the rover position and heading information is obtained in real-time through the dual-extended Kalman filter (Dual-EKF). The proposed system does not use accelerometers and thus avoids the acceleration noises. Also, the system uses a high-precision gyro to improve the navigation accuracy.Simulation results show that the proposed technique is able to obtain the rover navigation information in real time, and it overcomes the two shortcomings of more traditional navigation methods: the discrete output (of pure celestial navigation) and cumulative error (of inertial navigation).



---

*Source: 578719-2012-04-30.xml*