Motion Constraints and Vanishing Point Aided Land Vehicle Navigation

Abstract

In the typical Inertial Navigation System (INS)/ Global Navigation Satellite System (GNSS) setup for ground vehicle navigation, measures should be taken to maintain the performance when there are GNSS signal outages. Usually, aiding sensors are utilized to reduce the INS drift. A full motion constraint model is developed allowing the online calibration of INS frame with respect to (w.r.t) the motion frame. To obtain better heading and lateral positioning performance, we propose to use of vanishing point (VP) observations of parallel lane markings from a single forward-looking camera to aid the INS. In the VP module, the relative attitude of the camera w.r.t the road frame is derived from the VP coordinates. The state-space model is developed with augmented vertical attitude error state. Finally, the VP module is added to a modified motion constrains module in the Extended Kalman filter (EKF) framework. Simulations and real-world experiments have shown the validity of VP-based method and improved heading and cross-track position accuracy compared with the solution without VP. The proposed method can work jointly with conventional visual odometry to aid INS for better accuracy and robustness.

1. Introduction

Accurate vehicular navigation is of great importance for some core parts in “smart cities”. It is not only used in the Guidance, Navigation and Control systems for autonomous driving, but also in V2X (vehicle-to-everything) technologies for effective transportation and cooperative safety communications among vehicles. For example, the positioning and heading information is shared among the V2V (vehicle-to-vehicle) network, according to the Cooperative Awareness Message (CAM) and Basic Safety Message (BSM), from the European Telecommunications Standards Institute (ETSI) and US Society of Automotive Engineers (SAE), respectively [1,2]. The inertial navigation system (INS) has the sole capability to produce a complete and continuous set of navigation state data, with high precision during a short time span. However, the positioning error grows considerably with time, especially when using low-cost MEMS inertial measurement units (IMU). Therefore, INS should be integrated with other aiding sensors. INS and Global Navigation Satellite System (GNSS) integration is commonly used for outdoor vehicles navigation. Nevertheless, GNSS may not be available in tunnels, and can suffer from obstruction and multipath errors in city centers and mountainous regions. There is also a possibility of a GNSS receiver being jammed or spoofed [3,4].

Various aiding sensors can be used to mitigate the error growth of INS in GNSS denied environment, such as motion constraints (e.g., Non-holonomic Constraints (NHC) as a “virtual” sensor) [5], speed sensors (wheel odometers, Doppler radars) [6], LiDAR sensors [7], and digital maps for map-matching or map-aiding [8,9]. Recently, many researchers proposed to use vision sensors to aid the navigation system, in loosely coupled form, with visual odometry (VO) module [10,11] or in tightly coupled form [12,13,14], and achieved remarkable results. Apart from commonly used point features in the pose estimation, vanishing points extracted from parallel lines in the scene can be used to obtain attitude information, which in then fused with INS in a loosely coupled manner. A simple and fundamental work of determining the rotation between two camera views was introduced by [15]. Rotation matrix between the two camera coordinate systems can be solved linearly when one obtains coordinates of three non-collinear vanishing points represented in both images. An alternative method using only two dominant directions is presented by [16] based on the Rodrigues’ formula. The vanishing point-based attitude estimation method has been investigated to help indoor pedestrian navigation or improve VO performance in hallway environments [17,18,19] and UAV navigation in urban areas with structured buildings [20,21].

For land vehicle applications, Kim et al. [22] showed improved accuracy when adding the omnidirectional vision attitude module to the INS/odometer integration for car navigation with a “Manhattan world” assumption [23] during GPS outages. These methods rely on the observation of parallel lines in structured buildings. However, there may be no sufficient building observations, for example, when driving through dense treed areas. Parallel lane markings or boundaries can be generally observed and detected in the structured road. Previous research considered ground plane tracking [24], camera calibration [25], and vehicles’ in-lane lateral distance calculation or prior road map-based shape registration for better localization performance [26,27,28]. Basically, these methods use observed lane markings for relative lateral positioning. Few works have been done on the navigation aiding using the vanishing point of parallel lane markings. In the earlier work, we used VP measurements of parallel lane markings to aid the INS onboard a car [29]. The relative heading aiding is treated in an absolute way, i.e., using true heading aiding method, which may cause large positioning error after certain time during GNSS outages. As the heading error grows larger, the measurement will be less relevant to the true heading error.

For the estimator used for integrated navigation, EKF (Extended Kalman filter) is most widely used due to its computation efficiency and the fact that nonlinearities of the error system model and measurement model are moderate in common situations. There are other nonlinear filters that do not linearize the system model at all, such as the Unscented Kalman filters (UKF) and particle filters (PF) [30,31,32]. For the application of GPS/INS navigation system, a performance comparison between EKF and UKF was made in [31]. It is reported that EKF and UKF offer identical performance except for unrealistic situation, e.g., a sixty kilometres initial position error. For the in-motion alignment of a low-cost INS with large initial attitude errors, the UKF outperforms EKF [32]. Recently, a dual Kalman filter method [33,34] is developed to estimate input and state simultaneously, which is suitable for structures fatigue damage identification. However, in the inertial navigation-based multi-sensor integration, the inputs are from IMU sensor readings, which are already known. The sensor biases are usually augmented into the state vector.

In this paper, the motion constraints module with a full module and the VP module are developed to aid the INS in the framework of EKF. We develop the VP aiding method based on the idea of Stochastic Cloning Kalman filter [35,36] considering the relative nature of the attitude measurement, which is also the estimation tool in the application of Micro Aerial Vehicle indoor navigation [37]. Using the sequential updating EKF, the proposed method can be easily incorporated into the conventional VO-based loosely-coupled vision-aided inertial navigation system (VINS) to improve the accuracy and robustness.

The paper is organized as follows. The coordinate systems involved are defined and the relationship is established between VP 2D coordinates and relative attitude of the camera frame w.r.t. the road frame. Based on this relationship, the VP aiding module is developed with an augmented state. VP module is then added into existing motion constraint aided INS. We evaluate the proposed algorithm by the Monte Carlo simulations and real data experiments. Discussions and conclusions are presented in the end.

2. Relative Attitude from Vanishing Point Coordinates

2.1. Coordinate Systems Definition

The coordinate systems defined here follow the right hand rule as shown in Figure 1 [29].

• Navigation frame (n-frame): For the near-Earth navigation, it is defined as the local-level frame where x, y, z axes are in the directions of east, north and up.
• Vehicle motion frame (m-frame) [38]: x-axis is parallel with the non-steered axle, pointing to the right, y-axis points to the forward, and z-axis points up. Both y and z-axes are in the vertical plane of symmetry. The origin, which is the measurement origin of the vehicle, is the position at road height, mid-way between the wheels of non-steered axle.
• IMU body frame (b-frame): x-axis points right, the y-axis points forwards and the z-axis points up. The origin is the measurement origin of the IMU.
• Camera frame (c-frame): The camera is looking forward, so the z-axis points forward, x-axis points right, and y-axis points down.
• Camera intermediate frame ($c^{\prime }$-frame): rotate 90${}^{\circ }$ about x-axis of camera frame to get camera intermediate frame. It is introduced to derive the relationship between the vanishing point coordinates and relative attitude of the camera.
• Road markings frame (r-frame): fixed to a road and rotated with the road terrain and road direction. Suppose the vehicle is moving on the road with parallel lane markings. In r-frame, x and y axes are in the road plane, perpendicular to and along the lane markings, respectively. z-axis is perpendicular to and pointing out from the road surface.

2.2. Camera Relative Attitude Derived from Vanishing Point Coordinates

A vanishing point (VP) is the point of intersection of image projections of a set of parallel 3D lines, e.g., lane-lines. Each set of parallel lines is associated to a VP in an image. As shown in Figure 2, the VP of the parallel lines is the intersection of the image plane with a ray parallel to the world lines through the camera center. The VP image coordinates are not affected by the relative translation, but only affected by the relative rotation between the camera and the scene [39]. This property enables the VP-based module a tool to determine the camera-to-scene relative attitude, which can be utilized as an aiding source for the INS. It is known that at least two sets of parallel lines are needed to determine all three degree of freedom of a camera’s relative attitude [16].

Here we define some Euler angles and rotation matrices that are important for our proposed methods. The rotation matrix from the r-frame to the $c^{\prime }$-frame ($C_{c^{\prime }}^r$) can be represented by a set of Euler angles ($\theta ,\gamma ,\psi$), which denote the relative pitch, relative roll and relative yaw, respectively. The rotation order is as follows. First rotate $\psi$ about z-axis of r-frame, then $\theta$ about x-axis, and finally $\gamma$ about y-axis. When the first two rotations are completed, as illustrated in Figure 2, we derive the image coordinates of vanishing point

$$\begin{array}{c}{x}_{vp}^1=x_c+\frac{f}{cos\theta }tan\psi \\ y_{vp}^1=y_c+ftan\theta \end{array}$$

(1)

where f is the focal length of the camera, $x_c$ and $y_c$ are the coordinates of principle point. After the 3rd rotation, the final vanishing point coordinates ($x_{vp},y_{vp}$) can be expressed as

$$\left[\begin{array}{c}{x}_{vp}-x_c\\ y_{vp}-y_c\end{array}\right]=\left[\begin{array}{cc}cos\gamma & sin\gamma \\ -sin\gamma & cos\gamma \end{array}\right]\left[\begin{array}{c}{x}_{vp}^1-x_c\\ y_{vp}^1-y_c\end{array}\right]$$

(2)

Therefore, we have

$$\begin{array}{c}\hfill x_{vp}=x_c+\frac{f}{cos\theta }tan\psi cos\gamma +ftan\theta sin\gamma \end{array}$$

(3)

$$\begin{array}{c}\hfill y_{vp}=y_c-\frac{f}{cos\theta }tan\psi sin\gamma +ftan\theta cos\gamma \end{array}$$

(4)

From Equations (3) and (4), one can find that there are two knowns ($x_{vp}$ and $y_{vp}$), and three unknowns ($\theta ,\gamma ,\psi$); therefore, additional constraints are needed to solve the equations.

Here we derive the relative yaw angle from the vanishing point coordinates. Combine Equations (3) and (4) and cancel out the relative yaw angle $\psi$, then we have

$$(x_{vp}-x_c)sin\gamma +(y_{vp}-y_c)cos\gamma =ftan\theta$$

(5)

From Equation (3), we can obtain relative yaw of c-frame w.r.t. r-frame.

$$\psi =arctan\frac{\left(\frac{x_{vp}-x_c}{f}-tan\theta sin\gamma \right)cos\theta }{cos\gamma }$$

(6)

Here we assume the relative roll is zero,

$$\widehat{\gamma }=0$$

(7)

Then the relative pitch and yaw can be expressed as

$$\begin{array}{c}\hfill \widehat{\theta }=arctan\frac{y_{vp}-y_c}{f}\end{array}$$

(8)

$$\begin{array}{c}\hfill \widehat{\psi }=arctan\left(\frac{x_{vp}-x_c}{f}cos\widehat{\theta }\right)\end{array}$$

(9)

Note that the relative attitude here is the camera intermediate frame $c^{\prime }$ w.r.t. the road frame.

2.3. Straight Lane Determination and Vanishing Point Detection

Lane marking detection has been widely researched in the literature of autonomous driving and is gradually being incorporated into vehicles navigation modules [40]. In this paper, the measurement is derived from the image observations of parallel straight lane markings, so straight lane detection should be performed before the VP extraction. The VP extraction is based on the commonly used Hough transform which can extract dominant lines in the binary edge image.

The following will provide more details on the straight lane detection module (shown in Figure 3). First, the region of interest (ROI) is selected from the original image, and the edges are detected using the Canny Edge detector. The Hough transform is applied to the bottom ROI, which is near straight, to get multiple straight line parameters (polar coordinates $\rho -\theta$ space), which are grouped with into several lane marking groups using DBSCAN (Density-Based Spatial Clustering of Applications with Noise) clustering. Only one lane marking in the bottom ROI is selected, and we obtain the interested lane marking points by searching the binary image from bottom to top with small regions along the main direction trend. Then the straight lane marking detection consists of two parts: (1) curve-fitting using interested lane points to suggest initial decisions of straight or curved lane, and (2) a delayed-decision mechanism based on the accumulated initial decisions among frames.

For the first part, the interested lane marking points’ coordinates are stored for a quadratic polynomial curve fitting. The initial straight detection criterion is the comparison between estimated coefficient and the threshold of the second-degree term. Erroneous points may be selected as interested lane points, for example, the straight lane may be wrongly identified as a curved lane. For this reason, we add second part, a simple “delayed” decision mechanism to obtain final results. It works as follows: the initial decisions of both “straight” or “curve” lanes are accumulated among image frames. At least $N_c$ curve detections suggest a curved lane, and at least $N_s$ straight lane detection after a long-curved segment will determine a straight lane. The values of the thresholds ($N_c$ and $N_s$) in the decision mechanism are based on the velocity of the vehicle, heading change, and so on.

2.4. Measurement Uncertainty

Because the relative attitude measurement will be fused with INS solutions, it is essential to determine the measurement uncertainty. The error sources include IMU-camera calibration error, zero-roll assumption error, vanishing point detection error, and so on. We assume the accurate inter-sensor calibration, so calibration error is not considered here.

Now we investigate the measurement uncertainty caused by the zero-roll assumption error. As presented in the following subsection, relative yaw is the main aiding source, so we conduct a sensitivity analysis on the relative yaw w.r.t. the zero-roll assumption error.

From Equations (3), (4) and (6), we derive the partial derivation of $\psi$ w.r.t. $\gamma$

$$\frac{\partial \psi }{\partial \gamma }={cos}^2\psi (tan\psi tan\gamma -sin\theta )$$

(10)

Assuming the zero-roll assumption error is $▵\gamma$, the induced relative yaw error is

$$▵\psi =\frac{\partial \psi }{\partial \gamma }▵\gamma ={cos}^2\psi (tan\psi tan\gamma -sin\theta )▵\gamma$$

(11)

We set an example for better illustration of how large the induced error can be. The ranges of the relative yaw and the relative pitch are set $[-{20}^{\circ },{20}^{\circ }]$ and $[-3^{\circ },3^{\circ }]$, respectively. The true relative roll is set $-3^{\circ }$ (which means the relative roll error is $-3^{\circ }$). As shown in Figure 4, the maximum absolute value of $▵\psi$ is less than ${0.2}^{\circ }$, occurring at the maximum relative pitch and yaw values.

For the error of VP detection, it usually comes from the Hough transform in the line detection. The empirical VP uncertainty is set as pixel level. The measurement errors can also come from the faulty identifying curved lane marks as straight, which should be avoided as much as possible. Due to the nature of inertial navigation, an incorrectly identified straight lane marking can lead to larger errors in the navigation solution than a straight line not found. Therefore, our strategy is to give the relatively strict thresholding as illustrated in the “Straight Lane Determination” procedure in Figure 3. The influence of false detections will be presented in the discussion section.

3. Sensor Fusion Process

This section describes the system model, measurement model and EKF used for the sensor integration using vehicle motion constraints, odometer information and vanishing point observations. In this paper, we make following assumptions of the system:

(1)

The vehicle is moving on a certain road segment, where the road boundaries or lane markings are parallel;

(2)

The camera has been calibrated, i.e., the Interior Orientation Parameters (IOPs) are known. The relative rotations between IMU frame and camera frame are also known.

3.1. Filter State

The navigation state is presented by the position (latitude L, longitude $\lambda$ and height h), the velocity (east velocity, north velocity, and up velocity), and the attitude (pitch, roll, and heading). Differential equations of position ${\mathit{P}}^n$, velocity ${\mathit{V}}^n$, and attitude matrix ${\mathit{C}}_b^n$ are

$${\dot{\mathit{P}}}^n=\left[\begin{array}{c}\dot{L}\\ \dot{\lambda }\\ \dot{h}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{1}{(R_N+h)cosL}& 0& 0\\ 0& 0& 1\end{array}\right]{\mathit{V}}^n$$

(12)

$${\dot{\mathit{V}}}^n={\mathit{C}}_b^n{\mathit{f}}^b-\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)\times {\mathit{V}}^n+{\mathit{g}}^n$$

(13)

$${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n[{\mathit{\omega }}_{ib}^b\times ]-[({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times ]{\mathit{C}}_b^n$$

(14)

where ${\mathit{C}}_b^n$ is the attitude matrix, $R_M$ and $R_N$ are meridian radius of curvature and prime vertical radius respectively, ${\mathit{f}}^b$ is the specific force vector measured by the accelerometers, and ${\mathit{\omega }}_{ib}^b$ is the angular rate vector of the body frame w.r.t. the inertial frame, measured by the gyroscopes. ${\mathit{\omega }}_{ie}^n$ is the earth rotation angular rate vector w.r.t. the inertial frame, and ${\mathit{\omega }}_{en}^n$ is the angular rate of the navigation frame w.r.t. ECEF frame. ${\mathit{g}}^n={[00-g]}^T$ is the gravity acceleration vector in navigation frame, and g is the magnitude of local gravity acceleration. The notation $[\mathit{a}\times ]$ is the asymmetric matrix of a vector $\mathit{a}$. INS mechanization is the process to calculate the navigation states, propagated through Equations (12)–(14) by using numerical integration methods, given the initial navigation states and IMU measurements [41,42].

The navigation states can be considered as the full states, and the associated errors are called error states. For the EKF-based integrated navigation systems, error states are estimated and used to correct the full states once the measurement update is accomplished [43]. For convenience, the filter state refers to the error state, which is denoted as $\mathit{X}$. For the normal INS-based EKF, the state is a 15-dimensional vector, not including the scale factors of the IMU sensor:

$${\mathit{X}}_I={\left[\begin{array}{ccccc}\delta {\mathit{V}}^{nT}& {\mathit{\varphi }}^{nT}& \delta {\mathit{p}}^T& {\mathit{b}}_g^T& {\mathit{b}}_a^T\end{array}\right]}^T$$

(15)

where the navigation state error vectors $\delta {\mathit{V}}^n$, ${\mathit{\varphi }}^n$, and $\delta \mathit{p}$ are the velocity errors, attitude errors, and position errors, respectively. INS error model describes the differential equations of navigation state errors. ${\mathit{b}}_g$ and ${\mathit{b}}_a$ are the bias vectors of gyroscopes and accelerometers, each element of which is modeled as a 1st order Gauss-Markov process.

Here we augment two groups of states, one for the motion constraints aiding and the other for the VP aiding.

$$\mathit{X}={\left[\begin{array}{ccc}{\mathit{X}}_I^T& \underset{\mathrm{motion}\mathrm{constraint}}{\underbrace{\begin{array}{cccc}\delta {\beta }_x& \delta {\beta }_z& K_{od}& \delta {\mathit{r}}^{bT}\end{array}}}& \underset{\mathrm{VP}-\mathrm{aiding}}{\underbrace{{\varphi }_U^0}}\end{array}\right]}^T$$

(16)

where $\delta {\beta }_x$ and $\delta {\beta }_z$ are the misalignment errors between b-frame and m-frame about x and z axes. $\delta {\mathit{r}}^b$ is the lever-arm error vector of the IMU measurement origin w.r.t. the origin of m-frame. $K_{od}$ is the scale factor error of the odometer. ${\varphi }_U^0$ is the cloned state of the third component of ${\mathit{\varphi }}^n$ at the beginning of the detection of a straight lane segment. Thus, the dimension of the system state is 22.

The reason we augment the error state of $\delta {\beta }_x$, $\delta {\beta }_z$, $K_{od}$ and $\delta {\mathit{r}}^b$ is to achieve accuracy improvement of the system. Specifically, these values may not be calibrated beforehand or may not be accurate. Also, values can change in various driving scenarios due to the suspending system and different tyre pressures. In this paper, $\delta {\beta }_x$, $\delta {\beta }_z$, $K_{od}$ and $\delta {\mathit{r}}^b$ are modeled as random constants. ${\varphi }_U^0$ is also modeled as a random constant because it will not propagate with time.

3.2. Motion Constraint Aiding

Here Non-holonomic Constraint (NHC) is applied as the velocity constraint along the body x and z axes. It is based on the assumption that the land vehicle does not jump off the ground or slide sideways under normal conditions. The forward velocity can be derived from the wheel odometer if available. In this condition, the odometer measurement and pseudo-measurement of lateral and vertical velocity in the vehicle motion frame are formed as

$${\tilde{\mathit{V}}}_{\mathrm{veh}}^m=\left[\begin{array}{c}0\\ {\tilde{V}}_{od}\\ 0\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1+K_{od}& 0\\ 0& 0& 1\end{array}\right]{\mathit{V}}_{\mathrm{veh}}^m+{\mathit{v}}_V$$

(17)

where ${\mathit{V}}_{\mathrm{veh}}^m$ is the vehicle velocity in m-frame, ${\tilde{\mathit{V}}}_{\mathrm{veh}}^m$ is the measurement of NHC and the velocity indicator, and ${\mathit{v}}_V$ is the measurement noise vector.

Based on the equation of relative linear motion in [44], a basic relationship exists between IMU body frame velocity and vehicle frame velocity:

$${\mathit{V}}_{\mathrm{veh}}^m={\mathit{C}}_b^m({\mathit{V}}^b+[{\mathit{\omega }}_{eb}^b\times ]{\mathit{r}}^b)$$

(18)

where ${\mathit{V}}^b$ is the IMU velocity in the b-frame, $[{\mathit{\omega }}_{eb}^b\times ]$ is the cross product of the angular velocity of the b-frame w.r.t. Earth-centered Earth-fixed (ECEF) frame, ${\mathit{r}}^b$ and ${\mathit{C}}_b^m$ are the lever-arm vector and the rotation matrix between the m-frame and the b-frame.

Based on Equation (18), we can estimate ${\widehat{\mathit{V}}}_{\mathrm{veh}}^m$ from INS as follows,

$${\widehat{\mathit{V}}}_{\mathrm{veh}}^m={\widehat{\mathit{C}}}_b^m({\widehat{\mathit{C}}}_n^b{\widehat{\mathit{V}}}^n+[{\widehat{\mathit{\omega }}}_{eb}^b\times ]{\widehat{\mathit{r}}}^b)$$

(19)

where ${\widehat{\mathit{V}}}^n$ and ${\widehat{\mathit{C}}}_n^b$ are the INS calculated velocity and attitude matrix. ${\widehat{\mathit{C}}}_b^m$ and ${\widehat{\mathit{r}}}^b$ are estimated from the calibration, otherwise through an initial guess (for example, an identity matrix and a zero vector, respectively, for nearly aligned b and m frames).

The residual measurement is formed by subtracting the INS derived vehicle and velocity from odometer and NHC pseudo-measurement.

$${\mathit{Z}}_{NHC/OD}={\widehat{\mathit{V}}}_{\mathrm{veh}}^m-{\tilde{\mathit{V}}}_{\mathrm{veh}}^m$$

(20)

Neglecting the second-order error items, the measurement equation can be written as [29]

$${\mathit{Z}}_{NHC/OD}={\widehat{\mathit{C}}}_n^m\delta {\mathit{V}}^n-{\widehat{\mathit{C}}}_n^m[{\widehat{\mathit{V}}}^n\times ]{\mathit{\varphi }}^n+{\widehat{\mathit{C}}}_b^m({\widehat{\mathit{\omega }}}_{eb}^b\times )\delta {\mathit{r}}^b+\left[\begin{array}{ccc}0& -V_y& 0\\ 0& 0& -V_y\\ V_y& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\beta }_x\\ \delta {\beta }_z\\ K_{od}\end{array}\right]+{\mathit{v}}_V$$

(21)

where $V_y$ is the forward speed measured by the speed sensor or calculated from INS.

NHC aiding acts as a virtual sensor for land vehicle navigation, and can be applied in most driving conditions. If there is no odometer in the system, we select the first and third rows of Equation (21) to formulate the measurement.

3.3. Vanishing Point Aiding

In Section 2, we calculated the relative attitude from vanishing point coordinates. Now we will describe how this information can be used to aid the INS. As usual, we construct the measurement to associate with the state.

When the VP is detected for the first time (at time $t_0$) on the straight lane segment, we can calculate the attitude of this road segment (${\widehat{\mathit{C}}}_r^n$) w.r.t. the navigation frame:

$${\widehat{\mathit{C}}}_r^n={\widehat{\mathit{C}}}_{b,0}^n{\tilde{\mathit{C}}}_r^{b,0}={\widehat{\mathit{C}}}_{b,0}^n{\mathit{C}}_c^b{\left({\tilde{\mathit{C}}}_{c,0}^r\right)}^T$$

(22)

where ${\widehat{\mathit{C}}}_{b,0}^n$ is the INS attitude matrix at $t_0$, ${\mathit{C}}_c^b$ is the IMU-camera rotation matrix, ${\tilde{\mathit{C}}}_{c,0}^r$ is the camera attitude matrix w.r.t the road frame at time $t_0$, which is calculated by

$${\tilde{\mathit{C}}}_{c,0}^r={\tilde{\mathit{C}}}_{c^{\prime },0}^r{\mathit{C}}_c^{c^{\prime }}=f_{DCM}\left({[\widehat{\theta },\widehat{\gamma },\widehat{\psi }]}_{t_0}\right)·f_{DCM}\left([-{90}^{\circ },0,0]\right)$$

(23)

where $f_{DCM}(·)$ is the function to calculate the direct cosine matrix from the Euler angles. The value of ${\tilde{\mathit{C}}}_{c,0}^r$ is then stored for later use. As the vehicle moves on this road segment (at time t), we obtain the relative attitude from the VP measurement.

$${\tilde{\mathit{C}}}_{c,t}^r=f_{DCM}\left({[\widehat{\theta },\widehat{\gamma },\widehat{\psi }]}_t\right)·f_{DCM}\left([-{90}^{\circ },0,0]\right)$$

(24)

Then we can derive the attitude matrix ${\tilde{\mathit{C}}}_{b_{VP}}^n$ from VP module at time t

$${\tilde{\mathit{C}}}_{b_{VP}}^n={\widehat{\mathit{C}}}_r^n{\tilde{\mathit{C}}}_{c,t}^r{\mathit{C}}_b^c$$

(25)

The measurement matrix is constructed as:

$${\mathit{Z}}_M={\widehat{\mathit{C}}}_b^n{\left({\tilde{\mathit{C}}}_{b_{VP}}^n\right)}^T$$

(26)

where ${\widehat{\mathit{C}}}_b^n$ is the current attitude matrix from INS.

Then we derive the relationship between the measurement matrix and the state with small error angle assumption.

$${\mathit{Z}}_M\approx (\mathit{I}-{\mathit{\varphi }}^n\times ){\mathit{C}}_b^n{\mathit{C}}_r^b(\mathit{I}+{\mathit{\vartheta }}_{VP,t}^r\times )(\mathit{I}-{\mathit{\vartheta }}_{VP,0}^r\times ){\mathit{C}}_{b,0}^r{\mathit{C}}_n^{b,0}(\mathit{I}+{\mathit{\varphi }}_0^n\times )$$

(27)

Neglecting the second-order error items,

$${\mathit{Z}}_M=\mathit{I}-\left[({\mathit{\varphi }}^n-{\mathit{\varphi }}_0^n+{\mathit{\vartheta }}_{VP,0}^r-{\mathit{\vartheta }}_{VP,t}^r)\times \right]$$

(28)

where $\mathit{I}$ is the 3-dimensional identity matrix, ${\mathit{\varphi }}_0^n$ is the attitude error at time $t_0$, ${\mathit{\vartheta }}_{VP,t}^r$ and ${\mathit{\vartheta }}_{VP,0}^r$ are the relative attitude error from VP at current time instant t and time $t_0$, respectively. The vector form of the measurement is

$${\mathit{Z}}_V=-\frac{1}{2}\left[\begin{array}{c}{\mathit{Z}}_M(3,2)-{\mathit{Z}}_M(2,3)\\ {\mathit{Z}}_M(1,3)-{\mathit{Z}}_M(3,1)\\ {\mathit{Z}}_M(2,1)-{\mathit{Z}}_M(1,2)\end{array}\right]={\mathit{\varphi }}^n-{\mathit{\varphi }}_0^n+{\mathit{\vartheta }}_{VP,0}^r-{\mathit{\vartheta }}_{VP,t}^r$$

(29)

Here we do not want to incorporate the erroneous pitch/roll information from VP, which may deteriorate the pitch/roll estimation. Hence, the final measurement is the third component of ${\mathit{Z}}_V$.

$$Z_{VP}=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\mathit{Z}}_V={\varphi }_U-{\varphi }_U^0+v_{VP}$$

(30)

where $v_{VP}$ is the measurement noise.

3.4. State Augmentation Treatment

We treat the state ${\varphi }_U^0$ as an augmented state, which is the cloned state of ${\varphi }_U$ at the very beginning when a straight lane segment is detected. It is based on the idea of Stochastic Cloning Kalman filter [35,36]. Stochastic Cloning deals with the relative state measurement which depends on the current state as well as the previous state of the system. The core of Stochastic Cloning is considering the interdependencies (cross-correlation terms) between the previous states and current states. At the start time ($t_0$) of the relative measurement, the relevant state is cloned and augmented in the state vector, and the state covariance is also augmented with considering the cross-correlation between the original state and cloned state. This essential step is often called “re-initialization”, because there is a life-time for the cloned state. During the life-time of the cloned state, EKF will evolve the state covariance matrix properly as long as the cloned state is modeled as a random constant.

Denote the first 21 states as normal states ${\mathit{X}}_n$, and last cloned state as ${\mathit{X}}_c$, such that the state in Equation (16) is written as

$$\mathit{X}={\left[{\mathit{X}}_n^T{\mathit{X}}_c^T\right]}^T$$

(31)

Once the start of a new straight line segment is detected, we re-initialize the cloned state augmentation. The re-initialization is established as follows:

• Clone the value of ${\mathit{X}}_n\left(6\right)$ to ${\mathit{X}}_c$;
• Set the state covariance as

  $${\mathit{P}}_k=\left[\begin{array}{c}{\mathit{I}}_{21}\\ {\mathit{J}}_{1\times 21}\end{array}\right]{\mathit{P}}^{nn}{\left[\begin{array}{c}{\mathit{I}}_{21}\\ {\mathit{J}}_{1\times 21}\end{array}\right]}^T=\left[\begin{array}{cc}{\mathit{P}}^{nn}& {\mathit{P}}^{nc}\\ {\mathit{P}}^{ncT}& {\mathit{P}}^{cc}\end{array}\right]$$

  (32)

where ${\mathit{P}}^{nn}$ is the covariance matrix of normal state ${\mathit{X}}_n$, ${\mathit{P}}^{cc}$ is the covariance matrix of the cloned state ${\mathit{X}}_c$, and ${\mathit{P}}^{nc}$ is the cross-correlation between normal state and cloned state. ${\mathit{J}}_{1\times 21}$ is the Jacobian of ${\mathit{X}}_c$ to ${\mathit{X}}_n$, which is presented as

$${\mathit{J}}_{1\times 21}=\left[\begin{array}{cc}{O}_{1\times 3}& \left[\begin{array}{ccc}0& 0& 1\end{array}\right]O_{1\times 15}\end{array}\right]$$

(33)

From the calculation, we can find that ${\mathit{P}}^{cc}$ is the element of ${\mathit{P}}^{nn}$ at index of (6, 6), which is the variance of the attitude error about the up direction.

The flowchart of integrating vanishing point module and motion constraint module with INS is illustrated in Figure 5. In the VP aiding module, when a set of new parallel straight lane markings is first detected, the road attitude of this segment is calculated and stored, which is used for future vehicle heading computation. Also, the state and covariance re-initiation is performed. Then subsequent VP observations can be used to construct the measurement residual information for measurement update.

4. Results

The described algorithm in the preceding subsections has been evaluated using simulations and real experiments. The results are given in this subsection. The simulation and experiments have shown the validity of using VP of lane markings to mitigate the INS heading error in order to achieve better positioning accuracy.

4.1. Simulation Results

We conducted Monte-Carlo simulations to evaluate the performance of VP aided land vehicle navigation algorithm. Specifically, we compared two schemes: INS/NHC/Odometer integration with and without VP aiding. A trajectory lasting 356 s was designed and generated. Several straight lines, turns, accelerations and climbing maneuvers were conducted. As shown in Figure 6, the main trend is along the north direction. The IMU, camera, odometer, GNSS parameters, lever-arm and boresight error of IMU are listed in

Table 1. Sensor Parameters.

Sensors	Items	Values
Gyroscopes (200 Hz)	Bias	36${}^{\circ }$/h (1 $\sigma )$
	ARW	${0.6}^{\circ }/\sqrt{h}\left(1\sigma \right)$
Accelerometers (200 Hz)	Bias	1 mg $\left(1\sigma \right)$
	VRW	0.05 m/s/$\sqrt{h}\left(1\sigma \right)$
Camera (10 Hz)	IOPs	Same with the experiment
	VP accuracy	2 pixels (1 $\sigma$)
Speed indicator (odometer)	Scale factor error	0.001 (1 $\sigma$)
	Noise standard deviation	0.005 m/s (1 $\sigma$)
Relative Pose	IMU/Vehicle misalignment	x-axis: ${0.8}^{\circ }$, z-axis: $1^{\circ }$ (1 $\sigma$)
	IMU/Vehicle lever-arm	0.1 m in three directions (1 $\sigma$)
	camera/vehicle boresight error	$1^{\circ }$ for pitch (1 $\sigma$)
GNSS (1 Hz, valid in first 80 s)	Position accuracy	2 m (1 $\sigma$)
	Velocity accuracy	0.5 m/s (1 $\sigma$)

. The distance traveled is 1571 m during the GNSS outage, which starts from 80 s till the end of the simulation. The valid VP flags over time are shown in Figure 7, indicating when the VP is used as a measurement.

The estimation of IMU-vehicle frame boresight misalignment and scale factor of the odometer are shown in Figure 8. It can be seen that the boresight alignment angles ${\beta }_x$ and ${\beta }_z$ and the scale factor of the odometer $K_{od}$ can be quickly estimated in the first tens of seconds. The reason is that the vehicle undergoes some accelerations and decelerations during this period, and these maneuvers make the misalignment and scale factor observable [38]. The subsequent turns can also benefit their observability, contributing to good estimation accuracy.

The east and north position error, heading error, and vertical gyro bias estimation error of two schemes with 50 Monte Carlo trials and 3-sigma slope are illustrated in Figure 9 and Figure 10. We can see from the 3-sigma RMS error slope that the overall performance of INS/NHC/Odometer with VP aiding method is better than the one without VP aiding. For the final cross-track error (east position error in this trajectory), the VP-based method achieves 0.32% DT (1 $\sigma$) with 33% improvement compared with no VP method ( 0.48% DT, 1 $\sigma$). The along track errors (north direction) are almost the same, and relatively small, for both schemes, because the forward velocity information from the odometer is utilized, which can benefit the forward positioning accuracy. As illustrated in Figure 10a,b, VP-based method has better heading accuracy, and the heading error growth is slower when the VP measurement is valid during GNSS outage. This is the primary reason for the improved performance of lateral positioning. For the vertical gyroscope bias estimation, we can see that when VP-aiding is used, nearly half of the bias has been estimated in the end, which outperforms the method without VP aiding (Figure 10c,d).

4.2. Experimental Results

The vanishing point and NHC aided fusion algorithm described in the preceding section were tested using “2011_09_26_drive_0028” and “2011_09_26_drive_0101” dataset (shortened as Path-0028 and Path-0101, respectively) from the KITTI benchmark dataset [45]. The raw IMU data (100 Hz) and rectified grayscale images (10Hz, global shutter) are used to verify the algorithm. The biases of gyroscopes and accelerometers are 36${}^{\circ }$/h and 1 mg, respectively. The reference is navigation results from an IMU/GPS data with the L1/L2 RTK positioning accuracy of 0.01 m, pitch/roll accuracy of 0.03${}^{\circ }$, and heading accuracy of 0.1${}^{\circ }$ [46]. A rough value of the lever-arm between IMU and the vehicle frame can be calculated based on the IMU’s mounted position. The boresight angles between IMU and the vehicle frame is unknown, so the initial values are assumed to be zeros.

To illustrate the performance of the proposed algorithms, we simulate the GPS outages on the trajectory. There is no wheel odometer or Doppler radar data in the dataset. For Path-0028, we compared the performance of the following navigation schemes.

(1)

Free INS: Only INS mechanization is performed to calculate the navigation states.

(2)

INS/NHC: NHC is triggered every 1 s with the measurement noise being 0.1 m/s (1 $\sigma$).

(3)

INS/NHC/VP: Vanishing point aiding module is added into the system. It is triggered by the straight lane markings detection and valid vanishing point measurement.

For Path-0101, we added an odometer output module derived from reference data to perform the INS/NHC/OD integration. The performances are compared among free INS, INS/NHC/OD, and INS/NHC/OD/VP.

In the VP detection, we select three Hough peaks in the image and get three intersections of three lines to have better accuracy and robustness. The vanishing point coordinates are determined by averaging the three intersections. A thresholding testing is conducted to detect the wrong lane marking selection. A ${\chi }^2$ statistic for the measurement residuals is generated for blunder detection to further improve the robustness of VP-based measurement update. The valid VP flags over time are recorded and shown in Figure 11 for Path-0028 and Path-0101, indicating when the VP can be used as a measurement.

4.2.1. Results of experiment #1: Path-0028

The path-0028 is approximately 776m through dense trees and lasts 45 s (430 images), containing several direction changes of the path, as shown on Figure 12a. Here the whole trajectory of the GPS data was blocked except for the initial value of navigation states.

The position estimation results of the three navigation schemes are shown in Figure 12, and a summary of the navigation errors is given in

Table 2. Navigation Errors During GPS Outage (path-0028).

	Free INS			INS/NHC			INS/NHC/VP
	Mean	Max	RMS	Mean	Max	RMS	Mean	Max	RMS
East position (m)	0.918	4.337	1.731	−0.893	2.097	1.086	0.701	1.582	0.840
North position (m)	−4.337	13.23	5.670	−1.327	3.313	1.616	−0.839	2.139	1.026
Height (m)	0.063	0.558	0.282	0.091	0.566	0.267	0.072	0.547	0.271
Pitch (${}^{\circ }$)	0.096	0.216	0.108	0.012	0.103	0.038	0.009	0.103	0.038
Roll (${}^{\circ }$)	−0.032	0.114	0.046	−0.024	0.099	0.045	−0.020	0.095	0.043
Heading (${}^{\circ }$)	−0.241	0.409	0.254	−0.248	0.423	0.262	−0.189	0.423	0.206
East velocity (m/s)	0.089	0.221	0.124	−0.044	0.132	0.069	−0.031	0.122	0.059
North velocity (m/s)	0.315	0.824	0.372	−0.080	0.171	0.091	−0.055	0.132	0.072
Up velocity (m/s)	0.004	0.038	0.022	0.004	0.040	0.021	0.004	0.039	0.021

. We can see that NHC/INS integration improves the positioning accuracy about 71% (horizontal error dropped to 4 m from 14 m). When the VP module is added, a further 33% improvement over INS/NHC scheme is achieved and the positioning error reduces to 0.32% DT. Pitch estimation gets improved when NHC is applied, while roll estimation does not. The reason is that there is a relationship between vertical velocity and pitch angle inherently in NHC, which makes the pitch angle observable. There is no improvement for pitch/roll estimation when VP is added on INS/NHC, since only relative yaw information is utilized. As illustrated in Figure 13, the heading error gets smaller when the VP-aiding works, which is the main reason for increased positioning accuracy. The vertical gyroscope bias (not shown here) is estimated to a value of 3${}^{\circ }$/h when using VP. In this sense, we can see the complementary benefits to the INS when using motion constraint and VP observation.

The estimation of relative pose (lever-arm and boresight misalignment) error between IMU frame and the vehicle frame is presented in Figure 14. The value of misalignment about the pitch axis reaches to about 0.4${}^{\circ }$. If the misalignment has not been augmented into the states, the induced velocity error in vertical direction will be 0.126 m/s under the speed of 18 m/s. A method to account for this is to increase the measurement noise variance of NHC. However, the biased measurement error will still degrade the system performance. State augmenting has the potential strength to achieve improved results under the condition that the augmented states are observable.

4.2.2. Results of Experiment #2: Path-0101

The path-0101 lasts 91 s, and the total traveled distance is about 1236 m with a standstill at the beginning. The simulated GNSS outage starts from 32 s until the end of trajectory. The positioning results and navigation errors are shown in Figure 15 and in

Table 3. Navigation Errors During GPS Outage (path-0101).

	Free INS			INS/NHC			INS/NHC/VP
	Mean	Max	RMS	Mean	Max	RMS	Mean	Max	RMS
East position (m)	2.768	7.076	3.561	−1.970	4.336	2.355	−1.877	4.1224	2.241
North position (m)	−4.337	13.23	5.670	−1.327	3.313	1.616	−0.839	2.139	1.026
Height (m)	0.063	0.558	0.282	0.091	0.566	0.267	0.072	0.547	0.271
Pitch (${}^{\circ }$)	0.096	0.216	0.108	0.012	0.103	0.038	0.009	0.103	0.038
Roll (${}^{\circ }$)	−0.032	0.114	0.046	−0.024	0.099	0.045	−0.020	0.095	0.043
Heading (${}^{\circ }$)	−0.241	0.409	0.254	−0.248	0.423	0.262	−0.189	0.423	0.206
East velocity (m/s)	0.089	0.221	0.124	−0.044	0.132	0.069	−0.031	0.122	0.059
North velocity (m/s)	0.315	0.824	0.372	−0.080	0.171	0.091	−0.055	0.132	0.072
Up velocity (m/s)	0.004	0.038	0.022	0.004	0.040	0.021	0.004	0.039	0.021

, respectively. In the scheme of INS/NHC/OD, the horizontal position performance in the last epoch achieved 61% improvement compared with free INS. With the help of VP module, the cross-track error (most in north direction) is further improved over 50% to 0.153 m of RMS, and the heading RMS error is reduced to 0.167${}^{\circ }$ from 0.21${}^{\circ }$.

The lane markings in the whole trajectory are generally straight with several small curved segments. In the experiments, we find that if these small curved lane markings are also considered as straight and VP measurements are used, there will large variations in the heading and lateral position estimation, while the mean value of their errors are small. That is to say, small change of the lane marking direction will cause the heading estimation drift to the opposite direction, if the lane direction variation has not been detected. Thus, careful and strict treatment should be conducted on the straight lane detection module. We will discuss it in the following section.

5. Discussion

5.1. Robustness of VP-Aiding Approach

There may still be the circumstances that the curvature of the lane mark is small, but wrongly identified as the straight line. Assume the vehicle moves in a curved road, while the camera detects the lane markings as straight. When the initial road frame $r_0$ is obtained, it will be registered and act as the reference frame when the camera is still observing the “same” segment. However, the true road frame will change along the curve road as shown in Figure 16, from $r_1$-frame to $r_3$-frame, for instance. Now we will describe its influence on the Kalman filter. In this situation, the attitude matrix from VP module described in Equation (25) will be changed to the following form.

$${\tilde{\mathit{C}}}_{b_{VP}}^n={\widehat{\mathit{C}}}_{r_0}^n{\tilde{\mathit{C}}}_{c,t}^{r_i}{\mathit{C}}_b^c$$

(34)

where the subscript $r_0$ is the initial road frame and the superscript $r_i$ ($i=1,2,\dots$) is the instant road frame along the road. After the derivation, the measurement in Equation (30) can be written as

$$Z_{VP}=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\mathit{Z}}_V={\varphi }_U-{\varphi }_U^0-{\alpha }_i+v_{VP}$$

(35)

where ${\alpha }_i$ is the angle from $r_0$-frame to $r_i$-frame around the vertical axis. The term $-{\alpha }_i+v_{VP}$ will be considered as the measurement noise. The sensitivity analysis turns out to be the analysis of measurement error ${\alpha }_i$ to the filter performance. The most desirable situation is that the road lane markings are randomly curved, i.e., the direction changes slightly from one side to the other randomly, such that ${\alpha }_i$ is considered to be zero-mean white noise. The worst case is that the road is bending to one direction, causing ${\alpha }_i$ continuously growing, which will violate the Kalman filter assumption. The heading error state will be erroneously estimated and the gyro bias in z-axis will also get influenced. This kind of “soft” failure can be detected, for example by using the AIME (Autonomous Integrity Monitored Extrapolation) method. The concept is to form the averaged normalized innovation from last N measurements [4]. The AIME innovation test statistic for the filter at time k is [47]

$$s_k^2=\delta z_{\mu }^T{C}_{\mu }^{-1}\delta z_{\mu }$$

(36)

where

$$C_{\mu }^{-1}=\sum _{i=k+1-N}^k{C}_i^{-1}$$

(37)

$$\delta z_{\mu }=C_{\mu }\sum _{i=k+1-N}^k{C}_i^{-1}\delta z_i$$

(38)

where $\delta z_i$ and $C_i$ are filter innovation and corresponding covariance at time i. The test statistic has a chi-square distribution with the degree of freedom being the measurement vector size. The chi-square test is conducted for the fault detection. Here the test statistic $s_k^2$ has a chi-square distribution with one degree of freedom, since the VP measurement has one dimension. Two parameters should be set for the fault detection. One is the threshold $T_{\mathrm{AIME}}$ to be compared with the test statistic $s_k^2$. The other is the sample size N.

If $s_k^2$ exceeds $T_{\mathrm{AIME}}$, then the soft-fault is reported. The threshold $T_{\mathrm{AIME}}$ can be calculated or looked up in a table when the significance level ($\alpha$) or confidence level ($1-\alpha$) is given. Here we choose the normal significance level $\alpha =0.05$ (false alarm rate), so that $T_{\mathrm{AIME}}=3.84$ with one degree of freedom. The selection of $T_{\mathrm{AIME}}$ is a trade-off between the detection sensitivity and false-alarm rate.

The selection of sample size N is a trade-off between detection sensitivity and response time. Larger the sample size is, easier the detection will be. However, it is also demanded to have faster response time for less contaminated state estimates. In our setup, N is selected as 50 for 10 Hz image rate, which means a 5-s time window for the fault detection. As shown in

Table 4. Simulation Results of Different VP-aiding Schemes under Different Curve Radii (RMS Error).

Trajectory No.	Curve Detected	No VP		With VP		VP & AIME
		Heading	z-Gyro Bias	Heading	z-Gyro Bias	Heading	z-Gyro Bias
# 1: R = 3000 m	Success	0.27${}^{\circ }$	4.35${}^{\circ }$/h	0.25${}^{\circ }$	3.98${}^{\circ }$/h	–	–
# 2: R = 4584 m	Failed	0.27${}^{\circ }$	4.67${}^{\circ }$/h	14.18${}^{\circ }$	128.99${}^{\circ }$/h	0.34${}^{\circ }$	4.20${}^{\circ }$/h
# 3: R = 6000 m	Failed	0.32${}^{\circ }$	5.45${}^{\circ }$/h	10.87${}^{\circ }$	97.92${}^{\circ }$/h	0.32${}^{\circ }$	4.40${}^{\circ }$/h
# 4: R = 8000 m	Failed	0.28${}^{\circ }$	4.72${}^{\circ }$/h	8.12${}^{\circ }$	73.54${}^{\circ }$/h	0.31${}^{\circ }$	3.99${}^{\circ }$/h

, it can also detect the very slowly drifting case (#4: radius = 8000 m) with 5 s time window. Larger drift can be detected more quickly, if multiple test statistics with different sample sizes are computed.

To verify the robustness of proposed algorithm, we simulate several trajectories, each of which consists of a straight lane and a curved lane with a different radius, the worst case mentioned above. The straight and curved parts both last 120 s, with the constant speed of 10 m/s.

The sensors’ specifications are listed in Table 1. The GNSS is available during first 80 s, while motion constraints are used throughout the trajectory. To have a better idea of how the curviness can affect the proposed method, we examine three VP-aiding schemes, namely,

(1)

No VP: only motion constraints are used to aid the INS;

(2)

VP: Proposed VP-aiding method is used without soft faults detection;

(3)

VP & AIME: Proposed VP-aiding method is used and soft faults detection (AIME) is utilized.

Monte Carlo simulations are conducted 50 runs for each scheme and each trajectory. Corresponding curved lane detection and heading accuracy results (1-sigma RMS errors at the endpoint) are presented in Table 4. The heading and vertical gyro bias estimation error for the trajectory #3 are illustrated in Figure 17 without AIME and in Figure 18 when AIME is used.

For the straight line detection algorithm (the threshold of second-degree term $a_0=1\times {10}^{-4}$ in Figure 3 in our case), we find that it can report the curved lane successfully when the curve radius is less than 3150 m (for example the case radius R = 3000 m). When the radius is larger (cases #2 to #4 in Table 4), the straight line detection algorithm will wrongly detect the lane as straight. As a result, VP-aiding demonstrates very large heading and vertical-gyro bias estimation errors. More curved the lane is, larger errors will be. The heading error growth rate is approximately the turning rate in such curved lane. For example, for R = 4584 m, the turning rate is 10 m/s/4584 m = 450${}^{\circ }$/h, and heading error growth rate is ${14.18}^{\circ }$/120 s = 425.4${}^{\circ }$/h.

When we add the AIME soft fault detection function (AIME detection sample size N is 50 (5 s measurements)), the heading and vertical-gyro bias errors drop significantly compared with VP without AIME. The heading error is slightly larger than the scheme without VP, because VP still works in the 5 s detection period. The fault measurements have the direct influence to heading angle, while the vertical-gyro bias estimation is less influenced by the short fault interval, so we can observe that the vertical-gyro bias error is smaller than the scheme without VP in Table 4. As a result, the AIME is suggested for the soft failure detection in proposed VP-aided INS.

5.2. A Complement to Point-Based VO-Aiding

We emphasis that the proposed method is not necessarily superior to the conventional point-based VO. It acts as a complementary method to aid the INS. One of the main advantages of VP-aiding method is that it does not need to track or match the features among frames. Compared to point features, lane markings are "higher level" features containing road direction information. Therefore, our method has the potential to incorporate the digital map to obtain the absolute heading information for additional aiding, which is somehow related with our previous work [29]. However, the disadvantages are also obvious. VP-aiding can only work in the environment with parallel lines, and using the lane VPs only provides one dimension attitude information. For the conventional VO aiding, there is no shape requirement in the scene, and it can output 3-dimentional relative rotation and relative translation (up to scale for monocular case).

The main challenge for conventional monocular VO is the data degeneracy in the pose estimation. The essential (E) matrix is unstable when the feature points lie close to planes, for example, on the ground plane, or when there is no motion or pure rotation of the camera. On the contrary, homography (H) computation requires the feature points on a plane or pure rotation of the camera. To handle this, it is required to detect the degenerate configurations and automatically switch between E and H, which will lead to additional computation cost [16].

As VP-aiding and VO-aiding utilize different environmental features, they can work together to improve accuracy and robustness of the vision-aided INS.

6. Conclusions

In this work, we have investigated an alternative approach to mitigate the navigation error growth by using the vanishing point observations to aid the INS for land vehicle navigation applications. The VP module, extracting information from parallel lane markings, is added to the modified NHC/odometer aiding module to further constrain the navigation errors during GNSS outages. The main contributions are the development of VP-aided INS measurement model and the motion constraint aiding module considering the body-to-vehicle frame misalignment. The Monte Carlo simulations and real experiments have shown the smaller heading and position drifting by fusing VP measurements and motion constraint to the navigation system, because of the complementary information provided by these two aiding modules for the INS.