Propagating Integrity Bounds in Nonlinear State Estimation Kyle O’Brien, Tufts University Jason Rife, Tufts University

BIOGRAPHY Kyle O’Brien is a graduate student pursuing his M.S. at Tufts University in the Department of Mechanical Engineering as a research assistant under Professor Jason Rife in the Automated Systems and Robotics laboratory. Before coming to Tufts, he earned his B.S. in Aerospace Engineering from Boston University and worked two years as a systems engineer at Raytheon Integrated Defense Systems. Jason Rife is an Assistant Professor of Mechanical Engineering at Tufts University in Medford, Massachusetts. He received his B.S. in Mechanical and Aerospace Engineering from Cornell University in 1996 and his M.S. and Ph.D. degrees in Mechanical Engineering from Stanford University in 1999 and 2004, respectively. After completion of his graduate studies, he worked as a researcher with the Stanford University GPS Laboratory, serving as a member of the Local Area Augmentation System (LAAS) and Joint Precision Approach and Landing System (JPALS) teams. At Tufts, he directs the Automation Safety and Robotics Laboratory, which applies theory and experiment to characterize the integrity of autonomous vehicle systems. ABSTRACT Future safety-critical GNSS applications will require state-estimation techniques that ensure integrity. Currently existing state estimators are not suitable for many high-integrity applications because of the way they propagate random variables through nonlinear system dynamics. The major difficulty in nonlinear stateestimation problems dealing with integrity lies in accounting for the nonlinear distortions to the error distribution. This paper proposes a state-estimation method that employs a Gaussian overbound with integrity ensured by an additional unsigned bias. More specifically, the paper focuses on only the prediction step of Bayesian estimation. The resulting predictor resembles a Kalman filter that propagates mean, covariance, and one additional term: the unsigned bias. This solution has the

benefit of being relatively computationally efficient while still retaining rigorous integrity. INTRODUCTION Achieving high levels of navigation integrity will be essential for future use of the GNSS (Global Navigation Satellite System) in safety-of-life (SoL) situations where rare-event errors pose considerable risk. Aviation applications include precision approach and landing, situational awareness and surveillance, PerformanceBased Navigation (PBN) procedures such as RNP (Required Navigation Performance) and RNAV (Area Navigation), all essential elements of the Next Generation air transportation system (NextGen). Automobile applications include lane-departure warnings and automobile navigation for urban environments. So far, much has been accomplished to enable integrity assurance for GNSS navigation. Examples include augmentations such as WAAS (Wide Area Augmentation System) and LAAS (Local Area Augmentation System), and GNSS-only fault-detection schemes such as RAIM (Receiver Autonomous Integrity Monitoring). Even with these integrity measures in place, high availability of precise navigation cannot be guaranteed at the desired accuracy level (or with the desired level of robustness to interference, multipath error, or temporary satellite occlusion). Worldwide integrity coverage is also a concern, as reflected in the Phase I report of the GNSS Evolutionary Architecture Study (GEAS) [1]. One way to provide greater availability of precise navigation is by combining vehicle dynamic models with GNSS (and other sensor measurements) using state estimation [2]. Though state estimation can significantly enhance raw GNSS accuracy, integrity in state estimation is not well developed. As Greer [3] and Borja [4] point out, more attention has been paid to the accuracy of estimates rather than their far-tail integrity. A major concern is the way random variables are propagated through nonlinear system dynamics, which can significantly distort the far tails of error distributions where the probability of rare faults is described. Greer makes substantial progress by highlighting linearization errors of the EKF (Extended Kalman Filter) and by demonstrating improvements that

the Sigma Point Kalman Filter (SPKF) makes to faultdetection schemes. Similarly, Yun provides an approach for better representing nonlinear propagation effects by introducing a Gaussian Mixture Model (GMM) for the error distribution [5]. In this way, a more accurate nonlinear propagation is made that can handle arbitrarily shaped distributions, given enough kernels in the Gaussian mixture. Non-parametric approaches such as the Particle Filter are also well equipped for describing arbitrary distributions, but are computationally intensive and potentially difficult to certify for safety-of-life applications (because they rely on a randomized Monte Carlo simulation procedure). To our knowledge, no prior state-estimation method exists that rigorously bounds the far tails of a state-error distribution. To fill this gap we investigate state propagation, the first step in Bayesian state estimation. Specifically, we propose a method for integrity-assured state propagation. The new method employs a Gaussian overbound with unsigned biases, which bound higherorder (nonlinear) distortions of the error distribution that occur during state propagation. This solution has the benefit of being relatively computationally efficient while still ensuring integrity. In the first part of this paper we describe the motivation for improving conventional state-estimation techniques. In the second part, we describe our proposed methodology for integrity-assured state propagation. The fourth part presents results of a simulated case study involving a planar robot. The final section will discuss potential applications and summarize this paper’s principal results. EXISTING STATE ESTIMATION METHODS The proposed integrity-assured state-propagation methodology is a variant of that used in well-established parametric state estimators such as the EKF [6] and UKF [7,8]. These existing methodologies treat the state-error distribution as Gaussian and thereby provide an approximation of the error distribution that is accurate in the core but not in the tails. Thus, the propagated error model is not necessarily suitable for determining a rigorous integrity bound. Although non-parametric state-estimation approaches, such as the Particle Filter (PF) [9], provide a more general description of error distributions, they suffer from efficiency and implementation issues that make them poorly suited for integrity applications. One of the biggest limitations of the PF is the raw number of samples required to adequately discretize a random variable. Typically, the number of samples required to characterize

integrity is several orders of magnitude larger than the inverse of the allowed integrity risk. For example, if the allowed integrity risk is 10-7, the distribution must be sampled hundreds of millions or billions of times in order to assure the associated error bound. Another limitation of the PF is "clustering," where resampling causes particles to gravitate towards areas of higher probability, making far-tail sampling even less efficient. Moreover, PF implementations rely on random number generation, which is problematic for formal safety validation. Given these limitations of the PF, we would like to define a new state-propagation method that leverages the simplicity and computational efficiency of other parametric approaches while still providing a rigorous integrity bound. Our proposed method builds upon the EKF, adding one additional recursive equation to propagate a bias bound (in addition to the familiar equations for propagating the system’s state and its covariance matrix). Because the proposed method builds on the EKF, we briefly review the EKF propagation step here. The EKF estimates the state of a discrete-time system with nonlinear dynamics, described by the following equation: x k  f x k 1 , u k , w k  .

(1)

Here the state vector x k at time step k is a function of the previous state x k 1 , control vector u k , and propagation noise vector w k with zero-mean Gaussian covariance matrix Q k . In EKF processing, the nonlinear system is approximated with a first-order Taylor representation. f  xk 1 , uk , w k   f  xˆ k 1 , uk , w k   Fk (xk 1  xˆ k 1 )  G k w k

(2)

Here the matrix Fk is a Jacobian with respect to x k 1 and G k is a Jacobian with respect to the noise input w k ; both are evaluated at the previous state estimate xˆ k 1 . The prediction step of the EKF is based on an exact (e.g. nonlinear) propagation of the most likely state xˆ k and a linearized propagation of its error covariance Pˆ . k

xˆ k  f xˆ k 1 , u k ,0 Pˆ k  Fk Pk 1FkT  G k Q k G Tk .

(3) (4)

INTEGRITY-ASSURED STATE PROPAGATION USING BIAS BOUNDING We propose an alternative propagation step that assures integrity by introducing an additional recursive equation, which propagates an unsigned bias. The proposed integrity-assured state propagation algorithm has the following form. xˆ k  f xˆ k 1 , u k ,0 

(5)

Pˆ k  Fk Pk 1FkT  G k Q k GTk

(6)

 k   k 1  m k

(7)

Here k is the cumulative bias bound up to and including time step k , and m k is the bias bound for time step k . Together with the covariance estimate, the propagated bias can be used to form a rigorous error bound at each time step. The remainder of this section outlines a brief derivation of the bias bound k . We start by noting that the limitation of the EKF error estimate arises from the linearized system model used to propagate the state covariance. If we compare the exact nonlinear propagation equation to its linearized model, we can isolate the discrepancy that causes error in the EKF covariance propagation step, described by the bias vector b k . xk  f xk 1, u k , w k   f xˆ k 1, uk ,0  Fk xk 1  xˆ k 1   G k w k  b k

xk 1 , w k



(11)

In essence, the role of the bias is to shift the distorted distribution back into a tractable Gaussian form. Each differential element of probability in the actual state-error distribution is shifted by the bias bk such that the resulting distribution is a Gaussian, which can be propagated precisely using a linearized state-space representation of the system dynamics. The bias bk varies for each value of state and propagation error. The upper bound m k compresses this complicated function into a single variable, which can be tracked in a computationally efficient manner. Note that biases accumulate over time. Since the upper bound m k does not preserve local magnitude or sign information, the bound on the accumulated bias, k , must assume that the individual biases at each time step align in the same direction with the worst possible magnitude. Thus, the accumulated bias bound should have the following form: k

μ k   mi .

(12)

(8) This upper bound can then be used to model navigation errors in a provably conservative fashion that accounts for the nonlinearities of the system and provides the integrity bound of equations (5) through (7). Methods for deriving error bounds that account for both a bias and a Gaussian covariance have been described in previous research [10,11] and are not described in detail here.

(9)

Unfortunately, the value of the bias b k cannot be computed explicitly because it is a function of the stateestimation error, which is itself uncertain. The propagation noise vector wk is also uncertain. However, it is still possible to compute an error bound if the maximum possible bias can be determined over the range of these two random inputs. Consider the case in which an upper bound m k on the magnitude of the bias can be computed such that

mk , j  bk , j .



mk , j  max bk , j

i 0

In other words, the bias vector b k explicitly accounts for the higher-order terms in the Taylor series expansion (2). Rearranging the above equation gives the following expression for b k . b k  f x k 1 , u k , w k   f xˆ k 1 , u k ,0   Fk x k 1  xˆ k 1   G k w k

In other words, each element j of the bounding vector must be larger in magnitude than the corresponding element of the actual bias. Such a bound can be computed by performing a maximization over all possible values of the actual previous state and the propagation noise.

(10)

Thus by using a bias bound, we can accurately model the residual estimation error as Gaussian at every time step. This Gaussian-bounding ability allows for rigorous error bounding using a formulation that is only a slight variant on the conventional EKF. PRACTICAL IMPLEMENTATION For certain systems, there may be practical issues with the computation of the bias term since an upper bound for the maximization in equation (11) may not exist. For these systems, we hypothesize that it is possible to define an integrity bound by limiting the range over which the

maximization is computed, e.g., by computing the bound based on state-estimation error and process noise according to the level of acceptable integrity risk. This relaxation of the bias bound to cover a limited range of random variable values is an approximation of the rigorous error bound derived in the previous section. No formal proof of integrity has yet been derived for this approximation; however, simulations are presented that demonstrate proof-of-concept. If the range of each random variable is limited to an interval , the modified integrity bound has the following form:

m k , j 

max

x k 1 1 , w k 2

b . k, j

(13)

In this paper we will allow the range of each random variable to be a multiple n of its standard deviation. If the initial state-error distribution is Gaussian with standard deviation x, and the state propagation distribution is Gaussian with standard deviation w, then the closed intervals are 1   n x , n x  and  2  n w , n w  . This “relaxed” maximization reduces the size of the bias bound. To be conservative, we recommend choosing n to be larger than the multiple of the standard deviation that corresponds to the allowed integrity risk. This intuitive concept will be verified using Monte Carlo simulations that measure conservatism in the error distribution tails. GROUND ROBOT APPLICATION To provide an example of the proposed state-propagation methodology, we now consider a simple nonlinear vehicle model. Specifically, we consider a two-wheel robot well described by a planar kinematic motion model. The state of such a robot is defined by three variables including xk and yk , which denote the robot’s location in the plane, and  k , which defines its heading as illustrated in the figure below. The complete state vector is:  xk    x k   yk  .    k

(14)

Figure 1 Robot Coordinate System. The robot is controlled by a velocity command vector u k having two components: translational velocity  k (which is assumed to be aligned in the same direction as the robot’s current heading,  k ) and rotational velocity k .   uk   k   k 

(15)

Propagation noise for each input is assumed to be zeromean and Gaussian distributed. The additive noise vector is labeled wk. The total input, which combines both the control signal and the noise, is denoted with a tilde.    u k   k   u k  w k   k 

(16)

The following motion model describes the state transition over time step t . This model is an analytical kinematic solution, which assumes zero lateral velocity [9]. x k  x k 1  g x k , u k , w k 

~  ~k    ~ sin  k 1  ~k sin k 1  ~k t  k  k   ~k  ~k ~ g  k , u k , w k    ~ cos  k 1  ~ cos k 1   k t     k  k  ~k t      

(17)

The proposed state propagation method, as described by equations (5) through (7), can now be evaluated using this kinematic robot model. The derivation of the bias term is non-trivial, so in this paper, we consider two specific instances of the model. In both cases, an initial state error is assumed. In the first (Case 1), the propagation error vector wk is assumed to be zero. In the second case (Case 2), the propagation error for velocity is allowed to be nonzero. However, the propagation error for angular velocity remains zero. Derivations of the bounds are given in Appendix A, for Case 1, and in Appendix B, for Case 2.

were extracted from the resulting population using conventional statistical methods. MONTE CARLO SIMULATION A Monte Carlo simulation was performed with MATLAB to demonstrate the bounds of the proposed integrity method and to verify that the relaxed maximization yields a conservative bound. For comparison, far tail error bounds were also computed using the EKF and a Particle Filter. The Monte Carlo distribution serves as “ground truth,” our best estimate of the actual error distribution. For the Monte Carlo simulation, 105 sample points were used. To initialize the simulation, these samples were drawn from an initial Gaussian distribution with the following covariance P and mean x.

Pk 0

1 0 0    0 1 0  102  0 0 500 

x k 0

0    0   0 

(18)

The theta variance was chosen to be very high in order to exaggerate the nonlinear effects. Points were propagated through the robotic system model with discrete time steps of length t  0.1 time units and a constant control command designed to steer the robot in a circle: 1 uk    . 1

(19)

The baseline Monte Carlo simulation was used to assess the conservatism of each of the three state propagation methods under consideration: the EKF, the Particle Filter, and our proposed biased-Gaussian method. For the EKF, the robotic model of equation (17) was used to propagate the state assuming zero noise. The covariance was propagated linearly as in equation (4). For the Particle Filter, equation (17) was used to propagate each particle in a population size of 103. This number was chosen to be two orders of magnitude smaller than the number of particles used in the Monte Carlo simulation; this choice reflects our assumption that, in a practical implementation, the particle count will be restricted to a number much smaller than needed to fully resolve the distribution far tails. The particles were initialized from the above covariance matrix of equation (18). After each time step, the state and covariance matrix

Lastly, for the biased-Gaussian integrity method, the state was propagated in the same manner as the EKF. A zeroorder accurate linearization was used, however, to simplify computation of the bias term. Based on this linearization (see Appendix A for Noise Case 1 and according to Appendix B for Noise Case 2), the covariance and bias terms were propagated in the manner described by equations (6) and (7). In each case, our state-propagation methods do not specifically simulate the far-tails of the error distribution; instead, a far-tail error bound is generated from the covariance matrix (which describes the distribution core) and, in the case of the biased-Gaussian method, from an additional propagated bias term. To evaluate the validity of these bounds, we consider a necessary (but not sufficient) test of conservatism. Specifically, we consider an ellipsoidal error bound, which is essentially a protection level defined on an ellipsoidal surface. The surface is defined over all states x that satisfy the following ellipsoid equation, given that xˆ is the state estimate and P is the state-covariance. The size of the ellipsoid is scaled by a parameter  2 , where we call  the “ellipsoidal radius.” Formally, the ellipsoidal radius is known in statistics as the Mahalanobis distance.

 x  xˆ T P 1  x  xˆ    2

(20)

The probability of an error occurring outside the bound is computed using Gaussian statistics (assuming a Gaussian with a mean at xˆ and a covariance of P). In this way, we extend our knowledge of the statistics at the core of the distribution (whether computed by the EKF, Particle Filter, or biased-Gaussian filter) to the far tails. Different levels of integrity risk, Rint , correspond to different values of ellipsoidal raidus  . The relationship between  and Rint comes from the complement of the spherical volume integral of an isotropic Gaussian probability distribution function: Rint       1

  2  det  P  1

1/2

V

  2 exp   2 

  dV 

(21)

  2     2  1  erf   exp      2  2 

For the biased-Gaussian method, the ellipsoidal bound is slightly modified. The unknown bias is incorporated into

the error bound by shifting the dimensions of the ellipse accordingly.

error ellipse, especially as the elliptical bound is extrapolated to the far tails of the error distribution.

 x  xˆ    P  x  xˆ      2

The bounds for the biased-Gaussian method are shown in Figure 3 over the same snapshot of Monte Carlo sample points. In this case, the extent of the bias is based on the approximation of equation (13), evaluated at n=4.

T

(22)

To empirically evaluate the integrity of the ellipsoidal bound for each propagation method, the actual integrity risk Ract is analyzed as a function of the ellipsoidal radius. The actual integrity risk is computed as the fraction of Monte Carlo sample points beyond the bound. Any case in which the actual risk is greater than the allowed risk represents an integrity violation. As long as the actual risk is less than the allocated risk, the far-tail error bound is considered valid. In comparing the actual risk to the allocated risk, it is important to consider the effect of the population size of the Monte Carlo simulations. We do not consider the comparison of actual to allocated risk statistically significant for cases when fewer than ten points lie outside the ellipsoidal error bound. For our simulations (with 105 Monte Carlo samples), the comparison is only valid for integrity risk levels of Rint greater than 10-4. Noise Case 1 Noise case 1 involved only initial condition uncertainty and no propagation noise. The following plot shows the 1-sigma covariance ellipses for the EKF and PF over a projection of the Monte Carlo samples onto the x-y plane at time step k=5.

Figure 3 Bias Bounds shown over a snapshot of Monte Carlo sample points for Case 1 at time step 5. The vertical bars represent the 4-sigma bias bounds. The x-component of the bias bound is represented by the horizontal bar and the corresponding y-component is represented by the vertical bar. The nonlinear distortions to the initial Gaussian distribution are accounted for by these biases. The 1-sigma covariance ellipse of the integrity bound is at the intersection of these bars and a closer view is shown in the following figure. The covariance ellipse is circular because the state transition matrix was simplified to the identity matrix (see Appendix A).

Figure 2 Covariance ellipses from EKF and PF shown over a snapshot of Monte Carlo sample points for Case 1 at time step 5. The crescent-like shape of the non-Gaussian Monte Carlo distribution illustrates why it is not described well by an

integrity violations. The vertical line shows the simulation limit, placed at   4 corresponding to the allowed integrity risk. As the time steps progress, similar behavior is seen when comparing all three methods. The biased-Gaussian bounds are conservative while the EKF and PF bounds may be initially conservative, but eventually violate the allowed integrity risk. Noise Case 2 In this case, there is initial distribution uncertainty which is the same as in Case 1. There is also along-track velocity noise given by covariance matrix Figure 4 Closer view of covariance ellipse for the biasedGaussian bounds for Noise Case 1. To check for integrity violations of each propagation method’s error bounds, an empirical CDF (cumulative distribution function) plot is shown for the EKF, Particle Filter, and biased-Gaussian method In Figure 5. The empirical CDF plots the actual integrity risk. In each case, these empirical CDFs were computed by counting the fraction of Monte Carlo points inside the ellipsoidal bound as a function of ellipsoidal radius  . These were compared to the analytical, allowed integrity risk, described by equation (21) and plotted as dashed line. When the empirical curve is above the dashed line, the error bound is considered valid, but when the curve is below, the integrity bound is not valid.

1 0  2 Bk    10 . 0 0 

(23)

As for the analysis for Case 1, the bias bounds are shown in the following figure.

Figure 6 Bias Bounds shown over a snapshot of Monte Carlo sample points for Case 2 at time step 5. The vertical bars represent the 4-sigma bias bounds. The tangential velocity noise term (see Appendix B) results in larger bias bounds covariance ellipse. The following figure shows a closer view of the ellipse.

Figure 5 Empirical CDF for Case 1 at time step 5. As can be seen in Figure 5, the EKF is not conservative over most of its range. The Particle Filter is conservative for part of its range but still doesn’t capture the extreme far tails accurately, which is what we are most concerned about. However, the biased-Gaussian bound captures all of the Monte Carlo samples and doesn’t incur any

The empirical CDF plots show that the error bounds of the EKF and Particle Filter are not necessarily conservative, notably in the far tails of the error distribution. On the other hand, the biased-Gaussian approach yields conservative bounds out to the expected threshold.

Figure 7 Closer view of 1-sigma covariance ellipse of biased-Gaussian bounds for Noise Case 2 at time step 5. The empirical CDF plot is shown below.

However, this ensured bound on integrity risk comes at the expense of highly conservative estimates. Ongoing work will focus on tightening these bounds. One way to accomplish this may be to simply incorporate sensor information (by analyzing the estimator correction step in addition to the propagation step). A second way to reduce overconservatism may be to apply the proposed bias bound to a Gaussian Mixture Model formulation, which represents the non-Gaussian distribution with many Gaussian “kernels.” In concept, if enough Gaussian kernels were employed, dynamics would be locally linear, and the local level of bias required for integrity would converge toward zero. Ongoing work will also consider a hardware experiment that replicates the Monte Carlo simulations. The experiment will provide an integrity verification of the proposed state-propagation methods for the case of a real nonlinear system (with real, rather than simulated, propagation noise and initialization error). For these experiments, the iRobot Create robotic platform will be used. This robot has the same kinematics discussed above, in equation (17). Truth data, which will play the same role as the Monte Carlo sample points from the simulation, will be generated in the form of accurate position traces captured by an overhead camera.

Figure 8 Empirical CDF for Case 2 at time step 5. As seen in figure above, the EKF and PF violate integrity risk by crossing over the analytical line while the biasedGaussian method does not. In fact, all of the Monte Carlo points fall within the bias bounds so that the elliptical radius scaling has no effect. On the other hand, the EKF crosses over the analytical line at an elliptical radius of about 3.5, and the PF crosses over at about 2.3. For this particular time step, the EKF and PF perform fairly well in terms of far-tail integrity since the velocity noise tends to distort the Monte Carlo samples into a more Gaussian-like shape. However, as time steps progress, the nonlinear distortions grow and the EKF and PF begin to violate integrity at smaller values of elliptical radius.

Figure 9 iRobot Create robotic platform being used in the ongoing experiment. CONCLUSIONS

Future Directions

In this paper we presented a method for propagating integrity bounds in nonlinear state estimation using a

[6]

Gelb, A., Applied Optimal Estimation. The MIT Press, 1974.

[7]

Van der Mewe, R. W., Eric A., Sigma-Point Kalman Filters for Integrated Navigation, Proceedings of the Institute of Navigation’s ION-GNSS, 2004, pp.641654.

[8]

Julier, S., Uhlmann, J., Unscented Filtering and Nonlinear Estimation, Proceedings of the IEEE, 2004, Vol. 92, No. 3.

To verify the integrity of the relaxed bounding approach, Monte Carlo simulations were performed. The simulations indicated that the proposed state-estimation method provided conservative bounding out to elliptical radii corresponding to an order of magnitude less than the probability of a single Monte Carlo sample point. By comparison, the simulations indicated that EKF and PFbased bounds are not always conservative in their far tails.

[9]

Thrun, S., Burgard, W., Fox, D. Probabilistic Robotics. The MIT Press, 2005.

The greatest disadvantage of the proposed method is that the biased-Gaussian overbound grows steadily over time. Future work will consider methods for mitigating this growth. In particular, we believe that a measurement correction step will be necessary to restrict growth of the state error and that a GMM will minimize excessive conservatism.

[11] van Graas, T., Krishnan, V., Suddapalli, R., Skidmore, T., Conspiring Biases in the Local Area Augmentation System, Proc. Of ION Annual Meeting, 2004, pp. 300-307.

biased Gaussian overbounding approach. By carefully accounting for higher-order EKF linearization errors, it is possible to bound the effects of nonlinear distortions. The bound is generated by introducing an “unsigned bias” term, which is propagated by the state estimator. In some cases, a rigorous, absolute bound can be determined. In cases when the rigorous bound does not exist, or is severely conservative, a “relaxed” bound can be generated. The relaxed bound only considers the propagation of the most likely subset of system noise and errors.

REFERENCES [1]

Federal Aviation Administration, GNSS Evolutionary Architecture Study, Phase I – Panel Report, 2008.

[2]

Farrell, James L., Full Integrity Testing for GPS/INS, NAVIGATION, The Journal of the Institute of Navigation, Vol. 53, No. 1, pp. 33-40.

[3]

Greer, D., Bruggemann, T., Walker, R., Sigma Point Kalman Filters for GPS Navigation with Integrity in Aviation, IGNSS Symposium, 2007.

[4]

Borja, C., Tur, J. and Gordillo, J., Accurate Position Estimation and Propagation in Autonomous Vehicles, IEEE Robotics and Automation Magazine, 2009, Vol. 16, Issue 2.

[5]

Yun, Youngsun, Y., Yun, H., Kim, D., and Kee, C., A Gaussian Sum Filter Approach for DGNSS Integrity Monitoring, NAVIGATION, The Journal of the Institute of Navigation, 2008, Vol. 61, No. 4, pp. 687-703.

[10] J. Rife, S. Pullen, B. Pervan, and P. Enge., Paired Overbounding for Nonideal LAAS and WAAS Error Distributions, IEEE Transactions on Aerospace and Electronic Systems, 42(4), October 2006, pp. 13861395.

APPENDIX A The derivation for the bias bound (mk) of the robotic system with Noise Case 1 conditions is given here. We begin by linearizing the system model, as shown in equation (8), and assume the propagation noise wk is zero. xk  f x k 1, u k ,0  f xˆ k 1, uk ,0  Fk x k 1  xˆ k 1   bk

(24)

The state-transition matrix, Fk, need not be the Jacobian resulting from a Taylor Series expansion of the nonlinear equations of motion. Alternative linearizations may only be zero-th order accurate; however, the formulation of equation (8) works for any linearized representation of the system, Fk, with the residual error being absorbed into the bias term bk. For our purposes, we chose Fk to be the identity matrix to simplify the computation of the bias bk as much as possible. Fk  I

(25)

Choosing an identity state-transition matrix leaves the resulting bias vector.

b k  f  x k 1 , u k , 0 

(b)

(a)

 f  xˆ k 1 , u k , 0   Fk  x k 1  xˆ k 1   x k 1  g  k , u k , 0 

  g  , u , 0   g ˆ , u , 0 





 xˆ k 1  g ˆk , u k , 0   x k 1  xˆ k 1  k

k

k

(26)

k

Applying the definition of the function g, given by (17), the bias term can be expanded.

 bk  k k

 

  sin   sin    t   sin ˆ  sin ˆ   t k 1 k 1 k k 1 k 1 k   ˆ ˆ      t          cos cos cos cos  k 1 k  k 1 k 1 k 1 k t   0  

      

(27)

Our goal is to maximize this expression for the bias over the range of allowable angles, k-1. All other parameters – including the control commands (k, k) and the previous heading estimate, ˆk 1 – are known values. Although an analytical approach to the maximization is possible, we offer a geometric approach, which is more intuitive. The geometric approach frames the bias as the sum of two vectors of the same length. To obtain these two vectors, we can factor out the vector rk ,  sin k t  k  rk  1  cos k t  , k    0

(28)

by introducing a pair of rotation matrices, R, defined as follows. cos  R     sin   0

 sin  cos  0

0 0  1 

(29)

Then the bias can be written compactly as the following.

 

b k  R  k 1 rk  R ˆk 1 rk

Figure 10 Illustration of Vector Pair for Case 1. The bias error bk is the sum of these two vectors, one dependent on the actual heading (left) and the other dependent on the estimated heading (right).

(30)

This expression is the difference of two vectors of equal length. These two vectors are illustrated in Figure 10.

In order to obtain an unsigned bias bound, mk, we must maximize this bias over all possible values of the actual heading. For the most general case, we must consider all real values of heading,  k 1   . Since mk is defined to bound each term of the bias vector, individually, the maximum bias depends on the coordinate system used to define the state vector. For a coordinate system described by two orthogonal unit vectors, the absolute largest bias occurs when the unknown actual heading  k 1 (see Figure 10a) aligns with the coordinate axes. For this case:

   

 R ˆ r  xˆ  r  k 1 k k     m k   R ˆk 1 rk  yˆ  rk  .     0  

(31)

The x-coordinate bound is simply the length of the known heading estimate vector (Figure 10b) added to the length of the heading vector (Figure 10a), assuming that the heading vector is aligned with the x-axis. The ycoordinate bound is derived similarly. For practical applications, it may be sufficient to consider only a subset of values for the actual heading  k 1 . A restricted range of possible heading error values reduces the size of the bias bound. To consider this case, it is useful to define the heading estimation error,  k :  k 1  ˆk 1   k 1 .

(32)

The angular error  k 1 is illustrated in Figure 11. Introducing  k 1 into the bias equation, (30), results in the following.





   R ˆ  r  R ˆ  r   I  R ˆ  r

b k  R ˆk 1   k 1 rk  R ˆk 1 rk  R   k 1

k 1

  R   k 1

k 1

k

k 1

k

   0 (33)

and

mk , x

  

k

   

 R     I R ˆ r  xˆ   k 1 k     (37)  max   R     I  R ˆk 1 rk  xˆ     r  R ˆ r  xˆ  k k 1 k  

 

Similarly, there are three conditions for the y-component of the bias bound, shown in the table below. Table 2. y-components of the bias bound, mk, for Case 1 Condition Bias Bound

   2

Figure 11 Illustration of Constrained Bias.

     2

As  k 1 varies, the bias traces a circular arc length, shown by the dashed line in Figure 11. The extent of the arc length depends on the greatest value of allowed  k 1 , defined by  . The orientation of the arc length is defined by  , which is the nearest angle between the

   2

 

nominal vector R ˆk 1 rk and the x-axis, as defined

and      2

mk , y

   

 R     I R ˆ r  yˆ   k 1 k    max     R     I  R ˆk 1 rk  yˆ   

mk , y

mk , y

(38)  rk  R ˆk 1 rk  yˆ (39)

 

   

 R     I R ˆ r  yˆ   k 1 k      max   R     I  R ˆk 1 rk  yˆ     r  R ˆ r  yˆ  k 1 k  k 

(40)

 

APPENDIX B

below. 

  r  yˆ   R ˆ  r  xˆ     R ˆ

  tan 1 

k 1

k

k 1

k

The derivation for the bias bound (mk) of the robotic system with Noise Case 2 conditions is given here. (34)

There are three conditions for the maximum x-component of the bias bound, depending on the extent and orientation of the arc length. If     0 , we can examine the endpoints of the arc length to determine the maximum xcomponent. However, if      , the endpoints do not contain the maximum, and the maximum x-component is given by equation (31). Lastly, if     0 and      , then we must examine the endpoints of the arc length and the projection of the circular arc length onto the x-axis. These conditions are summarized in the following table. Table 1. x-components of the bias bound, mk, for Case 1 Condition x-Component of Bias Bound

   

   0

 R     I R ˆ r  xˆ   k 1 k  (35)  mk , x  max     R     I  R ˆk 1 rk  xˆ   

  

mk , x  rk  R ˆk 1 rk  xˆ (36)

 

Case 2 conditions assume that the noise vector wk has a nonzero tangential velocity component given by the covariance matrix below. 2   0

0  0 

(41)

We linearize the system model, shown below, as in equation (8). xk  f xk 1, u k , w k   f xˆ k 1, uk ,0  Fk xk 1  xˆ k 1   G k w k  bk

(42)

Note that we use a modified form of the state transition matrix, Fk. as done for Case 1, and a modified form of the matrix Gk. These matrices would normally be the Jacobians from the Taylor Series expansion of the system. However, since the formulation of equation (8) works for any linearized representation of the system, Fk is chosen to be the identity matrix and Gk is chosen to be a rotation matrix that maps the noise vector to the estimated heading

defined by angle ˆk 1 . This is done to simplify the computation of the bias bk as much as possible. Fk  I

   sin ˆk 1   cos ˆk 1 

b k  g  k , u k , 0   g ˆk , u k , 0



  rk  R ˆk 1 rk  Gw k 

 

 

 

and   

(44)

     R ˆk 1 rk  Gw k  k       k  R  k 1   R ˆk 1 rk  k 

 



where

mk , x

 

 1  G    R ˆk 1 rk  k     wk     0 

 

Note that the last line of the bk formulation above is identical to equation (30) in Appendix A but multiplied by a scalar value. The maximization of this bias goes to infinity because of the noise term k . As done in Appendix A with the heading  k 1 , it may be sufficient to consider only a subset of values for the actual noise, k   k   . A restricted range of possible noise error values reduces the size of the bias bound. To consider this case, it is useful

   max    R     I  R ˆk 1 rk  xˆ  k      max     R     I  R ˆk 1 rk  xˆ  k  

 

 

    rk  R ˆk 1 rk  xˆ   

 

(47)

       max  k   R     I  R ˆk 1 rk  xˆ  k         k   max    max     R     I  R ˆk 1 rk  xˆ  k        k   max   r  R ˆ r  xˆ    k k 1 k       k   

 

mk , x

(46)

(48)

 

 

Table 4. y-components of the bias bound, mk, for Case 2 Condition Bias Bound

   2

    

k   k  

   k    max   k   

    max mk , x   k k 

   0

 



Table 3. x-components of the bias bound, mk, for Case 2 Condition x-Component of Bias Bound

  

   k     rk    R ˆk 1 rk   k 

     R ˆk 1 rk  Gw k   k   k     R  k 1   R ˆk 1 rk  k 

Then the bias bound of this expression is identical to the bias bound of Appendix A but scaled by the additional noise terms. The x and y-components of the bias bound are given by the following tables.

   0

 Fk  x k 1  xˆ k 1   Gw k    R  k 1   k  k    R  k 1   k  k

(45)

(43)

Rearranging (42) to solve for the bias bk and applying the definition of the function g, given by (17) gives the following.



 



  k   max    R  k 1   R ˆk 1 rk k  

G k  R ˆk 1

cos ˆk 1   sin ˆk 1

to define the maximum noise error as  max . Inserting this into the bias (44) results in the following.

mk , y

   max    R     I  R ˆk 1 rk  yˆ  k      max     R     I  R ˆk 1 rk  yˆ  k  

    max mk , y   k k 

 

mk , y

(49)

 

    rk  R ˆk 1 rk  yˆ  (50)   

 

       max  k   R     I  R ˆk 1 rk  yˆ  k         k   max    max     R     I  R ˆk 1 rk  yˆ  k            max    k    rk  R ˆk 1 rk  yˆ     k   

 

   2 and     

   k    max   k   

 

 

(51)