AIAA 2011-6579 AIAA Guidance, Navigation, and Control Conference 08 - 11 August 2011, Portland, Oregon Distributed Extended Kalman Filtering for Reliable Navigation on Lunar Surface Ali Hooshmand∗ Javad Mohammadpour† Heidar Malki‡ University of Houston, Houston, TX 77004 Robert S. Provence§ Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 NASA Johnson Space Center, Houston, TX This paper presents a reliable surface navigation algorithm based on distributed extended Kalman filtering (DEKF) method. The proposed algorithm enables the network of sensors on the lunar surface to act as a collective observer for positioning the rovers or astronauts, making the navigation process more reliable by eliminating the need to a centralized estimator. The performance and effectiveness of the DEKF will be compared to those of an extended Kalman filter (EKF) implemented in a central manner by making use of the simulation data collected from the model of a network of rovers moving on the lunar surface. I. Introduction One of the most important steps in NASA vision for space exploration has been the safe travelling of astronauts to the moon. NASA is continuously examining new methods for precise exploration of the lunar and Mars surface. On earth, the Global Positioning System (GPS) allows its users to quickly determine their location and to plot courses of travel.1 However, the moon does not have the satellite network required to duplicate terrestrial GPS. Hence, new techniques for navigation, different than those used in terrestrial practice, must be developed for lunar surface missions. One feasible way is to install radio receivers, inertial measurement units (IMUs), and embedded processors to perform navigation calculations directly in the astronauts’ suit or rovers’ body.2, 3 In order to process the data collected by either the radiometric or inertial methods and generate a dynamic position, the Kalman filtering methods have been employed.4 In 1960, Prof. Kalman published his famous paper5 marked to be a significant contribution to the field of linear filtering by removing the stationary requirements of the Weiner filter and presenting a sequential solution to the time-varying linear filtering problem. The Kalman filter in its various forms has now become a fundamental tool to analyze a broad class of estimation problems. There have also been a number of modifications to the original version of the Kalman filter and been applied to many applications including aerospace engineering. For instance, extended Kalman filter (EKF) and unscented Kalman filter proved to be more accurate than the original Kalman filter depending on the specific conditions on the navigation system.6, 7 It is important to note that the navigation of astronauts or rovers on the moon surface should be extremely precise since missing sensor measurements for even periods of time can cause a large cumulative error in positioning and eventually losing them in the deep darkness of moon. Therefore, different types of measurement sensors in parallel to each other are used to guarantee receiving the data needed for the navigation purpose. Furthermore, instead of only one rover, there might be a team of rovers responsible for different tasks on the surface, all of which need to be navigated. Hence, the navigation process requires the use of a network of sensors. In this case, the aforementioned improvements of Kalman filtering such as EKF and unscented Kalman filter suffer from the dependency on a central processing unit responsible for performing data fusion and further analysis. In a centralized framework, it may be impossible for every sensor to keep contact with the central processing unit. In addition, every slack of the central processor can potentially cause disruption of positioning process. In the present paper, we propose a method to decentralize the position estimation process by using a distributed filtering method to eliminate the need to a central processing unit. ∗ Graduate Research Assistant, Department of Electrical Engineering, University of Houston, Houston, TX. Assistant Professor, Department of Mechanical Engineering, University of Houston, Houston, TX. ‡ Professor, Department of Engineering Technology, University of Houston, Houston, TX. § Aerospace Engineer at Aeroscience & Flight Mechanics Division, NASA Johnson Space Center, Houston, TX. † Research 1 of 10 Copyright © 2011 by Javad Mohammadpour. Published by the American Institute of Aeronautics and Astronautics, Inc., with permission. American Institute of Aeronautics and Astronautics Distributed filtering is a collaborative information processing task in sensor networks. Multi-sensor fusion and tracking problems have a long history in control theory and robotics. Decentralized filtering involves the use of a set of local filters that communicate with all other nodes.8 In this work, we will focus on extending the idea of DKF to navigate the systems with nonlinear dynamic models by developing a distributed extended Kalman filter (DEKF), in which each rover can only exchange information with its neighboring rovers on the network with a known topology.9 The paper is organized as follows: Section 2 provides a brief background on EKF method used to estimate the states of a nonlinear system. Distributed extended Kalman filtering method is then described in Section 3. Simulation results using EKF and DEKF on the model of a network of rovers with four different sensing modalities are presented in Section 4. Section 5 concludes the paper. Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 II. Extended Kalman Filter Due to the nonlinear nature of the system dynamics describing the position (x, y, z) of a rover, estimation of the states in the central manner is improved by the use of an EKF. In EKF, the system and measurement equations in general form are given as follows:10, 11 xk = fk−1 (xk−1 , uk−1 , wk−1 ) yk = hk−1 (xk , vk ) wk ∼ (0, Qk ) vk ∼ (0, Rk ) where the functions f and h are, in general, nonlinear. The state vector of the system and the input signal are represented by x and u, respectively. The process disturbance input w is a white noise with zero mean and covariance Q, y is the output signal, and v is the measurement noise with zero mean and covariance R. Then, by initializing as follows: x̂+ 0 = E(x0 ) + T P0+ = E[(x0 − x̂+ 0 )(x0 − x̂0 ) ] where x̂ is the state estimate, + is the symbol indicating a measurement update, and P is the estimation error covariance matrix, the EKF algorithm is implemented using an iterative procedure for k = 1, 2, . . .: 1. Linearize the nonlinear state equation at the measurement state of previous step: Fk−1 = Lk−1 = ∂fk−1 + ∂x |x̂k−1 ∂fk−1 + ∂w |x̂k−1 2. Update the state estimate and estimation error as follows: + T + Lk−1 Qk−1 LTk−1 Pk− = Fk−1 Pk−1 Fk−1 − + x̂k = fk−1 (x̂k−1 , uk−1 , 0) in which − represents the updated variable. 3. Linearize the nonlinear measurement equation at the time updated state of current step: Hk = Mk = ∂hk | − ∂x x̂k ∂hk | − ∂v x̂k 4. Update the state estimate and estimation error covariance matrix as follows: Kk x̂+ k Pk+ = Pk− HkT (Hk Pk− HkT + Mk Rk MkT )−1 − = x̂− k + Kk [yk − hk (x̂k , 0)] = (I − Kk Hk )Pk− . 2 of 10 American Institute of Aeronautics and Astronautics III. Distributed Extended Kalman Filter In this section, we present a new distributed method to estimate the positions of rovers using a network of sensors. The aforementioned sensor network can be a network of sensors embedded in rovers’ bodies (multi-rover navigation problem) or other types of measurement sensors. In this network, information from all the sensors is shared among their neighboring nodes. This allows the network of sensors to collectively and in a distributed way calculate the same state estimate x̂ obtained via the application of a centralized Kalman filter. Let us consider zi (k) = Hi (k)x(k) + vi (k) to be the sensing model for the ith sensor in the network, with x(k) denoting the state of its linear dynamic process driven by x(k) = Ak−1 x(k − 1) + Bk w(k − 1) Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 or in a nonlinear form by x(k) = fk−1 (x(k − 1), u(k − 1), w(k − 1)). In a more general case, a nonlinear sensing model can be considered as zi (k) = hi (x(k)) + vi (k). Both w and v are assumed to be zero-mean white Gaussian noise signals. The statistics of the measurement noise is given by E[w(k)wT (l)] = Q(k)δkl E[vi (k)vjT (l)] = Ri (k)δkl δij where δkl = 1 for k = l and δkl = 0 otherwise. We assume that ith sensor in the network can share its measurement zi , covariance information Ri , and output matrix Hi with its neighbors Ni . For the comparison purposes, a local Kalman filter is used as a basis for the distributed Kalman filtering algorithms. In local Kalman filtering, sensor i assumes that no other sensors than its neighbors exist as the information flow source, and hence,Ssensor i can use a central Kalman filter that only uses the observation and the output matrices of the nodes in Ji = Ni {i}. The question is to determine the local Kalman filters of the following form: − − x̂i (k) = x+ i (k) = xi (k) + Mi (k)[yi (k) − Si (k)xi (k)] x− i (k + 1) = Ak x̂i (k) where x̂i (k) is the estimate of the state vector corresponding to the ith sensor at step k and x− i (k) is the time updated state of the ith sensor at step k. In this equation, the average measurements yi , average inverse-covariance matrix Si , and distributed Kalman filter gain Mi need to be updated in real-time. Assume that the sensors in the network solve two consensus problems that allow them to calculate Si and yi at each iteration. Then, every sensor can calculate the state estimate x̂i at the iteration k using the update equations of its local Kalman filter. Using a high-gain version of dynamic consensus algorithm of Spanos et al.12 to perform distributed averaging allows us to have different matrices Hi across the entire network. It means that the process with state x(k) can be collectively observable by all the sensors. Therefore, the architecture of local filters on each sensor is similar to the one illustrated in figure 1.9 Let Ni = {j : (i, j) ∈ E} be the set of neighbors of sensor i on the graph denoted by G. Moreover, let L be the Laplacian matrix corresponding to the graph G. The high-pass consensus filter is a linear system in the form ( P P q̇i = β j∈Ni (qj − qi ) + β j∈Ni (uj − ui ) β>0 (1) pi = q i + u i where ui is the input to the ith sensor, qi is the state of the consensus filter (CF), and pi is its output. The gain β > 0 is relatively large for randomly generated ad hoc topologies that are rather sparse. The collective dynamics of this CF is given by ( q̇ = −βLq − βLu (2) p=q+u Proposition 1 9 : Consider the LTI system with input u(t) described by ṗ = −Lp + u̇ p(0) = u(0) 3 of 10 American Institute of Aeronautics and Astronautics (3) Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 1. The architecture of the local filters implemented in each sensor for DKF algorithm9 whose transfer function is Hpu = s(sI + L)−1 with L being the Laplacian of the connected undirected graph. Suppose the input signal U (s) has all its poles in the left half-plane and has at most one pole at s = 0. Then, for any i P limt→∞ (pi (t) − N1 j uj (t)) = 0. That is, each agent tracks the dynamic consensus with zero steady-state error. Considering the above proposition and defining q = p − u, we can obtain collective dynamics of CF in Eq. (2) from Eq. (3), and consequently the high-pass consensus filter equation (1). Finally, in order to find average measurements T −1 yi , in the S first high-pass consensus filter in figure 1, uj in Eq. (1) will have to be replaced by Hj Rj zj for any j ∈ Nj {i} which means sending information on output matrix, covariance and measurement information of the nodes in the neighborhood of the ith node as inputs to the ith sensor. Also, pi should be replaced by yi . Therefore, we have P limt→∞ (yi (t) − N1 j HjT Rj−1 zj (t)) = 0. Similarly, for calculating average inverse-covariance Si , in the second high-pass CF, we need to substitute uj with S HjT Rj−1 Hj for any j ∈ Nj {i}, and pi with Si . So, we determine the following as the result: limt→∞ (Si (t) − 1 N P j HjT Rj−1 Hj (t)) = 0. The following algorithm summarizes the DEKF algorithm with identical high-pass consensus filtering of measurement data for a nonlinear system. It is noted that β is a positive constant and T is the sampling time. 4 of 10 American Institute of Aeronautics and Astronautics 1. Initialization: qi = 0, ζi = 0m×m , Pi = P0 , x− i = x(0) where m is the dimension of the state x. 2. While new data exists do steps 3 to 6. 3. Update the state of the data in CF: qi [ HjT Rj−1 zj ∀j ∈ Nj {i} X X (uj − ui )] (qj − qi ) + β ← qi + T [β yi = uj = j∈Ni j∈Ni qi + ui . 4. Update the state of the covariance in CF: Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 ūj ζi [ HjT Rj−1 Hj ∀j ∈ Nj {i} X X (ūj − ūi )] (ζj − ζi ) + β ← ζi + T [β = j∈Ni Si = j∈Ni ζi + ūi . 5. Update the state estimate using local Kalman filters: Mi x̂i = (Pi−1 + Si )−1 − = x− i + Mi (yi − Si xi ). 6. Update the estimate of the system state vector: x− i ← f (x̂i ) ∂f |x̂ A = ∂x i ∂f |x̂ B = ∂w i Pi = AMi AT + BQB T . IV. Simulation Results In this section, we present the results of estimating the position of a rover moving on the lunar surface using the EKF and DEKF methods described in the previous sections. Four different sensing modalities are used from the data provided by NASA-JSC. The provided data have been collected from a simulation model JSC has developed. A. Extended Kalman filter results Using different sensing modalities results in different measurement noises. The estimation results are shown obtained from the use of an EKF implemented in MATLAB using the measurement data corrupted with two different types of noise signals, namely Inertial Navigation System (INS) and Radio Frequency (RF).13, 14 The results are illustrated in figure 2 and figure 3, respectively. Simulation results in the two figures demonstrate the capability of EKF method to estimate the rover position using the limited noisy measurements from sensors. B. Distributed extended Kalman filter results In this part, we illustrate the results of implementing the DEKF for a sensor network, which includes four sensors leading to four major types of noise signals including INS model, RF model, directional RF model, and timing RF model. Each one of these four sensors can measure the position of the rover with a different noise characteristics. The sensing model for the ith node in the sensor network is as follows: yi (k) = Hi (k)x(k) + vi (k) in which x(k) denotes the discrete time state of the dynamic process. 5 of 10 American Institute of Aeronautics and Astronautics Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 2. The estimation results using an EKF with INS noise input 1. A fully connected sensor network First, it is assumed that the sensor network is a fully connected undirected graph, which implies that the Laplacian matrix is as follows: 3 −1 −1 −1 −1 3 −1 −1 L= −1 −1 3 −1 −1 −1 −1 3 In addition, the output matrix for all sensors are the same and as follows: " # 1 0 0 Hi = . 0 1 0 This means that all the sensors can measure only two dimensions of rover position (x, y). The result of the position estimation in this case is shown in figure 4. As observed, the DEKF algorithm provides a highly accurate estimate of the rover position using the four sensor measurements with different noise characteristics. 2. Dynamic sensor network In this section, we demonstrate the two important advantages of DEKF, namely reliability and collective observability. To show the reliability, we illustrate the estimation results of a dynamic sensor network. It is assumed that the network is initially fully connected and after a period of time, its configuration changes due to a failed connection between 6 of 10 American Institute of Aeronautics and Astronautics Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 3. The estimation results using an EKF with RF noise input the sensors or sensor movement on lunar surface. Hence, the Laplacian matrix of the network will change. Shown in figure 5 is the network configuration we consider for simulation studies. Corresponding to this network configuration, the Laplacian matrices are as follows: 3 −1 −1 −1 −1 3 −1 −1 L1 = −1 −1 3 −1 −1 −1 −1 3 2 −1 L2 = 0 −1 −1 0 L3 = 0 −1 −1 0 −1 3 −1 −1 −1 2 −1 −1 −1 3 0 0 −1 2 −1 −1 −1 2 −1 −1 −1 3 7 of 10 American Institute of Aeronautics and Astronautics Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 4. The estimation results using DEKF for a fully connected sensor network −1 0 0 −1 2 −1 −1 0 L4 = 0 −1 1 0 −1 −1 0 2 In addition, in order to show the collective observability of the DEKF algorithm, we assume that each sensor can only measure one dimension of position, either x or y. The following sensing models are considered for the four nodes in the network: h i H1 = 0 1 0 h i H2 = 1 0 0 h i H3 = 1 0 0 h i H4 = 0 1 0 where Hi is the output matrix corresponding to the ith sensor. Using one-dimensional measurements lead to the divergence of the EKF. On the other hand, DEKF still provides reliable position estimates as illustrated in figure 6. Simulation results of this case corroborate the capability of DEKF method to estimate the rover positions using limited noisy measurements of sensors. In addition, different sensing modalities with partial system information, as well as missing a sensor data will not interrupt the estimation process. This leads to a highly reliable estimate since it eliminates the need for a central controller. 8 of 10 American Institute of Aeronautics and Astronautics Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 5. Configuration of the sensor network V. Conclusions Presented in this paper is a reliable estimation method for lunar surface navigation based on a distributed extended Kalman filtering method. The distributed structure of the filtering method eliminates the need for a central unit to process the measurement data from sensor networks. Simulation results are employed to validate the distributed estimation algorithm and compare them with those of an EKF. The distributed filtering scheme can be employed to estimate the multiple rover positions, where some of the rovers exchange limited information with the neighboring ones in the network with a known configuration. The authors are currently investigating the effects of communication delay in estimation problem. VI. Acknowledgement The financial supports provided by NASA - Johnson Space Center and by the University of Houston Institute for Space Systems Operations (ISSO) are greatly appreciated. References 1 P. Daly, “Navstar GPS and GLONASS: Global satellite navigation systems,” Journal of Electron. Commun. Engrg., vol. 5, issue 6, pp. 349-357, Dec. 1993. 2 D.T. Chelmins, B.W. Welch, O.S. Sands, and B.V. Nguyen, “A Kalman approach to lunar surface navigation using radiometric and inertial measurements,” NASA technical report: gltrs.grc.nasa.gov, 2009. 3 K.B. Bhasin, J.D. Warner, and L.M. Anderson, “Lunar communication terminals for NASA exploration missions: Needs, operations concepts, and architectures,” in Proc. International Communications Satellite Systems Conference, 2008. 4 D.J. Simon, Optimal state estimation: Kalman, H , and nonlinear approaches, John Wiley and Sons, 2006. ∞ 5 R.E. Kalman, “A new approach to linear filtering and prediction problems,” Trans. ASME-Journal of Basic Engineering, pp. 35-45, Mar. 1960. 6 K. Lemon and B.W. Welch, “Comparison of nonlinear filtering techniques for lunar surface roving navigation,” NASA technical report: gltrs.grc.nasa.gov, 2008. 7 S.J. Julier and J.K. Uhlmann, “A new extension of the kalman filter to nonlinear systems,” in Proc. Int. Symp. Aerospace/Defense Sensing, Simul. and Controls, 1997. 8 B.S. Rao and H.F. Durrant-Whyte, “Fully decentralized algorithm for multi sensor Kalman filtering,” IEE Proceedings, Vol. 138, No. 5, Sep. 1991. 9 of 10 American Institute of Aeronautics and Astronautics Downloaded by UNIVERSITY OF ADELAIDE on October 28, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6579 Figure 6. The estimation results using DEKF for a collective observer dynamic sensor network 9 R. Olfati-Saber, “Distributed kalman filtering for sensor networks,” in Proc. 46th Conference on Decision and Control, pp. 5492-5498, New Orleans, LA, Dec. 2007. 10 D.J. Simon, Optimal state estimation, John Wiley and Sons, 2006. 11 J. Bellantoni and K. Dodge, “A square root formulation of the Kalman-Schmidt filter,” AIAA Journal, pp. 1309-1314, 1967. 12 D. Spanos, R. Olfati-Saber, and R.M. Murray, “Dynamic consensus on mobile networks,” in Proc. 16th IFAC World Congress, Prague, Czech, 2005. 13 B. Barshan and H.F. Durrant-Whyte, “Inertial navigation systems for mobile robots,” IEEE Trans. Robotics and Automation, 1995. 14 L. Li, E. Njoku, E. Im, P. Chang, and K.S. Germain, “A preliminary survey of radio-frequency interference over the US in Aqua AMSR-E data,” IEEE Trans. Geosci. Remote Sens., vol. 42, no. 2, pp. 380-390, Feb. 2004. 10 of 10 American Institute of Aeronautics and Astronautics

1/--страниц