HTML
-
Navigation system is one of the most used applications on wheelchairs in the field of robotic systems, it can provide advantages to users of this type of chairs due to its simplicity. In ordinary life, this category needs to navigate and to know strategic places such as parking, metro stations and also inside a building composed of several places without the help of anyone. This requires the development of a system capable to accomplish these tasks, which allow the places identification and easy navigation, including helping to locate the chair mobility at any time. Practically, the location determination using a single sensor is insufficient to capture all relevant characteristics of real environments. Then, it is necessary to combine data from multiple sensors in a process known as fusion. Multi-sensor data fusion is a technology to allow combining information from several sensors, where the goal is to have a better accuracy and to give a more precise result than the one that could be obtained by the use of one single sensor[1, 2]. On the other hand, the multi-sensor data fusion techniques are applied to improve the system performance in several ways, such as increasing the robustness and reliability, and the methods are used to optimize uncertainty and to reduce the reduction time of measurement and cost[3, 4]. In the literature, a few authors have worked in this research area[5, 6]. For instance, Naidu[7] presented different fusion architectures to combine the radar data with infra-red search and track (IRST) in the target tracking system by using extended Kalman filter (EKF). In [8], an EKF was designed. It combines the noisy data measurements of encoders (ENC), compass and accelerometer (ACC) in order to obtain the best position estimate while reducing measurement uncertainties. The proposed algorithm is based on two data fusion architectures, state vector fusion (SVF) and measurement fusion (MF). Simpson et al.[9] developed smart wheelchairs for navigation assistance mainly in indoor environments, however Ding et al.[10] developed a smart wheelchair equipped with wireless internet access and global positioning system (GPS). Defects of the previous methods are of large cost in addition to being dependent to being not effective in closed places and other places with internet access. In the network-centric applied research team (N-CART), within the NEPWAK project, Ferworn et al.[11] introduced techniques for modifying and using power wheelchairs as mobile platforms, enabling communication and remote control, and extended the mobility range of Ferworn NEPWAK beyond a local area. Whereas in [12], the authors described the development of a navigation system to give a semi-autonomous operation of wheelchairs where they make extensive use of freely available open source software. However, Wattavarangkul and Wakahara[13] present an indoor navigation system for wheelchairs, using smartphones as a sensor for navigation tools. The system in our hands is depending on a range of sensors which are different in terms of manufacturer, technology, mechanism of action and the action protocol. There exist many complications and problems in the case of mixing them[14]. For this reason, our task in this paper is to find the best way that allows the use of different information from various sensors and simultaneously make correct judgments. In the literature, different navigation systems are reformulated as a nonlinear estimation problem where the optimal solution is given by the EKF[15]. EKF has been a popular approach for localization of a mobile robot[16]. However, the estimation quality performance of the EKF depends on the correct a priori knowledge of process measurement noise and covariance matrices
$Q$ and$R$ , respectively. Imprecise knowledge of these statistics can lead to a significant degradation in performance. The appearance of the family of deterministic methods shows that the unscented Kalman filter (UKF) can aim higher estimation precision than EKF. Fundamental difference between EKF and UKF is on the manner in which the Gaussian random variable (GRV) is shown for propagating through the system dynamics. In the EKF, state distribution is approximated analytically, which can introduce large errors in the true covariance of GRV, this can lead to suboptimal performance and sometimes filter divergence. Nevertheless, in UKF the reconciliation of state estimated distribution is made by points construction with different weights based on deterministic methods[17]. These points are able to capture the true mean and covariance of the GRV. The problem connected with UKF resides in computational cost and numerical instability, also it is not always able to have these two properties and algorithms may diverge. The divergence in general may occur due to different reasons, including incorrect or incomplete model underlying the physical system[18]:1) loss of information to capture the true posterior density changes completely, for example, a nonlinear filter designed under the gaussian assumption may fail to capture the key characteristics of a posterior density multimodal.
2) The high degree nonlinear system describing numerical errors in equations at the state space model levels.
3) Numerical errors. Indeed, each of the aforementioned filters has its own domain of applicability and it is doubtful that a single filter would be considered effective for a wide range of applications. The motivation of this study was to introduce a more accurate nonlinear filter that could be applied to solve a wide range (from low to large) nonlinear filtering problems. In this aspect, we find two architectures of multi-sensor data fusion, able to use the information from all sources and merge them to provide optimal information.
The purpose of this paper is to improve the performance of this estimator (UKF) and find architectures able to combine noisy measurements from all sources in order to obtain the best estimate of the state, while reducing measurement uncertainty. The first technique is a fusion architecture which is a measurement fusion (MF), and the second one is another architecture with different principles that is the state vector fusion (SVF)[5, 14]. The two proposed methods of fusion architectures proved their effectiveness in this area and can handle any kind of sensors. They require only a small number of sensors with the lowest cost.
This paper is organized as follows: details of the system and the fusion algorithms are respectively described in Sections 2 and 3. The study is reinforced by evaluation of each studied method in Section 4. Section 5 presents the results of the simulation. The conclusion includes system evaluation as well as the advantages and disadvantages of the proposed algorithm.
-
The sensor system consists of a magnetic compass, an accelerometer and two odometers (absolute encoders). These odometers determine the position
$(x, y)$ and the heading using the rotary encoders[19]. Mounted on the wheel, they deliver information about elementary rotation, which by integration gives a measure of the overall motion. The resolution of the measurement of the wheels speed is increased in a ratio corresponding to that of the motion transmission system. Its accuracy depends on the sharpness of the network features, the quality of the mechanical design and the performance of the electronic signal processing (interpolation, scanning)[20]. The encoder consisted of three parts: mechanical axis, disc with its reader, and output signals. The disc has$n$ tracks divided into$n$ equal segments alternating opaque and transparent. A pair of optical transceivers is assigned in each track. For each position of the axis, the disc provides a code when the length$n$ is$(\frac{1}{{2^n }})$ -th rounds. We have chosen a simple absolute encoder ($2^{11} = 2 048$ points) of$64$ laps. The odometers are based on simple equations that are verified when the wheel revolutions can be translated precisely into linear displacements relative to the earth[21]. In the case of wheel slippage or other more subtle causes, the wheel laps cannot translate the linear movement proportionally. The resulting errors can be classified into two groups: systematic and unsystematic. Systematic errors are those resulting from the kinematic imperfections of the vehicle, such as unequal wheel diameters or uncertainty about the exact wheel-base. On the other hand, unsystematic errors are those resulting from the road interaction with wheels like the dents or the cracks[20]. Many solutions have been proposed to improve the systematic errors. Decoders must be put on special wheels which are not subject to the vehicle load effects, and can be matched to the drive wheels. Unsystematic errors are corrected by the inclusion of other types of sensors. The orientation of wheelchair is determined by using a magnetic compass HMC1002, also called a magnetometer which indicates the direction of magnetic north[22]. Generally, magnetic declination is compensated, so that the sensor delivers continuously an absolute measure of heading related to the direction of true north. The module has two axes$ X$ and$Y$ , based on the Hall effect with the resolution of$0.1^\circ$ as in [23]. In order to achieve the degree of compass accuracy, a magnetic sensor is needed to reliably solve the angular variations of$0.1^\circ$ using the following relationship of orientation[24]:$ \begin{equation} \label{eq1} \theta=\tan^{-1}(\frac{y}{x}). \end{equation} $
(1) To take account of the tangent function validity over
$180^\circ$ and to not allow calculation involving division by$0$ , the following equations can be used:$ \begin{equation*} \label{eq2} ~~~~ {\rm \theta } = \left\{ \begin{array}{l} 90^\circ {\rm }, ~~~~{ x} = 0{{\rm ~~~~and} ~~~~~~~~y} < 0 \\ 270^\circ {\rm }, ~~~~{ x} = 0{{\rm ~~~~and} ~~~~~~~~y} > 0 \\ \left( {\left( {180 - \tan ^{ - 1} (\dfrac{{ y}}{{ x}})} \right){\times}\dfrac{{180}}{{{ pi}}}} \right)^{\rm o}, ~{ x} < 0 \\[0.3cm] \left( {\left( { - \tan ^{ - 1} (\dfrac{{ y}}{{ x}})} \right){\times}\dfrac{{180}}{{{ pi}}}} \right)^{\rm o}, ~{ x} > 0~{\rm and}~y < 0 \\[0.3cm] \left( {\left( {360 - \tan ^{ - 1} (\dfrac{{ y}}{{ x}})} \right){\times}\dfrac{{180}}{{{ pi}}}} \right)^{\rm o}, { x} > 0~{\rm and}~y > 0. \\ \end{array} \right. \end{equation*} $
(2) To measure the linear acceleration, an accelerometer is used. The principle of most accelerometers is based on the fundamental law of motion which is
$F = m\times a $ , where$F$ is the force (N),$m$ is the mass (kg) and$a$ is the acceleration (m/s$^2 $ ). Specifically, it consists of the equality between the inertial mass of the seismic force sensor and a biasing force applied to the mass. One thing in common for all accelerometers is that the displacement of the seismic mass will be measured by a position measurement interface circuit and it is then converted to a type of signal[25-27]. In our study, it is assumed that the measurement interface provides two outputs$x$ and$y$ .
-
The proposed platform occupies the wheelchair with three sensor types, compass, accelerometer, and absolute encoders having the ability to perceive parameters helping to determine the position. The raw data of these sensors are combined using an unscented Kalman filter. The objective is then to determine the position of the wheelchair during operations as accurately as possible[21].
We start with the kinematic model of the wheelchair. On each rear wheel, an odometer is mounted. The measurements are made by rotary encoders coupled to the wheel axis. Fig. 1 shows the system coordinates as well as the wheelchair notations where (
$X_r$ ,$Y_r$ ) are the global coordinates and ($X_f$ ,$Y_f$ ) are the local coordinates of the wheelchair frame.$r$ denotes the radius of the wheel and$l$ the distance between the two wheels. During a sampling period$\Delta t$ [28, 29], left and right rotational speeds$v_g$ and$v_d$ create elementary displacements$\Delta d_g $ and$\Delta d_d $ which are driven respectively by left and right wheels as in (3):$ \begin{equation*} \label{eq3} \left\{{\begin{array}{l} {\Delta d_g=\Delta t\times r\times v_g}\\ {\Delta d_d=\Delta t\times r\times v_d}.\\ \end{array}}\right. \end{equation*} $
(3) 
Figure 1. Configuration and measurement model. System is equipped with both encoders, compass and accelerometer
These can be converted to linear incremental movement
$\Delta d$ of the chair frame center and the orientation angle$\Delta \theta$ as in (4):$ \begin{equation*} \label{eq4} \left\{{\begin{array}{l} {\Delta d =\dfrac{{\left({{\rm\Delta} d_g+{\rm\Delta }d_d}\right)}}{2}} \\ {\Delta \theta=\dfrac{{\left({{\rm\Delta }d_g-{\rm\Delta }d_d}\right)}}{l}}.\\ \end{array}}\right. \end{equation*} $
(4) According to [27, 28], the basic movements during time
$k$ to$k+1$ are given by the following equation:$ \begin{equation*} \label{eq5} \left\{ {\begin{array}{l} {x_{k + 1} = x_k + {\rm \Delta }d_k \times \cos (\Phi )} \\ {y_{k + 1} = y_k + {\rm \Delta }d_k \times \sin (\Phi )} \\ {\theta _{k + 1} = \theta _k + {\rm \Delta }\theta _k } \\ \end{array}} \right. \end{equation*} $
(5) where
$ { \Phi} = (\theta _k + \frac{{{\rm \Delta } {\theta} _k }}{2})$ .Practically, (5) is not really accurate because inevitable errors occur in the system. These errors can be both systematic such as the imperfect model and unsystematic such as wheel slippage. They have accumulated characteristics that affect the system stability if adequate compensation is not considered. In our system, the compensation is performed by merging measures of a compass, an accelerometer and absolute encoders. This is accomplished by using the UKF. Equation (6) is described by a nonlinear function
$f$ , coordinate of a wheelchair and a process of Gaussian noise$w$ . The state of the system can be observed by some absolute measures that can be described by a nonlinear function$h$ , coordinate of a wheelchair and an independent Gaussian noise process$v$ . The system is described as$ \begin{equation*} \label{eq6} \left\{ {\begin{array}{l} {X_{k + 1} = f(X_k, u_k, w_k )} \\ {Z_k = h(X_k, v_k )} \\ \end{array}} \right. \end{equation*} $
(6) where
$X_k $ is the state vector at time$k$ and$Z_k $ is the observation vector at time$k$ .The random variables
$w_k$ and$v_k$ represent the process and measurement noise respectively. They are assumed to be independent from each other, white, and with normal probability distributions:$ w_k \sim N(0, Q_k ), v_k \sim N(0, R_k ), E(w_i v_j^{\rm T} )=0. $
In the case of the nonlinear model, the optimal estimation solution is the EKF. This method of filtering gives good results when the models of evolution of the status and measurement are close to the linear case (first order approximation). However, the optimality of the filter and the convergence are not guaranteed. It can be linearized only in the case of existence of the Jacobian matrix that can be difficult to calculate and may generate errors in the algorithm. In this work, the deterministic methods that share the same principles are studied. These filters are based on a deterministic sampling approach for the Bayesian solution numerically, using the unscented transformation for obtaining the unscented Kalman filter (UKF)[30].
-
Unscented transformation (UT) is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation. The UT builds on the principle that it is easier to approximate a probability distribution of an arbitrary nonlinear function. The UT uses a small number of chosen test points, which are called Sigma points. Each sigma point is propagated through the nonlinear function, and then the mean and covariance are computed through the use of a weighted statistical linear regression process[31]. These points are then propagated through the nonlinear function whose mean and covariance of the estimation are then recovered. The result is a filter that captures most accurate mean and the true covariance[32].
-
For the
$n$ -dimensional random variable with known mean and covariance$P_x$ , a given known nonlinear transformation${ y} = { h} _{NL} ({ X)}$ , the mean and covariance of$y$ , denoted as$y$ and$P_y$ can be estimated from$2 L+1$ sigma point vectors as follows[33]:$ \begin{eqnarray*} \label{eq7} %\setcounter{equation}{7} \left\{ {\begin{array}{ll} {{ X}_0 = { \bar X}, ~~ i = 0{\rm }} \\ {{ X}_{ i} = { \bar X} + {\rm }\left[{\sqrt {\left( {{ L} + { \lambda }} \right){ P}_{ x} } } \right]_{\rm i} {\rm, }~~ i = 1 \cdots { L}} \\[0.3cm] {{ X}_{ i} = { \bar X} - \left[{\sqrt {\left( {{ L} + { \lambda }} \right){ P}_{ x} } } \right]_{{ i} - { n}_{ x} }, ~~{ i} = { L} + 1 \cdots 2{ L}} \\ \end{array}} \right. \end{eqnarray*} $
(7) where
${\rm }\left[{\sqrt {(n_x + {\rm \lambda })P_x } } \right]_i $ is the$ i$ -th column of the matrix square root of$\left({L + {\rm \lambda }} \right){ P}_{\rm x} $ ,$i$ is an index of sigma points,$L$ is the dimension number of the augmented state vector,$\lambda$ is computed by${\rm \lambda } = \alpha ^2 \left({L + k} \right) -L$ , with$\alpha$ and$k$ are scaling parameters that determine how far the sigma points are spread from the mean$\bar X$ . In this paper,$\alpha = 10^{ -3} $ ,$k = 0$ and$B = 2$ . The weight is defined as$ \begin{equation*} \label{eq8} \left\{ \begin{array}{l} { W}_{0\left( { X} \right)} = \dfrac{{ \lambda }}{{{ n}_{\rm x} + { \lambda }}}, ~~ { i} = 0 \\ { W}_{0\left( { P} \right)} = \dfrac{{ \lambda }}{{{ n}_{\rm x} + { \lambda }}} + \left( {1 - {\rm \alpha }^2 + {\rm \beta }} \right), ~~ { i} = 0 \\ { W}_{{ i}\left( { X} \right)} = { W}_{{ i}\left( { P} \right)} = \dfrac{1}{{2({ n}_{ x} + { \lambda })}}, ~~ { i} = 1 \cdots { }2{ L}. \\ \end{array} \right. \end{equation*} $
(8) UKF algorithm proceeds according to the following successive stages[17]: We assume that the observations denoted as
${ y}_{ k}$ , state noise and observation are$Q$ and$R$ . The initial state and initial covariance are${ X}_0$ and${ P}_0$ .1) Initialisation
$X_{0|0} = X_0 $ ,$P_{0|0} = P_0 $ . Compute the weight$W_i$ , refer to (8).2) Estimation
For
$k=1$ to infinitya) Prediction
ⅰ) Calculate
${ X}_i $ deterministic samples according to (7):$ ({ X}_0, { X}_1, \cdots, { X}_L, \cdots, { X}_{2L} ). $
ⅱ) Evaluation of samples propagates
$\left({\hat X_{0, k|k -1}, \cdots, \hat X_{n_{x, k|k -1} }, \cdots, \hat X_{2n_x, k|k -1} } \right) $ such that$ \begin{equation} \hat X_{i, k|k - 1} = f\left( {{ X}_{i, k - 1|k - 1}, u_{k - 1} } \right). \end{equation} $
(9) ⅲ) The predicted state and the predicted covariance are computed as
$ \begin{equation} \label{eq10} X_{k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat X_{i, k|k - 1} \end{equation} $
(10) $ \begin{align} \label{eq11} P_{k|k - 1} \approx& \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1} - X_{k|k - 1} )\times\nonumber\\ &(\hat X_{i, k|k - 1} - X_{k|k - 1} )^{\rm T} + Q_k \end{align} $
(11) where
$ \begin{equation} Q_k = \left[{\begin{array}{*{20}c} {\sigma _x^2 } & 0 & 0 \\ 0 & {\sigma _y^2 } & 0 \\ 0 & 0 & {\sigma _\theta ^2 } \\ \end{array}} \right]. \end{equation} $
(12) b) Correction
ⅰ) Correct and propagate evaluation samples
$ \begin{equation} (\widehat{{ Y}}_{0, k|k - 1}, \cdots, \widehat{{ Y}}_{L, k|k - 1}, \cdots, \widehat{{ Y}}_{2L, k|k - 1} ) \end{equation} $
(13) knowing that
$ \begin{equation} \label{eq14} \hat { Y}_{i, k|k - 1} = h\left( {\hat X_{i, k|k - 1}, u_k } \right). \end{equation} $
(14) ⅱ) Estimate the predicted observation
$\widehat{y}_{k|k -1} $ , the predicted covariance error$P_{yy, k|k -1}$ and the cross covariance$P_{xy, k|k -1}$ $ \begin{equation} \label{eq15} \widehat{y}_{k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat { Y}_{i, k|k - 1} \end{equation} $
(15) $ \begin{equation} \label{eq16} \begin{array}{l} P_{yy, k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat { Y}_{i, k|k - 1} - \widehat{y}_{k|k - 1} )\times~\\ ~~~~~~~~~~~~~~~~\left( {\hat { Y}_{i, k|k - 1} - \widehat{y}_{k|k - 1} } \right)^{\rm T} + R \end{array} \end{equation} $
(16) $ \begin{equation} \label{eq17} \begin{array}{l} P_{xy, k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1} - X_{k|k - 1} )\times\\ ~~~~~~~~~~~~~~~~\left( {\hat { Y}_{i, k|k - 1} - \widehat{y}_{k|k - 1} } \right)^{\rm T} . \end{array} \end{equation} $
(17) ⅲ)Compute the Kalman gain
$G_k$ :$ \begin{equation} \label{eq18} G_k = \frac{P_{xy, k|k - 1} }{P_{yy, k|k - 1}}. \end{equation} $
(18) ⅳ) Correct the state and the covariance matrix error:
$ \begin{equation} \label{eq19} X_{k|k} = X_{k|k - 1} + G_k \left( {y_{k} - \widehat{y}_{k|k - 1} } \right) \end{equation} $
(19) $ \begin{equation} \label{eq20} P_{k|k} = P_{k|k - 1} - G_k P_{yy, k|k - 1} G_k ^{\rm T}. \end{equation} $
(20) End for.
-
To improve the measurement performance, we have proposed two architectures to merge the sensors measures state vector fusion (SVF) and measurement fusion (MF). In both architectures, we used compass measures merged with the accelerometer measurements in a single measurement vector,
$Y_k^{\left(b \right)} = \left[{x_a {\rm }y_a \theta _b } \right]^{\rm T} $ .The covariance matrix of this vector is defined as follows:
$ \begin{equation} \label{eq21} R^{\left( b \right)} = \left[{\begin{array}{*{20}c} {\sigma _{a, x}^2 } & 0 & 0 \\ 0 & {\sigma _{a, y}^2 } & 0 \\ 0 & 0 & {\sigma _{b, \theta }^2 } \\ \end{array}} \right] \end{equation} $
(21) where
$Y_k^{\left(c \right)} = \left[{x_c~ y_c ~\theta _b } \right]^{\rm T} $ vector corresponds to the odometer measurements merged with the compass measurements. The covariance matrix of this vector is defined as$ \begin{equation} \label{eq22} R^{\left( c \right)} = \left[{\begin{array}{*{20}c} {\sigma _{c, x}^2 } & 0 & 0 \\ 0 & {\sigma _{c, y}^2 } & 0 \\ 0 & 0 & {\sigma _{b, \theta }^2 } \\ \end{array}} \right]. \end{equation}\\ $
(22) -
The operation principle of SVF is based on the position calculation from the measurement of each sensor separately and the resulting state vectors are merged to obtain a final state vector. The covariance matrices are combined to give the final state matrix (Fig. 2).
Figure 2. Diagram of SVF architecture
For measurements of
$Y_k^{\left(c \right)}$ :1) Prediction stage:
$ \begin{equation} \label{eq23} \hat X_{i, k|k - 1}^{\left( c \right)} = f\left( {{ X}_{i, k - 1|k - 1}^{\left( c \right)}, u_{k - 1} } \right) \end{equation} $
(23) $ \begin{equation} \label{eq24} X_{k|k - 1}^{\left( c \right)} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat X_{i, k|k - 1}^{\left( c \right)} \end{equation} $
(24) $ \begin{align} \label{eq25} P_{k|k - 1}^{\left( c \right)} =& \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1}^{\left( c \right)} - X_{k|k - 1}^{\left( c \right)} ) \times\nonumber\\ & (\hat X_{i, k|k - 1}^{\left( c \right)} - X_{k|k - 1}^{\left( c \right)} )^{\rm T} + Q_k. \end{align} $
(25) 2) Correction stage:
$ \begin{equation} \label{eq26} \hat {\cal Y}_{i, k|k - 1}^{\left( c \right)} = h\left( {\hat X_{i, k|k - 1}^{\left( c \right)}, u_k } \right) \end{equation} $
(26) $ \begin{equation} \label{eq27} \widehat{y}_{k|k - 1}^{\left( c \right)} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat { Y}_{i, k|k - 1}^{\left( c \right)} \end{equation} $
(27) $ \begin{align} \label{eq28} P_{yy, k|k - 1}^{\left( c \right)} = & \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat { Y}_{i, k|k - 1}^{\left( c \right)} - \widehat{y}_{k|k - 1}^{\left( c \right)} - )\times\nonumber\\ &\left( {\hat { Y}_{i, k|k - 1}^{\left( c \right)} - \widehat{y}_{k|k - 1}^{\left( c \right)} } \right)^{\rm T} + R^{\left( c \right)} \end{align} $
(28) $ \begin{align} \label{eq29} P_{xy, k|k - 1}^{\left( c \right)} =& \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1} - X_{k|k - 1} )\times\nonumber\\ &\left( {\hat { Y}_{i, k|k - 1}^{\left( c \right)} - \widehat{y}_{k|k - 1}^{\left( c \right)} } \right)^{\rm T} \end{align} $
(29) $ \begin{equation} \label{eq30} G_k^{\left( c \right)} = \frac{P_{xy, k|k - 1}^{\left( c \right)} }{P_{yy, k|k - 1}^{\left( c \right)}} \end{equation} $
(30) $ \begin{equation} \label{eq31} X_{k|k}^{\left( c \right)} = X_{k|k - 1}^{\left( c \right)} + G_k^{\left( c \right)} (Y_k^{\left( c \right)} - \widehat{y}_{k|k - 1}^{\left( c \right)} ) \end{equation} $
(31) $ \begin{equation} \label{eq32} P_{k|k}^{ ( c )} = P_{k|k - 1}^{ ( c )} - G_k^{ ( c )} P_{yy, k|k - 1}^{(c)} G_k^{{(c)}{\rm T}}. \end{equation} $
(32) For measurements of
$Y_k^{\left(b \right)}$ :1) Prediction stage: Equations (23)-(25).
2) Correction stage: Equations (26)-(23).
SVF:
$ \begin{align} \label{eq33} X_{f{\rm }, k|k} =& X_{k|k}^{\left( c \right)} {\rm } + P_{k|k}^{\left( c \right)} \times\left[{P_{k|k}^{\left( c \right)} + P_{k|k}^{\left( b \right)} } \right]^{ - 1} \times\nonumber\\ &\left( {X_{k|k}^{\left( b \right)} {\rm } - X_{k|k}^{\left( c \right)} {\rm }} \right) \end{align} $
(33) $ \begin{equation} \label{eq34} P_{f, k|k} = P_{k|k}^{\left( c \right)} + P_{k|k}^{\left( b \right)} \times\left( {P_{k|k}^{\left( c \right)} + P_{k|k}^{\left( b \right)} } \right)^{ - 1} \times P_{k|k}^{{\left( c \right)} ^{\rm T}}. \end{equation} $
(34) -
In this architecture (Fig. 3), all sources of measurements are merged into a measurement vector. On the other hand, the covariance matrices of these sources are also merged to produce the same results.
Figure 3. Diagram of MF architecture
1) Prediction stage:
$ \begin{equation}\label{eq35} \hat X_{i, k|k - 1} = f\left( {{ X}_{i, k - 1|k - 1}, u_{k - 1} } \right) \end{equation} $
(35) $ \begin{equation}\label{eq36} X_{k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat X_{i, k|k - 1} \end{equation} $
(36) $ \begin{equation*}\label{eq37} \begin{array}{l} P_{k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1} - X_{k|k - 1} )\\ ~~~~~~~~ (\hat X_{i, k|k - 1} - X_{k|k - 1} )^{\rm T} + {\rm }W_k \times Q_{k - 1} \times W_k^{\rm T}.~~~~ \end{array} \end{equation*} $
(37) 2) MF:
$ \begin{equation} Y_f = Y^{\left( c \right)} + Y^{\left( c \right)} \times \left[{R^{\left( c \right)} + R^{\left( b \right)} } \right]{\rm }^{ - 1} \times \left( {Y^{\left( b \right)} - Y^{\left( c \right)} } \right) \end{equation} $
(38) $ \begin{equation}\label{eq39} R_f = R^{\left( c \right)} - R^{\left( c \right)} \times \left[{R^{\left( c \right)} + R^{\left( b \right)} } \right]{\rm }^{ - 1} \times R^{\left( c \right)}. \end{equation} $
(39) 3) Correction stage:
$ \begin{equation}\label{eq40} \hat { Y}_{i, k|k - 1} = h\left( {\hat X_{i, k|k - 1}, u_k } \right) \end{equation} $
(40) $ \begin{equation}\label{eq41} \widehat{y}_{k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( x \right)} \hat { Y}_{i, k|k - 1} \end{equation} $
(41) $ \begin{align}\label{eq42} %\begin{array}{l} P_{yy, k|k - 1} = \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat { Y}_{i, k|k - 1}-\widehat{y}_{k|k - 1} )\times \nonumber\\ ~~~~~~~~~~~~~~~\left({\hat { Y}_{i, k|k - 1} - \widehat{y}_{k|k - 1} } \right)^{\rm T} + R_f %\end{array} \end{align} $
(42) $ \begin{align}\label{eq43} P_{xy, k|k - 1} =& \sum \limits_{i = 0}^{2L} w_{i\left( p \right)} (\hat X_{i, k|k - 1}- X_{k|k - 1} )\times\nonumber \\ &\left({\hat { Y}_{i, k|k - 1} - \widehat{y}_{k|k - 1} } \right)^{\rm T} \end{align} $
(43) $ \begin{equation}\label{eq44} G_k = \dfrac{P_{xy, k|k - 1} }{P_{yy, k|k - 1}} \end{equation} $
(44) $ \begin{equation}\label{eq45} X_{k|k} = X_{k|k - 1} + G_k \left( {Y_f - \widehat{y}_{k|k - 1} } \right) \end{equation} $
(45) $ \begin{equation}\label{eq46} P_{k|k} = P_{k|k - 1} - G_k P_{yy, k|k - 1} G_k ^{\rm T}. \end{equation} $
(46)
3.1. Kinematics model design
3.2. Unscented transformation (UT)
3.3. Unscented Kalman filter (UKF)
3.4. Multi-sensor data fusion
3.4.1. SVF architecture (state vector fusion)
3.4.2. MF architecture (measurement fusion)
-
Both fusion architectures are evaluated by computing the following performance metrics:
1) Absolute error (AE) in
$x$ positions is$ \begin{equation}\label{eq47} AE_x = \left| {x_i - \widehat{x_i }} \right|, \quad i = 1, 2, \cdots, N \end{equation} $
(47) similarly for
$y$ positions and$\theta $ orientation.Where
$x_i, y_i, \theta _i $ are the true positions and$\widehat{x_i }, \widehat{y_i }, \widehat{\theta _i }$ are the estimated$x_i, y_i $ positions and$\theta _i $ orientations.2) Mean absolute error in
$x$ positions is$ \begin{equation} \label{eq48} MAE_x = \frac{1}{N} \sum \limits_{i = 1}^N |x_i - \widehat{x_i }| \end{equation} $
(48) similarly for
$y$ positions and$\theta $ orientation.3) The percentage fit error (PFE) in
$x$ positions is$ \begin{equation} PFE_x = 100\times \frac{{norm\left( {x - \widehat x} \right)}}{{norm\left( x \right)}} \end{equation} $
(49) similarly for
$y$ positions and$\theta $ orientation.4) Root mean square error in
$x$ and$y$ position is$ \begin{equation} RMSPE = \sqrt {\frac{1}{N} \sum \limits_{i = 1}^N \frac{{(x - \widehat x)^2 + (y - \widehat y)^2 }}{2}}. \end{equation} $
(50) 5) Root sum square error in
$x$ and$y$ positions:$ \begin{equation} RSSPE = \sqrt {(x - \widehat x)^2 + (y - \widehat y)^2 }. \end{equation} $
(51) 6) State error (SE) is
$ SE_x = (x - \widehat x) $
with theoretical bounds of
$ \pm 2\sqrt {\widehat{P}_x} $
$ SE_y = (y - \widehat y) $
with theoretical bounds of
$ \pm 2\sqrt {\widehat{P}_y } $
$ SE_\theta = (\theta - \widehat\theta ) $
with theoretical bounds of
$ \pm 2\sqrt {\widehat{P}_\theta. } $
7) Compute innovation sequence
$IS = Z_k -\widetilde Z_{{\rm }k|k -1}$ while the measurements are available in Cartesian coordinates:$IS_x $ with theoretical bounds of$ \pm 2\sqrt {\widehat{S}_x } $ $IS_y$ with theoretical bounds of$\pm 2\sqrt {\widehat{S}_y } $ .
-
In this section, an experiment was simulated to calculate the position under two different scenarios: with the MF algorithm and the SVF algorithm.
The simulation uses the following parameters:
1) Sampling period
$\Delta t{\rm = 0}{\rm.1}$ s.2) Initial state vector:
$X_0 = \left[{{\rm }\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array}} \right]^{\rm T}.$ 3) The initial covariance matrix is defined by
$ \begin{equation} P_0 = \left[{\begin{array}{*{20}c} {0.01} & 0 & 0 \\ 0 & {0.01} & 0 \\ 0 & 0 & {0.3} \\ \end{array}} \right]. \\ \end{equation} $
(52) 4) The covariance matrices of measurements noise:
$ \begin{equation} R_ b = \left[{\begin{array}{*{20}c} {10^{-8} } & 0 & 0 \\ 0 & {10^{-8} } & 0 \\ 0 & 0 & {85\times 10^{-9} } \\ \end{array}} \right] \\ \end{equation} $
(53) $ \begin{equation} R_c = \left[{\begin{array}{*{20}c} {10^{-8} } & 0 & 0 \\ 0 & {10^{-8} } & 0 \\ 0 & 0 & {8\times 10^{-4} } \\ \end{array}} \right].{\rm }\\ \end{equation} $
(54) 5) The noise system covariance matrix is
$ \begin{equation} Q_k = {\rm }\left[{\begin{array}{*{20}c} {0.01^2 } & 0 & 0 \\ 0 & {0.01^2 } & 0 \\ 0 & 0 & {0.1^2 } \\ \end{array}} \right]. \end{equation} $
(55) Both data fusion architectures are evaluated by using a sequential simulation algorithm of the unscented Kalman filter. The mean absolute error in position is shown in Table 2 and Figs. 4-7.
Table 1. Percentage fit errors in position
Figure 4. Mean absolute errors in
$x$ 
Figure 5. Mean absolute errors in
$y$ 
Figure 6. Mean absolute errors in
$\theta$ 
Figure 7. Zoomed view of Fig. 6
Table 2. Mean absolute error in position
Percentage of fit error (PFE) and the root mean square error in position (RMSPE) are presented in Table 1. The variations in the mean square error and the execution time for each algorithm are shown in Table 3 and Fig. 8.
Figure 8. Execution time for each algorithm
Table 3. Mean root sum variance in position and execution time
The state error with its theoretical limits shows the filter robustness. Figs. 9 and 10 show that the MF algorithm is more robust than the SVF one and therefore presents a relatively high performance.
Figure 9. State error in positions with theoretical bounds for MF algorithm
Figure 10. State error in positions with theoretical bounds for SVF algorithm
Bold values indicate the best results. The root sum square error in position is shown in Fig. 11. From the tables and figures, we can see that the location with the odometer or accelerometer shows performance degradation compared to fusion architectures. MF shows the lowest uncertainty followed by SVF. The uncertainty in the state estimate is high if the odometer is followed by the accelerometer. This shows the need for fusion. For the execution time, localization with odometer takes less time with degraded performance.
Figure 11. Root sum square error in position
On the other hand, we investigated the consistency of the MF and SVF algorithms using sequences of innovation with theoretical limits, as shown in Figs. 12 and 13. The simulation results show that the two algorithms are consistent.
Figure 12. Innovation sequence with theoretical bounds for MF algorithm
Figure 13. Innovation sequence with theoretical bounds for SVF algorithm
Finally, we studied the autocorrelation (AC) of the innovation sequence to show that the two algorithms extracted all the information represented by the signal. The simulation result shows in Figs. 14 and 15 that both algorithms are satisfactory.
Figure 14. Autocorrelation of innovation sequence along with theoretical bounds for MF algorithm
Figure 15. Autocorrelation of innovation sequence along with theoretical bounds for SVF algorithm
-
In order to increase the UKF algorithm performance for the position estimation, we showed the importance and the need for data fusion. Two fusion algorithms (SVF and MF) were addressed and detailed mathematical expressions are given which could be useful for the implementation. The simulation results show that the MF algorithm provides state estimates with relatively less uncertainty followed by the SVF algorithm. For future development, the estimation problem of wheelchair position could also be investigated by using the cubature Kalman filter (CKF)[34]. In addition, the Bayesian nonlinear filtering using quadrature and cubature rules[35], will also solve this problem. We hope that this work will be an effective solution for reducing navigation problems for wheelchair users.
DownLoad: