Millimeter Scale Track Irregularity Surveying Based on ZUPT-Aided INS with Sub-Decimeter Scale Landmarks

Figure 4 illustrates an overview of the data processing procedure of the Kalman filtering and smoothing algorithm based on ZUPT-aided INS combined with landmarks employed in the paper. The system makes use of the measurements of IMU (angular increments $\Delta \theta$ from gyros and specific force integrations $\Delta v$ from accelerometers) and the initial position measured by total station for initial alignment to calculate the initial attitudes. After that the trolley is pushed forward manually on the track at walking speed. After it moves across a certain distance interval (60 m in this paper), the trolley stops and a zero-velocity observation and a position observation will be updated. Then Kalman filtering and smoothing algorithm is executed to output the optimized position, velocity and attitude measurements of the interval. Since the wheels of the trolley can keep continuous contact with the railway track, the 3-dimensional track geometry can be determined by position and attitude sequences of the INS uniquely. Then track parameters can be calculated and track irregularities can be detected.

The error equations of the attitude, velocity and position for the railway track surveying application can be expressed as Equation (2) [10]: where $i$-frame is the inertial frame. $n$-frame is the local level frame (North-East-Down) used as the navigation frame. b-frame is the body frame of the IMU (Forward-Right-Down). ${\mathsf{\varphi }}^n={\left[\begin{array}{lll}{\varphi }_N& {\varphi }_E& {\varphi }_D\end{array}\right]}^T$ represents the vector of attitude errors about the north, east and downward axes of the navigation frame. $C_b^n$ represents the direction cosine matrix. ${\mathsf{\omega }}_{in}^n$ represents the turn rate of the navigation frame with respect to the inertial frame expressed in the n-frame. It can be obtained by summing the Earth’s rotation rate with respect to the inertial frame and the turn rate of the navigation frame with respect to the Earth as: ${\mathsf{\omega }}_{in}^n={\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n$. $\delta {\mathsf{\omega }}_{ib}^b$ represents the drift errors of gyroscopes. $\delta v^n$ is the vector of velocity errors. $f^n$ represents the specific force in navigation axes. $\delta f^b$ is the drift errors of accelerometers. $\delta {\mathbf{r}}^n$ represents the position error in navigation axes. The errors of inertial sensors in this paper are normally modeled as piecewise constant values. The position coordinates of the measured track are expressed in segmentation with $n_0$-frame, which is so near with the $n$-frame that $C_n^{n_0}\approx I$ and $\delta {\mathbf{r}}^{n_0}=C_n^{n_0}\delta {\mathbf{r}}^n\approx \delta {\mathbf{r}}^n={\left[\begin{array}{lll}\delta r_N& \delta r_E& \delta r_D\end{array}\right]}^T$. For medium wavelength (30 m chord) track irregularity surveying, the track segmentation is set to be 60 m long in this paper. Moreover $\delta {\mathsf{\omega }}_{ib}^b$ and $\delta f^b$ can be expressed as shown by Equations (3) and (4): where $\delta {\mathsf{\omega }}_{ib}^n$ is the drifts of gyros expressed in $n$-frame. ${\epsilon }_N$, ${\epsilon }_E$ and ${\epsilon }_D$ are the equivalent gyro biases of the north, east and downward directions. $w_{gx}$, $w_{gy}$ and $w_{gz}$ are the random noises of gyros: where $\delta f^n$ is the drifts of accelerometers expressed in $n$-frame. ${\nabla }_N$, ${\nabla }_E$ and ${\nabla }_D$ are the equivalent accelerometer biases of the north, east and downward directions. $w_{ax}$, $w_{ay}$ and $w_{az}$ are the random noises of accelerometers.

The railway track surveying application has its own characteristics compared with some other applications based on inertial measurement such as land vehicle navigation, airborne gravity measurement and so on. Since the track of high speed railways is almost level and straight with a very large radius of curvature, trolley maneuvers are rather weak when moving on the track at low speed [3,6]. Some error parameters of the INS are coupled together with others, for example, the orientation error is coupled with the equivalent east gyro bias, and the level errors are coupled with equivalent horizontal accelerometer biases, so the equivalent east gyro bias ${\epsilon }_E$ and the equivalent horizontal accelerometer biases ${\nabla }_N$ and ${\nabla }_E$ are unobservable. They will not be estimated as error states in the Kalman filter. Since the trolley moves in walking speed (less than 8 km/h [8]) and the length of the measurement interval is short, the terms of ${\mathsf{\omega }}_{en}^n$ and $\delta {\mathsf{\omega }}_{in}^n$ can be ignored as shown in Equation (2).

Consider the analysis above, a typical Kalman filter with 12 dimensional error states is established in this paper. The system error model and the observation model can be expressed as Equation (5): where the error state vector $x\left(t\right)$ can be written as in Equation (6):

According to Equation (2), the system error matrix $A$ and the system noise matrix $G$ can be expressed in simplified form by Equations (7) and (8):

$z\left(t\right)={\left[\begin{array}{ll}\delta {\mathbf{v}}^n& \delta {\mathbf{r}}^n\end{array}\right]}^T$ is the filter observation vector and velocity as well as position of the trolley are used as update information in the Kalman filter. The measurement matrix $H$ is defined by Equation (9):$w\left(t\right)$ and $\upsilon \left(t\right)$ are the system noise and the measurement noise, whose Power Spectral Density (PSD) are $Q\left(t\right)$ and $R\left(t\right)$ respectively. They can be expressed as Equation (10):

The position errors and their covariance between two observation updates will increase with time caused by the residual system errors. It is even more serious for the situations that the observation is few and not much precise. In order to obtain optimal position estimations during the updates outages, a smoothing algorithm must be applied utilizing all the past, current and future measurements [11,12]. This paper employs the well-known RTS smoothing algorithm to estimate the states in the measurement intervals. The RTS smoother consists of a common forward Kalman filter and a backward smoother. The backward sweep begins at the end of the forward Kalman filter. Figure 5 illustrates the computation procedure of the RTS smoother.

The forward Kalman filter is the common one, can be expressed in discrete form as Equation (11) shows: where ${\widehat{x}}_{fk}^{+}$ and $P_{fk}^{+}$ represent the updated estimate of state vector and its corresponding covariance matrix of the forward filter at epoch k. ${\widehat{x}}_{fk+1}^{-}$ is the optimal predicted estimate and $P_{fk+1}^{-}$ represents its covariance matrix. $H_k$ is the measurement matrix. $K_k$ is the gain matrix of forward Kalman filter at epoch k. ${\mathsf{\Phi }}_k$ is the system state transition matrix which can be calculated by matrix $A$.

The backward smoother can be expressed in the discrete form as shown by Equation (12) [13,14]: where ${\widehat{x}}_k$ is the optimal smoothed estimate of state vector at time epoch k. $P_k$ is the error state covariance matrix of the smoother. ${\stackrel{⌢}{H}}_k$ is the smoothing gain matrix.

In order to assess the track irregularity, the relative geometry parameters of alignment and level should be calculated after obtaining the absolute position of the track. As Figure 6 illustrates, an arbitrarily 30 m chord on the 60 m track segmentation is determined by two points $p_{0m}$ and $p_{30m}$ whose coordinates are respectively marked in $n_0$-frame as ${\mathbf{r}}_{p_{0m}}^{n_0}={\left[\begin{array}{lll}{r}_{p_{0m}N}& r_{p_{0m}E}& r_{p_{0m}D}\end{array}\right]}^T$ and ${\mathbf{r}}_{p_{30m}}^{n_0}={\left[\begin{array}{lll}{r}_{p_{30m}N}& r_{p_{30m}E}& r_{p_{30m}D}\end{array}\right]}^T$. The measurement values of their coordinates are marked as ${\tilde{r}}_{p_{0m}}^{n_0}={\left[\begin{array}{lll}{\tilde{r}}_{p_{0m}N}& {\tilde{r}}_{p_{0m}E}& {\tilde{r}}_{p_{0m}D}\end{array}\right]}^T$ and ${\tilde{r}}_{p_{30m}}^{n_0}={\left[\begin{array}{lll}{\tilde{r}}_{p_{30m}N}& {\tilde{r}}_{p_{30m}E}& {\tilde{r}}_{p_{30m}D}\end{array}\right]}^T$ respectively. $p_s$ represents an arbitrary point on the trajectory for this chord, coordinates of which are marked as ${\mathbf{r}}_{p_s}^{n_0}={\left[\begin{array}{lll}{r}_{p_{s}N}& r_{p_{s}E}& r_{p_{s}D}\end{array}\right]}^T$ for the true value and ${\tilde{r}}_{p_s}^{n_0}={\left[\begin{array}{lll}{\tilde{r}}_{p_{s}N}& {\tilde{r}}_{p_{s}E}& {\tilde{r}}_{p_{s}D}\end{array}\right]}^T$ for the measurement value. $d_s$ is the vector distance from $p_s$ to the chord, projections of which in horizontal plane and vertical plane are alignment and level, respectively.

As shown in Figure 6, the track segmentation is 60 m long for ZUPT and absolute position update. For arbitrary 30 m chord on the track segmentation, we define a new frame as $c$-frame, whose x-axis is identical with the chord and can be obtained by a rotation ${\theta }_a$ about z-axis and a rotation ${\theta }_l$ about y-axis of $n_0$-frame sequentially. We can calculate the vector distance values for every point of a trajectory by transforming the coordinates from $n_0$-frame to $c$-frame. The relationship between $n_0$-frame and $c$-frame can be written as Equation (13):

According to the definition of alignment and level together with the Figure 6, $r_{p_{s}y}^c$ represents the alignment and $r_{p_{s}z}^c$ represents the level and they can be calculated by Equation (14):

In addition, we can express ${\theta }_a$ and ${\theta }_l$ by coordinate values as shown in Equation (15) according to Figure 6:

The deviations of $r_{p_{s}y}^c$ and $r_{p_{s}z}^c$ can be calculated by variational method as expressed by Equations (16) and (17):

Substituting Equations (16) and (17) into Equation (1) yields the alignment irregularity and the level irregularity. Since gradient of the railway track is very small (25 m/1000 m for the largest gradient) and the turning radius is very large (2000 m) in general, the alignment irregularity and the level irregularity can be simplified by ignoring the small terms as Equations (18) and (19) show: where $l_c=\sqrt{{\left(r_{p_{30m}N}^{n_0}-r_{p_{0m}N}^{n_0}\right)}^2+{\left(r_{p_{30m}E}^{n_0}-r_{p_{0m}E}^{n_0}\right)}^2+{\left(r_{p_{30m}D}^{n_0}-r_{p_{0m}D}^{n_0}\right)}^2}$ represents the length of the chord. $l_{5m}=5m$ is the distance between points $p_s$ and $p_{s+5m}$. The derivation processes of Equations (18) and (19) are shown in Appendix A. As Figure 7 illustrates, even though the absolute position measurements may have deviations bigger than centimeter scale, the relative deviations can also be millimeter scale due to the common offset contained by the adjacent points $p_s$ and $p_{s+5m}$.