Advanced
Terrain Slope Estimation Methods Using the Least Squares Approach for Terrain Referenced Navigation
Terrain Slope Estimation Methods Using the Least Squares Approach for Terrain Referenced Navigation
International Journal of Aeronautical and Space Sciences. 2013. Mar, 14(1): 85-90
Copyright ©2013, The Korean Society for Aeronautical Space Science
This is an Open Access article distributed under the terms of the Creative Commons Attribution Non-Commercial License (http://creativecommons.org/licenses/bync/3.0/) which permits unrestricted non-commercial use, distribution, and reproduction in any medium, provided the original work is properly cited.
  • Received : January 07, 2013
  • Accepted : March 25, 2013
  • Published : March 30, 2013
Download
PDF
e-PUB
PubReader
PPT
Export by style
Share
Article
Author
Metrics
Cited by
TagCloud
About the Authors
Sung-Hoon Mok
shmok@ascl.kaist.ac.kr
Hyochoong Bang

Abstract
This paper presents a study on terrain referenced navigation (TRN). The extended Kalman filter (EKF) is adopted as a filter method. A Jacobian matrix of measurement equations in the EKF consists of terrain slope terms, and accurate slope estimation is essential to keep filter stability. Two slope estimation methods are proposed in this study. Both methods are based on the least-squares approach. One is planar regression searching the best plane, in the least-squares sense, representing the terrain map over the region, determined by position error covariance. It is shown that the method could provide a more accurate solution than the previously developed linear regression approach, which uses lines rather than a plane in the least-squares measure. The other proposed method is weighted planar regression. Additional weights formed by Gaussian pdf are multiplied in the planar regression, to reflect the actual pdf of the position estimate of EKF. Monte Carlo simulations are conducted, to compare the performance between the previous and two proposed methods, by analyzing the filter properties of divergence probability and convergence speed. It is expected that one of the slope estimation methods could be implemented, after determining which of the filter properties is more significant at each mission.
Keywords
1. Introduction
Terrain referenced navigation (TRN) estimates an aircraft status, by using a radar altimeter to measure distance between terrain and the aircraft. TRN has been widely investigated since the 1980s [1 - 5] , and it is usually integrated with an inertial navigation system (INS), to prevent drift error. The integration scheme is similar to the well-known GPS/ INS navigation. TRN compensates for drift, and provides a bounded navigation solution.
Filtering methods, such as a Kalman filter (KF), can be applied to TRN. However, because of the nonlinearity of the measurement equation, the KF cannot be used directly. Alternatively, an extended KF (EKF) linearizing measurement equation could be adopted. The linearized equation requires terrain slope calculation, for which the slope could be estimated, using the terrain map. When the position error between true and estimated positions is small, the estimated slope would be accurate; however, because of terrain nonlinearity, the accuracy may degrade as the position error increases. Because the Jacobian matrix of measurement equations in the EKF consists of the terrain slope, inaccuracy of the slope estimation could break the filter consistency. In other words, conventional EKF may no longer be valid, with large position error covariance. To overcome such a problem, various strategies have been employed in TRN, such as a bank of KFs [1] , a particle filter [2] , and an unscented KF [3] .
In addition, Hostetler and Anderas [4] proposed the slope estimation method, using the least squares in EKF-based TRN. The method searches the best slope, in the least squares sense, representing the terrain map over the linearization region determined by the position error covariance. Reference [5] utilizes linear regression, composed by two least squares at latitude and longitude axes, respectively. In this paper, two slope estimation methods modifying the previous method [5] are proposed. One is planar regression, estimating the slope using a plane, instead of the two lines of the previous linear regression. The method could incorporate correlation of the two axes, so the slopes could be more accurate than the previous one. The other method is weighted planar regression. By considering that the state is estimated using a Gaussian probability density function (pdf) in a KF, additional weights formed by the Gaussian pdf are multiplied to the proposed planar regression. Two Monte Carlo simulations are performed, to compare the performance between the previous and two proposed methods, by considering the filter divergence probability and convergence speed.
This paper is summarized as follows. Strapdown INS kinematics and EKF-based TRN are introduced in Section 2. In Section 3, the previous and two proposed slope estimation methods are discussed. Monte Carlo simulations are conducted to compare performances between the three methods in Section 4, and a conclusion is given in Section 5.
2. Extended Kalman Filter Formulation
Strapdown INS kinematics are adopted to propagate aircraft status. An error-state is propagated, instead of the full-state, and the kinematic equations are described as
Lager Image
The error-state
Lager Image
is the difference between the estimated state
Lager Image
and true state x .
The term
Lager Image
is composed of of errors of position, velocity, Euler angle, IMU biases and barometer bias. w is a noise vector depending on the IMU specification, and components of F and G matrices determined by the aircraft status are derived in [6] .
Measurements equations expressing the sensor outputs of radar altimeter and pressure altimeter are such that
Lager Image
where,
Lager Image
are sensor outputs, random noise and bias terms, respectively. ϕ , λ and h are latitude, longitude and height, respectively. Function h DB ( ϕ , λ ) provides a terrain altitude at the ( ϕ , λ ) position. Eq. (2) can be re-written in error-state form, by linearizing the equation at
Lager Image
as
Lager Image
where,
Lager Image
Now, the error-state EKF can be directly formulated using kinematic equations, measurement equations and Jacobian matrices F , G and H .
3. Terrain Slope Estimation Methods
The Jacobian matrices in Eqs. (1) and (4) are statedependent, and this may cause filter divergence. In TRN in general, nonlinearity is more profound in measurement equation, than in kinematics. Jacobian H consists of terrain slope terms, so accurate slope estimation is essential to preserve filter stability. With the imprecise slope estimation, false fix of EKF could occur, and the filter could become unstable.
In conventional EKF, terrain slopes are estimated by taking partial derivatives along the latitude and longitude at the estimated position
Lager Image
This method is valid when
Lager Image
however, estimation accuracy decreases as the position error becomes larger. To remedy such a problem, stochastic linearization (SL) is suggested in [4] . SL utilizes the least squares method to obtain the slopes. Fig. 1 depicts the method. In [5] , two linear regressions are performed to obtain the slopes.
Lager Image
where, S Line,ϕ and S Line,λ are the sums of squares of residual errors along the two axes, respectively. Nx and Ny are the number of points to be used in least squares. The linearization region related to position covariance determines Nx and Ny .
Lager Image
denote each position in the estimated region, and their mean position is represented by
Lager Image
Terrain
Lager Image
Terrain slope estimation, using least squares
slopes are defined by a , b , and optimal slopes minimizing the sum of squares can be obtained using ∂SLine,ϕ / ∂a =0, ∂S Line, λ / ∂b =0 conditions.
Lager Image
The variance of residual error shown in Fig. 1 can be given by
Lager Image
which is added to the sensor noise covariance, to incorporate the linearization error of slope estimation.
- 3.1 Planar Regression Method
In previous research [5] , two linear regressions are applied, to derive terrain slopes at each axis. One axis is fixed, while the slope of the other axis is computed. For example, longitude is fixed at the mean position
Lager Image
when latitude slope â Line is obtained in Eq. (6). However, as the linearization region becomes larger, estimated slopes become inaccurate, because variation of the terrain altitude is coupled in the two axes. For such a case, a planar (bilinear) regression method could be proposed, to consider the coupling as
Lager Image
The two separate sums in Eq. (5) are merged into one summation over the linearization region. The optimal slopes minimizing can be found by
Lager Image
In comparison with the linear regression of Eq. (6), the longitude
Lager Image
is no longer fixed to the mean value in latitude slope estimation. The linearization error of planar regression is
Lager Image
The planar regression method using a plane instead of two lines could reflect the correlation of two axes in terrain altitude. By doing so, estimated slopes could be estimated more accurately than using linear regression, and it could enhance filter stability.
- 3.2 Weighted Least Squares Method
In a Kalman filter, the state is estimated based on a Gaussian probability density function (pdf). This characteristic could be applied in terrain slope estimation. In Eq. (8), the square of residual error is computed at each position in the linearization region. However, the probabilities of the estimated positions are different. To incorporate the difference, a Gaussian pdf can be utilized to generate a weight term at each position. By augmenting the weight, Eq. (8) can be modified to a weighted least squares problem, as
Lager Image
where,
Lager Image
Lager Image
is a 2×2 matrix consisting of the estimated position
Lager Image
Terrain slope estimation, using unweighted least squares
Lager Image
Terrain slope estimation, using weighted least squares
covariance. The corresponding optimal slopes, and variance of linearization errors, are given by
Lager Image
Lager Image
Figures 2 and 3 depict the two slope estimation methods, unweighted least squares (LS) and weighted least squares (WLS). The estimated slopes are different, in spite of the same terrain and position covariance. The weighted residual error is represented by the shaded area. We can see that the weights are even in LS, whereas larger weights are multiplied for adjacent regions in WLS. From a probabilistic view, the probability that the true position is in the near region is higher, than in the far region. So, commonly, the slope of WLS could be more accurate than that of LS, and the filter convergence speed could be faster. However, the true position might be distant, and in that case, the conservative LS solution could be more applicable in maintaining filter stability. In fact, the true position is unknown, so in the next section, Monte
Lager Image
Trajectory samples
Lager Image
Estimated position samples
Carlo simulation is conducted for various position errors, to compare the filter stability and convergence time between the two methods.
4. Simulation Results
The two proposed methods, planar regressions with unweighted and weighted least squares, are applied in artificial missions. Monte Carlo simulation is performed by varying the initial position, and estimated error of the initial position. Figure 4 describes 101 mission trajectories, with various starting points. The other simulation conditions are summarized in Table 1 . Initial position errors at each starting point are drawn in Fig. 5 . Eight samples are positioned in each error radius, defined by multiplication of the estimated standard deviation of the position error σ . The other initial errors are defined in Table 2 . In addition, an initial error covariance is initialized with square of the errors, except that the position covariance is fixed, with the square of the smallest error radius σ 2 .
- 4.1 Planar Regression Method
A performance comparison between the linear regression and planar regression is conducted. Table 3 represents the probability of filter divergence and convergence time according to the initial error radius in EKF, with the two methods. The convergence is defined as the position error in some boundary after a certain time, called the convergence time. In this simulation the grid distance of the map, 90m, is applied as the boundary. The last column in Table 3 denotes the weighted means of results at each initial error. A Gaussian pdf is applied as weights, and the values are 0.242 at 1 σ error radius, 0.054 at 2 σ , and 0.004 at 3 σ .
In Table 3 , the probability of divergence increases as the position error grows, because the difference between the slopes at true and estimated positions becomes
Simulation conditions
Lager Image
Simulation conditions
Initial state errors
Lager Image
Initial state errors
Filter properties of linear and planar regressions
Lager Image
Filter properties of linear and planar regressions
larger. Between the two slope estimation methods, the convergence times are similar, but the divergence probability is significantly reduced when the planar regression is employed. The simulation verifies that the slope could be estimated more accurately by using a plane, instead of two lines, and filter stability could be improved. Figures 6 and 7 describe the longitude error histories and planar trajectories at specific conditions, at 127.68° longitude and 300m initial error. In Fig. 6 , the longitude error is well-bounded by the 3-sigma line with planar regression, but the filter is inconsistent and unstable in linear regression. The convergence characteristics can also be found in Fig. 7 .
Lager Image
Longitude error histories of linear (upper), and planar (bottom), regressions
Lager Image
Two-dimensional trajectories of linear and planar regressions
- 4.2 Weighted Least Squares Method
The filter properties of planar regressions with unweighted least squares (LS) and weighted least squares (WLS) are shown in Table 4 . Simulation conditions and initial errors are summarized in Tables 1 and 2 . In addition, the results of LS in Table 4 are identical with the planar regression results of Table 3 . In Table 4 , the convergence time is reduced about 20% in the smallest initial error 1 σ , when WLS is used instead of LS, while maintaining zero divergence probability. WLS uses larger weights than LS in adjacent regions, so the estimated slope could be more accurate when the initial error is small. Accurate estimation could make the filter convergence
Lager Image
Longitude error histories of unweighted (upper), and weighted (bottom), least squares
Lager Image
Two-dimensional trajectories of unweighted and weighted least squares
Filter properties of unweighted and weighted least squares
Lager Image
Filter properties of unweighted and weighted least squares
speed faster. Figures 8 and 9 represent longitude errors and trajectories with 1 σ initial error at 127.5° longitude. In both graphs, the estimated position approaches the true position quicker with WLS, than with LS.
However in Table 4 , as initial error increases, the difference of convergence speeds shrinks, and the divergence probability of WLS rises more rapidly than of LS. The less stable performance than LS in large initial error is because the WLS applies small weights for the far region. For the large error case, slope estimation with LS could be more effective in preserving filter stability. In summary, the superiority between LS and WLS depends on the initial error magnitudes. In the last column in Table 4 , two filter properties are averaged over various initial errors. Overall, the probability of divergence is lower with LS, but the convergence speed is faster with WLS. One of the slope estimation methods could be implemented, after determining which of the filter properties is more significant, at each mission.
5. Conclusion
Terrain slope estimation methods based on least squares are developed. One is the planar regression, which improves upon the previous linear regression, and the other is the weighted planar regression, which adds Gaussian-shaped weights to the proposed planar regression. Two Monte Carlo simulations are conducted to compare the performances between the previous and two proposed methods, by considering the filter divergence probability and convergence speed. In the first simulation, the probability of divergence is reduced, when planar regression is employed instead of linear regression. In the second simulation, the performance difference between planar regressions with weighted and unweighted least squares changes, as the initial error radius varies. After taking the averages over various initial errors, it is shown that the convergence time is shorter with weighted least squares; but the probability of divergence is lower with unweighted least squares. By considering which of the filter properties is more important at missions, either of the proposed slope estimation methods could be implemented.
Acknowledgements
This research was funded by the Korea Agency for Defense Development (ADD), under project code ADDR-417-101671.
References
Metzger J. , Wendel J. , Trommer G. F. , Tumbragel F. , Taddiken B. 2004 “Hybrid Terrain Referenced Navigation System using a Bank of Kalman Filters and a Comparison Technique” AIAA Guidance, Navigation and Control Conference Providence, Rhode Island, USA
Bergman N. , Ljung L. , Gustafsson F. 1997 “Point-Mass Filter and Carmer-Rao Bound for Terrain-Aided Navigation” Conference on Decision & Control San Diego, California, USA
Metzger J. , Wisotzky K. , Wendel J. , Trommer G. F. 2005 “Sigma-Point Filter for Terrain Referenced Navigation” AIAA Guidance, Navigation, and Control Conference and Exhibit San Francisco, California, USA
Hostetler L. , Andreas R. 1983 “Nonlinear Kalman Filtering Techniques for Terrain-Aided Navigation” IEEE Transactions on Automatic Control 28 (3) 315 - 323    DOI : 10.1109/TAC.1983.1103232
Mok S. H. , Choi M. , Bang H. 2011 “Performance Comparison of Nonlinear Estimation Techniques in Terrain Referenced Navigation” KINTEX Proceedings of the 11th International Conference on Control, Automation and System Gyeonggi-do, Korea 1244 - 1249
Titterton D. H. , Weston J. L. 2004 Strapdown Inertial Navigation Technology, The Institution of Electrical Engineers Reston Virginia, USA 17 - 58