1. INTRODUCTION
In recent years, stereo visual-inertial vehicle positioning systems (VIVPS) have become popular in environments where GPS is unreliable or unavailable because the two sensing modalities offer complementary characteristics (Xu and Choi, Reference Xu and Choi2016; Khoshelham and Ramezani, Reference Khoshelham and Ramezani2017; Ramezani and Khoshelham, Reference Ramezani and Khoshelham2018). The low-frequency drift of the strapdown inertial navigation system (SINS) can be significantly corrected via visual observations (Leutenegger et al., Reference Leutenegger, Furgale and Rabaud2014). On the other hand, fast and highly dynamic motions can be precisely tracked by a SINS in a short time, thus the problem of scale ambiguity and large latency in vision can be settled to a certain extent (Zhang and Singh, Reference Zhang and Singh2015; Mueggler et al., Reference Mueggler, Gallego and Rebecq2017). Visual-inertial fusion approaches found in the literature can be divided into two categories: loosely-coupled and tightly-coupled systems. Loosely-coupled systems independently estimate the pose by a vision-only algorithm and fuse SINS measurements only in a separate estimation step (Aufderheide and Krybus, Reference Aufderheide and Krybus2010; Kneip et al., Reference Kneip, Weiss and Siegwart2011). Tightly-coupled methods in contrast take all the correlations between visual and inertial sensors into consideration and process the raw information of both sensors in a single estimator, thus leading to higher accuracy (Leutenegger et al., 2013; Shen et al., Reference Shen, Michael and Kumar2015; Yang et al., Reference Yang, Gao and Shen2017). Therefore, most high-accuracy VIVPS presented recently have implemented a tightly-coupled estimator.
Since the measurement model of tightly-coupled stereo VIVPS is highly nonlinear, suitable nonlinear filtering methods are indispensable to improve the accuracy of tightly-coupled stereo VIVPS (Abeywardena and Dissanayake, Reference Abeywardena and Dissanayake2015; Li et al., Reference Li, Jiang and Song2017). Numerous nonlinear filtering approaches can be adopted for VIVPS and extensively studied, including methods based on the extended Kalman filter (EKF) (Quan and Piao, Reference Quan and Piao2017; Quan et al., Reference Quan, Piao and Tan2017), unscented Kalman filter (UKF) (Ebcin and Veth, Reference Ebcin and Veth2007; Kong et al., Reference Kong, Wu and Zhang2015; Song-hui et al., Reference Song-hui, Ming-ming and Peng2016) and particle filter (PF) (Durrie et al., Reference Durrie, Gerritsen and Frew2009; Yap et al., Reference Yap, Li and Morikis2011). On the basis of Taylor first-order linear approximation, EFK is able to deal with weak nonlinear systems well with a small amount of computation. However, the quality of EKF is often poor for intrinsically nonlinear systems because of the local linearisation to nonlinear systems. UKF can simulate the probability density function of nonlinear systems via the sigma points from UT transformation, which is a method based on the theory of Gaussian distribution to approximate the probability density distribution of nonlinear functions. The mean and covariance of the state estimations propagated by the nonlinear system can be acquired via a finite number of discrete points of Gauss distribution with the same mean and covariance as the original state distribution (Wan and Van Der Merwe, Reference Wan and Van Der Merwe2000; Kandepu et al., Reference Kandepu, Foss and Imsland2008). Although UKF can reach third-order Taylor precision, the filtering accuracy is not enough for strongly nonlinear systems in some high-precision applications, such as highly accurate tightly-coupled stereo VIVPS. PF can offer higher filtering precision for highly nonlinear systems than UKF because of the random sampling applied in PF. As a variation of PF, unscented particle filter (UPF) can improve the filtering precision further by using the latest measurement information to obtain the optimised importance density function (Van Der Merwe et al., Reference Van Der Merwe, Doucet and De Freitas2001; Yong and Chen, Reference Yong and Chen2001; Qiang et al., Reference Qiang, Lei and Cui2013). Unfortunately, UPF cannot handle the contradiction between accuracy and speed well enough due to the large computational complexity, limiting its application in tightly-coupled stereo VIVPS.
In order to obtain high-precision results at medium speed, a hybrid UPF optimised by firefly algorithm is designed in this paper for tightly-coupled stereo VIVPS. On account of the linear state equation model, the time updating of UKF in hybrid UPF is based on the standard Kalman filter to improve the filtering precision and reduce the computational complexity. The measurement updating is conducted via a less computational spherical simplex sampling strategy rather than UT transformation. The fusion results of tightly-coupled stereo VIVPS are obtained via the particle updating optimised by firefly algorithm, which is a nature-inspired metaheuristic algorithm based on the flashing patterns and behaviour of fireflies (Yang, Reference Yang2010; Yang and He, Reference Yang and He2013; Zhang et al., Reference Zhang, Liu and Yang2016). Fireflies are randomly distributed in the solution space at the beginning, so that the brightness of each firefly is different due to their different locations. The optimal value can be obtained by attracting and moving fireflies of lower brightness to fireflies of higher brightness for many times. By means of moving particles towards high likelihood region via the attraction and movement of fireflies, the filtering reduces the number of required particles, leading to the great reduction of computational complexity. The formulas of position and attraction are revised in this paper so that the firefly algorithm can be implanted into the particle updating of hybrid UPF.
The experimental results demonstrate that the designed hybrid UPF for tightly-coupled stereo VIVPS can significantly reduce the computation amount of traditional UPF, meanwhile improving the positioning precision to a certain extent. The hybrid UPF presented in this paper is a practical fusion algorithm for tightly-coupled stereo VIVPS of high accuracy and medium computational complexity.
2. PRINCIPLE OF TIGHTLY-COUPLED STEREO VIVPS
The principle of tightly-coupled stereo VIVPS in this paper is shown in Figure 1. The VIVPS consists of the SINS, stereo camera and fusion filter which is the research focus of this paper. The input of the fusion filter is made up of the motion information obtained via the SINS and feature points matched between two consecutive images by the SURF algorithm. The motion error parameter values obtained from the filter are fed back to the SINS for correction, leading to a high-precision autonomous vehicle positioning system.
Diagram of the tightly-coupled stereo VIVPS via the fusion filter.
2.1. Filter state model
The standard Kalman filter formulation requires that the measurements employed for the state updating be independent of any previous filter states. However, the camera measurement models relate the system state at two consecutive time instants when the image pair is captured. In order to make sure that the Kalman filter can be applied in the tightly-coupled stereo VIVPS, the state vector is augmented to include a history of SINS pose when the last image pair is recorded. Thus, the state vector is given by:
$$X=\left[\begin{matrix} \phi_E, \phi_N, \phi_U, \delta V_E, \delta V_N, \delta V_U, \delta R_E, \delta R_N, \delta R_U, \varepsilon_{bx} ... \\ ...\varepsilon_{by}, \varepsilon_{bz}, \zeta_x, \zeta_y, \zeta_z, \delta p_x, \delta p_y, \delta p_z, \delta \theta_x, \delta \theta_y, \delta \theta_z \\ \end{matrix} \right]^T$$where the subscript E, N and U of each state represent three directions in the geographic coordinate system; ϕE, ϕN and ϕU are the angular error of SINS; δ V E, δ V N and δ V U denote the velocity error of SINS; δ R E, δ R N and δ R U represent the position error of SINS; εbx, εby and εbz are the random drift error of the gyroscope in three directions along the carrier coordinate system; ζx, ζy and ζz denote the random drift error of the accelerometer in three directions along the carrier coordinate system; δ p x, δ p y and δ p z denote the position error of the SINS at the time when the last image pair is recorded; δθx, δθy and δθz denote the angular error of the SINS at the time when the last image pair is recorded.
Since the past pose is unchanged during the filter prediction step, the process model can be assumed as zero:
$$\begin{cases} \delta \mathop {p_x }\limits^\bullet =0 \\ \delta \mathop {p_y }\limits^\bullet =0 \\ \delta \mathop {p_z }\limits^\bullet =0 \\ \delta \mathop {\theta _x }\limits^\bullet =0 \\ \delta \mathop {\theta _y }\limits^\bullet =0 \\ \delta \mathop {\theta _z }\limits^\bullet =0 \\ \end{cases}$$Thus, the linear filter state model can be obtained by Equation (2) and the error-state kinematics describing the time evolution of the SINS state as follows:
$$\mathop X\limits^{\bullet} (t)=F(t)X(t)+G(t)W(t)$$
where the system state transition matrix 
$F(t)=\left[\begin{smallmatrix}
F_N (t) & F_S (t) & 0_{9\times 6} \\ 0_{12\times 9} & 0_{12\times 6} & 0_{12\times 6} \end{smallmatrix} \right]$; F N (t) denotes the classical inertial system error 9 × 9 matrix; the transformation matrix between navigation parameters and inertial element errors 
$F_S (t)=\left[\begin{smallmatrix} 0_{3\times 3} & 0_{3\times 3} \\ 0_{3\times 3} &\hbox{C}_b^n \\ \hbox{C}_b^n & 0_{3\times 3} \end{smallmatrix} \right]$; 
$\mbox{C}_{b}^{n}$ is the attitude matrix; the noise vector W(t) = [w εx, w εy, w εz, w ζx, w ζy, w ζz] T; w εx, w εy and w εz are the noise term of the gyroscope; w ζ x, w ζ y and w ζ z denote the noise term of the accelerometer; the noise transformation matrix 
$G(t)=\left[\begin{smallmatrix} I_3 &0_{3\times 3} \\ 0_{3\times 3} &\hbox{C}_b^n \\ 0_{3\times 3} &0_{3\times 3} \\ I_3 &0_{3\times 3} \\ 0_{3\times 3} &I_3 \\ 0_{6\times 3} &0_{6\times 3} \\ \end{smallmatrix} \right]$.
2.2. Filter measurement model
The measurement model for the tightly-coupled stereo VIVPS in this paper is deduced via the trifocal geometry of stereo vision. The trifocal tensor describes the geometric relations among three views and depends only on the relative motion between the different views. Figure 2 shows the point-line-point correspondence between three views by trifocal tensor.
Relationship between corresponding points in three images.
Assuming that the camera matrices of three views are P A = K A [R A|t A], P B = K B [R B|t B] and P C = K C [R C|t C], the elements of the trifocal tensor can be derived accordingly (Hartley and Zisserman, Reference Hartley and Zisserman2004):
$$T_i^{qr} =\left( {-1} \right)^{i+1}\det \left[\begin{matrix} {\sim} a^i \\ b^q \\ c^r \\ \end{matrix} \right]$$where ~a i is P A without the ith line; b q denotes the qth line of P B; c r represents the rth line of P C.
Once the trifocal tensor is computed, a pair of matched points x A ↔ x B in the first and second views can be mapped into the third view, using the homography between the first and third view induced by a line in the second image. Thus, the relationship between corresponding points in three images can be derived (Hartley and Zisserman, Reference Hartley and Zisserman2004):
$$x_C^k =x_A^i l_{B,j} T_i^{jk}$$where l B is a straight line passing through the point x B.
Figure 3 shows the stereo camera configuration of two consecutive frames. Without loss of generality, the coordinate of the right camera at the previous time step is assumed to be aligned with the world coordinate system. Thus, the camera matrices of the right and left camera at time k − 1 can be represented in canonical form as
Configuration of stereo cameras at adjacent times.
$$\begin{cases} P_{R,k-1} =K_R [ I{|}0] \\ P_{L,k-1} =K_L [ R_C{|}{t_C}] \\ \end{cases}$$where P R, k−1 and P L, k−1 denote the camera matrix of the right and left camera, respectively; K R is the calibration matrix of the right camera; [R C|t C] is the relative pose of the left camera at time k − 1 obtained by calibrating; K L is the calibration matrix of the left camera.
The rotation matrix and translation vector of the cameras at time k can be obtained from the true state of SINS according to the relationship between the world and geographic coordinate system as
$$\begin{cases} [ R_R {|} {t_R } ] =RT(tss_k ) \\ [ R_L {|} {t_L } ] =RT(tss_k ) \\ \end{cases}$$where R R and R L are the rotation matrix of the right and left camera; t R and t L are the translation vector of the right and left camera; RT denotes the relationship between the world and geographic coordinate system; tss k is the true state of SINS at time k:
$$tss_k =nss_k \oplus X_k$$where nss k is the nominal state of SINS acquired by the strapdown processing; X k denotes the state vector of the filter state model at time k; ⊕ means that the nominal state nss k is compensated by the corresponding motion error elements of the state vector X k.
Thus, the camera matrices of the right and left camera at time k can be derived:
$$\begin{cases} P_{R,k} =K_R [ R_R{|}{t_R } ] \\ P_{L,k} =K_L [ R_L {|} {t_L } ] \\ \end{cases}$$Then, the trifocal tensor of the right and left camera at time k can be calculated by Equation (4):
$$\begin{cases} T_R =T(K_R, K_L, R_C, t_C, R_R, t_R ) \\ T_L =T(K_R, K_L, R_C, t_C, R_L, t_L ) \\ \end{cases}$$where T R represents the relationship between the right camera and the stereo image pair at time k; T L represents the relationship between the left camera and the stereo image pair at time k.
The matching pixels of the right and left camera at time k can be obtained according to the point transition Equation (5) as
$$\begin{cases} x_{R,k} =H(T_R, x_{R,k-1}, x_{L,k-1} ) \\ x_{L,k} =H(T_L, x_{R,k-1}, x_{L,k-1} ) \\ \end{cases}$$where x R, k and x L, k are the corresponding matching pixels of the right and left camera at time k.
In consideration that the matching pixels x R, k−1 and x L, k−1 is unchanged during the filter measurement step at time k, the nonlinear measurement model can be represented based on Equations (7)–(11) as
$$Z_k =h(nss_k \oplus X_k )$$where Z k = [x R,k, x L,k] T is the measurement vector at time k; h(•) denotes the nonlinear relationship between the state and measurement vector.
3. HYBRID UPF OPTIMISED BY FIREFLY ALGORITHM
The linear/nonlinear discrete-time system of tightly-coupled stereo VIVPS can be derived by discretising the linear/nonlinear mixed filter model constructed in Section 2:
$$\begin{cases} X_k =\phi _{k/k-1} X_{k-1} +W_k \\ Z_k =h(nss_k \oplus X_k )+V_k \\ \end{cases}$$
where X k ∈ R n and Z k ∈ R m denote the state and measurement vector at time k; ϕk/k−1 is the discrete state transition matrix; h(·) is the nonlinear function describing the measurement model; W k and V k are the uncorrelated zero-mean Gaussian white noise processes with covariances 
$E [ W_{k} W_{k}^{T} ] =Q_{k}$ and 
$E [ V_{k} V_{k}^{T} ] =R_{k}$.
3.1. Simplified hybrid UKF
Without consideration of the linear nature of the system state equation, the standard UKF obtains the predicted mean and covariance by the weighted sum of the sampling points acquired via UT transformation, bringing about a great amount of redundant computation. In order to overcome this shortcoming and reduce the computation cost, the designed hybrid UKF for tightly-coupled stereo VIVPS calculates the predicted mean and covariance via the formula of linear Kalman filter. The symmetric sampling in standard UKF is also replaced by hyperspherical simplex sampling to reduce the number of needed sigma points, resulting in further decline in computation.
The procedure of the simplified hybrid UKF can be described as follows:
Step 1 Initialisation: initialise the state vector 
$\widehat{X}_{k-1}$ and state covariance matrix P k−1.
Step 2 Prediction: the predicted mean and covariance are calculated with the same equations as the KF given the linear state equation of tightly-coupled stereo VIVPS:
$$\begin{align} \widehat{X}_{k/k-1} &=\Phi _{k/k-1} \widehat{X}_{k-1}\label{eq14} \end{align}$$
$$\begin{align} P_{k/k-1} &=\Phi _{k/k-1} P_{k-1} \Phi _{k/k-1}^T +Q_k\label{eq15} \end{align}$$Step 3 Sigma points calculation: the sigma points are acquired via hyperspherical simplex sampling based on the predicted mean and covariance:
$$\chi _{i,k/k-1} =\widehat{X}_{k/k-1} +\sqrt {P_{k/k-1} } e_i^n\quad i=0,...,n+1$$Step 4 Update: the sigma points of the predicted measurement are obtained as follows:
$$\gamma _{i,k/k-1} =h(nss_k \oplus \chi _{i,k/k-1} )\quad i=0,...,n+1$$Then the predicted measurement, measurement covariance, cross-correlation covariance matrix and Kalman gain are computed by
$$\begin{align} \widehat{Z}_{k/k-1} &=\sum_{i=0}^n {w_i \gamma _{i,k/k-1} }\label{eq18} \end{align}$$
$$\begin{align} \widehat{P}_{\widehat{Z}_{k/k-1} } &=\sum_{i=0}^n {w_i (\gamma_{i,k/k-1} -\widehat{Z}_{k/k-1} ) (\gamma _{i,k/k-1} -\widehat{Z}_{k/k-1})^T+R_k }\label{eq19} \end{align}$$
$$\begin{align} \widehat{P}_{\widehat{X}_{k/k-1} \widehat{Z}_{k/k-1} } &=\sum_{i=0}^n {w_i } (\chi _{i,k/k-1} -\widehat{X}_{k/k-1} )(\gamma _{i,k/k-1}-\widehat{Z}_{k/k-1} )^T \label{eq20} \end{align}$$
$$\begin{align} K_k &= \widehat{P}_{\widehat{X}_{k/k-1} \widehat{Z}_{k/k-1} } \widehat{P}_{\widehat{Z}_{k/k-1} }^{-1}\label{eq21} \end{align}$$ Thus, the state estimate 
$\widehat{X}_{k}$ and the corresponding error covariance matrix P k at time k can be updated as follows:
$$\begin{align} \widehat{X}_k &=\widehat{X}_{k/k-1} +K_k (Z_k -\widehat{Z}_{k/k-1} )\label{eq22} \end{align}$$
$$\begin{align} P_k &=P_{k/k-1} -K_k \widehat{P}_{\widehat{Z}_{k/k-1} } K_k^T \label{eq23} \end{align}$$where Z k is measurement at time k.
3.2. PF optimised by modified firefly algorithm
PF can perform high-precision nonlinear filtering via the weighted average of a large number of random sampling points, leading to great computational complexity which restricts the direct application of PF in high-dimensional tightly-coupled stereo VIVPS. The computational complexity of PF can be reduced by improving the distribution of sampling points because the more reasonable the particle distribution is, the less the number of required particles. The firefly algorithm is an attractive swarm intelligence optimisation algorithm proposed by Yang (Reference Yang and Deb2010) due to the fine convergence speed and precision. In order to decrease the number of required particles, the firefly algorithm is introduced into PF in this paper to move particles towards high likelihood regions via the attraction and movement of fireflies.
The standard firefly algorithm needs to calculate the movement value between each firefly and all other fireflies for position updating, resulting in large amounts of computation. Aiming at avoiding interaction between all fireflies, in this paper, the brightness of the global optimal firefly is used to guide particles to move. Thus, the position updating formula of the firefly algorithm is revised as follows:
$$b_j (\kappa +1)=e^{-u(\kappa )/u(\kappa -1)}b_j (\kappa )+\beta _j (\kappa )(b_j (\kappa )-b_{\max } (\kappa ))+\alpha (\mbox{rand}-0.5)$$where b j (κ) and b max(κ) represent the position of the firefly j and global optimal firefly at the κ th iteration; βj(κ) is the attraction between firefly j and the global optimal firefly; the third term is randomisation with α ∈ [0, 1]; the term e −u(κ)/u(κ−1) is designed for adjusting the position updating step and the u(κ) can be given by
$$u(\kappa )=\frac{1}{L}\sum_{l=1}^L {(w(b_l (\kappa ))-w(b_{\max }(\kappa )))^2}$$where w(b l(κ)) and w(b max(κ)) represent the brightness of firefly l and the global optimal firefly at the κ th iteration; L is the number of fireflies. As can be seen from Equations (24) and (25), when the u(κ) decreases faster, the position updating step is larger, with the result that the global searching capability of the firefly algorithm will be more powerful; conversely, if the u(κ) decreases more slowly, the ability of local search will be stronger. Thus, the results of the firefly algorithm can be prevented from falling into extreme points or repeated shocks near extreme points via the term e −u(κ)/u(κ−1).
Given that the attenuation rate of 1/(1 + r 2) is slower than that of 
$1/e^{r^{2}}$, the formula of attraction between firefly j and the global optimal firefly at the κ th iteration is revised to prevent the firefly algorithm from degenerating into a simulated annealing algorithm due to the too fast attenuation rate as follows:
$$\beta _j (\kappa )=\beta _0 \frac{1}{1+\gamma r_{j}^2 (\kappa )}\tau (\kappa )$$where the maximum attraction β0 and absorptivity γ are set to 1 and [0.1 10], respectively; r j (κ) is the Cartesian distance between firefly j and the global optimal firefly at the κ th iteration; the attenuation factor τ(κ) is introduced for adjusting the variation of attraction with iteration number κ:
$$\tau (\kappa )=\psi +\frac{1}{1+\sqrt \kappa }$$where ψ is a set constant. As Equations (26) and (27) show, the attraction implanted with attenuation factor decreases as the number of iterations increases, avoiding the firefly oscillation in the last stage of iteration and improving the convergence speed of the algorithm to a certain extent.
The detailed process of the PF optimised by the modified firefly algorithm in this paper is given below:
Step 1 Initialisation: initialise M particles 
$\left\{d_{j{,}0}\right\}_{j=1}^M $ from a prior distribution p(d 0) with the mean m d0, and variance R d0 and set the importance weights w j, 0 = 1/M.
Step 2 Calculating the weight of particles:
$$\widehat{w}_{j{,}k} =\widehat{w}_{j{,}k-1} \frac{p(Z_k {|} {d_{j{,}k} })p(d_{j{,}k} {|} {d_{j{,}k-1} } )}{q(d_{j{,}k} {|} {d_{j{,}k-1}, Z_k } )}$$where p(•) and q(•) denote the probability and importance density function, respectively; Z k denotes the measurements at time k.
Step 3 Optimising particles via the modified firefly algorithm:
A. Take each particle as a firefly and the weight of the particle is regarded as the brightness of the corresponding firefly; the firefly with the greatest brightness is chosen as the global optimal firefly at the first iteration.
B. Calculate the attraction between each firefly and the global optimal firefly according to Equation (26); then update the position of each firefly by the modified formula (24).
C. Calculate the brightness of each firefly again to find the global optimal firefly at this iteration after the position is updated, and then update the position of all fireflies via Equations (26) and (24).
D. Repeat Step C until the greatest brightness or the number of iterations reaches the set threshold.
Step 4 Revising and normalising the weight of particles: after each particle is iteratively optimised, its position in the state space changes, so that the distribution density function of particles is no longer the original p(d k|Z 1 : k−1). Thus, the weight of particles needs to be revised:
$$w_k^j =\frac{p(d_k^j {|} {Z_{1:k-1} } )}{q(d_k^j )}p(Z_k {|} {d_k^j } )$$
where 
$d_{k}^{j} $ denotes the optimised particle j after iteration at time k.
Then the weight of particles is normalised as follows:
$$w_{j{,}k} =\frac{w_k^j }{\sum_{j=1}^M {w_k^j } }$$Step 5 Calculating the filtering results:
$$\widehat{X}_k =\sum_{j=1}^M {w_{j{,}k} d_k^j }$$3.3. Complete process of hybrid UPF for tightly-coupled stereo VIVPS
On the basis of Sections 3.1 and 3.2, the detailed process of hybrid UPF for tightly-coupled stereo VIVPS in this paper can be described as follows.
Step 1: Initialisation (at time k = 0).
For each particle (i = 1,...N), sample from the prior 
$s_0^i \sim p(x_0)$, calculate 
$\hat {w}_0^i \sim p(z_0 \left| {s_0^i } \right.)$, calculate 
$w_T = \sum_{i=1}^N {\hat {w}_0^i }$, normalise 
$w_0^i =\hat {w}_0^i /w_T$.
Step 2: Prediction and updating (at time k ≥ 1).
A. A. Importance sampling: For each particle (i = 1,...N), the optimised mean
$X_{k-UKF}^{i} $ and variance $P_{k-UKF}^{i} $ is calculated by the Step 2–4 of the simplified hybrid UKF; then draw a sample from the optimised importance density $s_k^i \sim N(X_{k-UKF}^i, P_{k-UKF}^i )$.B. Particle updating: the acquired N particles
$\left\{ {s_k^i } \right\}_{i=1}^N $ in the previous step is updated by Step 2–4 of the PF optimised by the modified firefly algorithm.C. Output: the filtering results at time k are obtained via Equation (31).
Step 3: Repeat Step 2 for the next sample.
4. EXPERIMENTAL RESULTS AND DISCUSSION
4.1. Results of numerical simulation
Experiments by numerical simulation were conducted to test the performance of the designed simplified hybrid UKF and PF optimised by modified firefly algorithm separately. The univariate nonsteady growth model (UNGM) was adopted for the simulation due to the adjustable degree of nonlinearity. The state and measurement model of UNGM is given by
$$\begin{cases} D_c =\alpha _U D_{c-1} +\beta _U \frac{D_{c-1} }{1+D_{c-1}^2 }+\gamma _U \cos (1.2(c-1))+\mu _c \\ Y_c =\kappa _U D_c^2 +\lambda _U D_c +v_c \\ \end{cases}$$
where c is the serial number of discrete value; 
$\mu _{c} \sim N ( 0, \; \sigma _{c}^{2} ) $ and 
$v_{c} \sim N ( 0, \; \sigma _{c}^{2} ) $ are the system and observation noise, respectively.
If αU = 1, βU = 0, γU = 0, κU = 0.2 and λU = 0.1, UNGM is a linear/nonlinear mixed filter model. Assuming that D 0 = 0.5, the filtering results of the designed simplified hybrid UKF and standard UKF are shown in Figure 4. The state variables of the model were specified as dimensionless numbers given that it is unnecessary to set the units of state variables for comparing the performance of different filtering algorithms. The abscissa and ordinate of Figure 4(a) represent the times of Monte Carlo simulations and error variance of filtering results, respectively. Monte Carlo simulation tests were conducted for 100 runs in order to obtain reliable analysis results and the average run times are reported in seconds in Figure 4(b).
Performance of the hybrid and standard UKF. (a) MSE. (b) Average run times.
The MSE was calculated as follows:
$$\mbox{MSE}=\frac{1}{C}\sum_{j=1}^C {(D_j -D_j {|} {Y_{0:j} } )(D_j -D_j {|} {Y_{0:j} })}$$where C represents the times of Monte Carlo simulations; D j and D j|Y 0 : j are the actual value and filtering estimation of state variables at the jth simulation, respectively.
Compared with the standard UKF, the simplified hybrid UKF reaches higher estimation accuracy with only about 78.7% of the computation time, thus very promising for real-time tightly-coupled stereo VIVPS. The results demonstrate that the linear Kalman filter can effectively avoid the cumulative errors existed in the standard UKF caused by the approximation of predicted covariance.
The advantage of PF optimised by the modified firefly algorithm is that it can achieve similar filtering accuracy with much fewer particles. As shown in Table 1, the variation of filtering accuracy with particle number was analysed to compare the performance of the optimised PF with that of the standard PF. Figure 5 illustrates the performance of the optimised PF with 50 particles and standard PF with 300 particles. The abscissa and ordinate of Figure 5(a) represent the times of Monte Carlo simulations and error variance of filtering results, respectively. Compared with the standard PF, the PF optimised by firefly algorithm can reach similar estimation accuracy with only about 86.7% of the computation time, indicating that the optimised PF is more suitable for the real-time requirement of the system.
Performance of the optimised and standard PF. (a) MSE. (b) Average run times.
Variation of filtering accuracy with number of particles.
The results show the PF optimised by modified firefly algorithm can obtain similar estimation accuracy with much fewer particles due to the movement towards the high likelihood region, leading to the reduction of consumption time.
4.2. Experimental results of the designed hybrid UPF for tightly-coupled stereo VIVPS
The designed method was evaluated using the publicly available KITTI Vision Benchmark Suite (Urtasun et al., Reference Urtasun, Lenz and Geiger2012), which provides ground truth GPS/IMU data as well as stereo sequences at a resolution of 1226*370 pixels and 10 fps. The selected dataset was captured in a residential area from the experimental vehicle rigidly mounted with all the sensors. The experimental trajectory is about 4128 m, which takes 517.8 s with an average speed of about 29 km/h. For the position ground truth, the trajectory of the GPS/IMU output was adopted, with open sky localisation errors of less than 5 cm owing to RTK correction signals.
The performance of the designed hybrid UPF for tightly-coupled stereo VIVPS was compared with the standard UPF. Figure 6 and Table 2 show the position estimation results of the hybrid UPF with 800 particles and standard UPF with 3,500 particles. The abscissa and ordinate of Figure 6(a) represent the x-axis and y-axis of the map, respectively. The jumps from 75 s to 100 s in Figure 6(b) were caused by the ground truth errors due to the temporary obstruction of GPS signals. Experimental results show the average execution time of the designed hybrid UPF decreases to 23.8% that of the standard UPF with similar accuracy, indicating that the hybrid UPF for tightly-coupled stereo VIVPS can better satisfy the real-time requirement of the system.
Performance of the hybrid and standard UPF. (a) Motion trajectory. (b) Position errors. (c) Average run times.
Position errors of different methods.
The translational error at each timestep was measured from the estimated position and ground truth. The root mean square error (RMSE) of the translation errors was calculated as follows:
$$\mbox{RMSE}=\sqrt {\frac{\sum_{i=1}^K {(p_{e,i} -p_{t,i} )^2} }{K}}$$where K is the total number of timesteps; p e, i and p t, i are the estimated position and ground truth at the ith timestep, respectively.
Given that the position errors are related to the trajectory length, the percentage of RMSE to trajectory length (PRMSE) was adopted to assess the accuracy of the estimated trajectories as follows:
$$\mbox{PRMSE}=\frac{\mbox{RMSE}}{L_T}$$where L T is the trajectory length.
As illustrated in Table 2, the hybrid UPF can reach similar estimation accuracy with much fewer particles compared with the standard UPF. Figure 6(c) shows that the average time for each run of the hybrid UPF is much smaller than that of the standard UPF. On the one hand, the simplified hybrid UKF reduces the computational complexity of the importance sampling (Step 2.A of the hybrid UPF) to a certain extent. More importantly, the number of particles needed is greatly reduced via the modified firefly algorithm. Since each particle needs to be filtered by UKF separately, the running times of the UKF in Step 2.A of the hybrid UPF decrease significantly due to the dramatic reduction of the number of required particles. The experimental results of the hybrid and standard UPF indicate that the designed hybrid UPF for tightly-coupled stereo VIVPS can better satisfy the real-time requirement of the system.
The methods proposed by Ramezani (Ramezani and Khoshelham, Reference Ramezani and Khoshelham2018) and Song-hui (Song-hui et al., Reference Song-hui, Ming-ming and Peng2016) were adopted for the evaluation of the designed hybrid UPF in this paper. The test results show that the hybrid UPF performs better and reaches an accuracy of 0.304% of the trajectory length compared with 0.885% and 1.153% for the methods proposed by Ramezani and Song-hui, respectively. The higher accuracy of the hybrid UPF is due to the fact that PF does not perform linearisation approximation as the EKF used in the method presented by Ramezani does. The hybrid UPF can obtain more accurate positioning results because UPF is more suitable for highly nonlinear systems than the UKF adopted in the method proposed by Song-hui.
5. CONCLUSIONS
This paper proposes a hybrid UPF optimised by the firefly algorithm for tightly-coupled stereo VIVPS. The linear/nonlinear mixed filter model of tightly-coupled stereo VIVPS is established based on the error-state kinematics of the SINS and trifocal geometry of stereo vision. A simplified hybrid UKF is designed via the employment of the formula of standard linear Kalman filter for the time updating and spherical simplex sampling strategy for the measurement updating. A PF optimised via modified firefly algorithm is presented to reduce the number of particles needed. The formulas of position and attraction are revised so that the firefly algorithm can be implanted into the particle updating. Experiments of numerical simulation verify the performance of the designed simplified hybrid UKF and PF optimised by modified firefly algorithm. The proposed hybrid UPF optimised by firefly algorithm was tested using the KITTI data which is a publicly available real-world dataset. Compared with the standard UPF, the hybrid UPF optimised by the firefly algorithm can reach similar estimation accuracy with only about 23.8% of the computation time, indicating that the designed approach for tightly-coupled stereo VIVPS is more suitable for the real-time requirement of the system.
ACKNOWLEDGEMENTS
This work was supported by the Talent Award Scheme of Shanxi Province (Grant no. 304/18001618) and the Science Foundation of North University of China (Grant no.304/20170024).
