Terrain Slope Estimation Methods Using the Least Squares Approach for Terrain Referenced Navigation

  • cc icon
  • 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.


  • KEYWORD

    Terrain referenced navigation , Extended Kalman filter , Terrain Slope Estimation , Least squares method

  • 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

    image

    The error-state

    image

    is the difference between the estimated state

    image

    and true state x.

    The term

    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

    image

    where,

    image

    are sensor outputs, random noise and bias terms, respectively. , λ and h are latitude, longitude and height, respectively. Function hDB(, λ) provides a terrain altitude at the (, λ) position. Eq. (2) can be re-written in error-state form, by linearizing the equation at

    image

    as

    image

    where,

    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

    image

    This method is valid when

    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.

    image

    where, SLine, and SLine,λ 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.

    image

    denote each position in the estimated region, and their mean position is represented by

    image

    Terrain

    slopes are defined by a, b, and optimal slopes minimizing the sum of squares can be obtained using ∂S Line,?/∂a=0, ∂SLine, λ∂b=0 conditions.

    image

    The variance of residual error shown in Fig. 1 can be given by

    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

    image

    when latitude slope aLine 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

    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

    image

    In comparison with the linear regression of Eq. (6), the longitude

    image

    is no longer fixed to the mean value in latitude slope estimation. The linearization error of planar regression is

    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

    image

    where,

    image
    image

    is a 2×2 matrix consisting of the estimated position

    covariance. The corresponding optimal slopes, and variance of linearization errors, are given by

    image
    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

    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

    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.

       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

    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.

  • 1. 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] google
  • 2. Bergman N., Ljung L., Gustafsson F. 1997 “Point-Mass Filter and Carmer-Rao Bound for Terrain-Aided Navigation” [Conference on Decision & Control] google
  • 3. 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] google
  • 4. Hostetler L., Andreas R. 1983 “Nonlinear Kalman Filtering Techniques for Terrain-Aided Navigation” [IEEE Transactions on Automatic Control] Vol.28 P.315-323 google doi
  • 5. Mok S. H., Choi M., Bang H. 2011 “Performance Comparison of Nonlinear Estimation Techniques in Terrain Referenced Navigation” [Proceedings of the 11th International Conference on Control, Automation and System] P.1244-1249 google
  • 6. Titterton D. H., Weston J. L. 2004 Strapdown Inertial Navigation Technology, The Institution of Electrical Engineers P.17-58 google
  • [Fig. 1.] Terrain slope estimation, using least squares
    Terrain slope estimation, using least squares
  • [Fig. 2.] Terrain slope estimation, using unweighted least squares
    Terrain slope estimation, using unweighted least squares
  • [Fig. 3.] Terrain slope estimation, using weighted least squares
    Terrain slope estimation, using weighted least squares
  • [Fig. 4.] Trajectory samples
    Trajectory samples
  • [Fig. 5.] Estimated position samples
    Estimated position samples
  • [Table 1.] Simulation conditions
    Simulation conditions
  • [Table 2.] Initial state errors
    Initial state errors
  • [Table 3.] Filter properties of linear and planar regressions
    Filter properties of linear and planar regressions
  • [Fig. 6.] Longitude error histories of linear (upper), and planar (bottom), regressions
    Longitude error histories of linear (upper), and planar (bottom), regressions
  • [Fig. 7.] Two-dimensional trajectories of linear and planar regressions
    Two-dimensional trajectories of linear and planar regressions
  • [Fig. 8.] Longitude error histories of unweighted (upper), and weighted (bottom), least squares
    Longitude error histories of unweighted (upper), and weighted (bottom), least squares
  • [Fig. 9.] Two-dimensional trajectories of unweighted and weighted least squares
    Two-dimensional trajectories of unweighted and weighted least squares
  • [Table 4.] Filter properties of unweighted and weighted least squares
    Filter properties of unweighted and weighted least squares