Adaptive Fading Extended Kalman Filtering for Mobile Robot Localization Using a Doppler–Azimuth Radar

Abstract

In this paper, the localization problem of a mobile robot equipped with a Doppler–azimuth radar (D–AR) is investigated in the environment with multiple landmarks. For the type (2,0) robot kinematic model, the unknown modeling errors are generally aroused by the inaccurate odometer measurement. Meanwhile, the inaccurate odometer measurement can also give rise to a type of unknown bias for the D–AR measurement. For reducing the influence induced by modeling errors on the localization performance and enhancing the practicability of the developed robot localization algorithm, an adaptive fading extended Kalman filter (AFEKF)-based robot localization scheme is proposed. First, the robot kinematic model and the D–AR measurement model are modified by considering the impact caused by the inaccurate odometer measurement. Subsequently, in the frame of adaptive fading extended Kalman filtering, the way to the addressed robot localization problem with unknown biases is sought out and the stability of the developed AFEKF-based localization algorithm is also discussed. Finally, in order to testify the feasibility of the AFEKF-based localization scheme, three different kinds of modeling errors are considered and the comparative simulations are conducted with the conventional EKF. From the comparative simulation results, it can be seen that the average localization error under the developed AFEKF-based localization scheme is ${\left[0.0245\mathrm{m}0.0224\mathrm{m}0.0039\mathrm{rad}\right]}^T$ and the average localization errors using the conventional EKF are ${\left[1.0405\mathrm{m}2.2700\mathrm{m}0.1782\mathrm{rad}\right]}^T$, ${\left[0.4963\mathrm{m}0.3482\mathrm{m}0.0254\mathrm{rad}\right]}^T$ and ${\left[0.2774\mathrm{m}0.3897\mathrm{m}0.0353\mathrm{rad}\right]}^T$, respectively, under the three cases of the constant bias, the white Gaussian stochastic bias and the bounded uncertainty bias.

1. Introduction

Because of the extensive applications of mobile robots in diverse fields, such as aerospace, intelligent industry and intelligent transportation, and so on, many efforts have been made on the studies of the robot. As an essential issue and a research hot topic in the robot field, the localization problem has attracted much research attention [1,2]. For robot localization problems, various types of external sensors, including inertial sensors [3], LIDAR [4,5], Doppler–azimuth radar [6] (D–AR) and ultrasonic sensors [7], have been used to obtain the measurements. Among them, the D–AR with the merits of lower cost, smaller size and lighter weight, has obtained initial attention in the robot localization problem. To the best of authors’ knowledge, only a few study results can be found in the existing literature, see, for example, [6,8,9,10,11]. For instance, in a given environment where the associations between landmarks and the D–AR are known, an extended Kalman filter (EKF) is employed to perform the robot localization in [6], where the D–AR is equipped on the robot platform to output the measurements containing the Doppler frequency shift and the azimuth. In Reference [11], regarding a specific case in which the associations between landmarks and the D–AR are unknown, a particle filtering-based localization scheme has been proposed.

It is worth mentioning that, for the kinematic model of a type (2,0) robot, both of the displacement and angular velocities are obtained by the odometer measurement [4]. The above-mentioned results all acquiesce in that the odometer measurement can be acquired precisely. Notwithstanding, in reality, owing to the aging of electronic components, it is unavoidable that the sensitivity of the odometer would be attenuated, which would result in an inaccurate measurement output of the odometer. In general, such inaccurate measurement is usually treated as a kind of inaccurate measurement-induced unknown bias, which can lead to modeling errors. If such a situation takes place, the kinematic model cannot accurately characterize the robot’s state, comprising the position and the orientation. If not taken into consideration adequately, a satisfactory localization performance cannot be achieved and may even cause the localization error divergence. Therefore, for the considered robot localization problem, it is necessary to take the inaccurate odometer measurement-induced unknown bias into consideration, which motivates the current study.

In the existing literature, one of the commonly used schemes is to introduce model uncertainties accounting for inaccurate measurement-induced unknown bias, see, for example [12,13]. For the sake of decreasing the effect of model uncertainties on the system performance, a number of approaches have been developed and, accordingly, some representative research results have been published for linear systems [14], nonlinear systems [15], master–slave systems [16], neutral systems [17], robotic manipulators [18], unmanned aerial vehicles [19] and marine vehicles [20]. It is noted that, among them, certain induced positive scalars have been presented in the process of dealing with uncertainties. By suitably adjusting these induced scalars, the desired system performance can be met. This feature means that the solution to deal with uncertainties is not suitable for on-line use. Owing to the fact that the robot system has a high realtime requirement and strong mobility, it is unrealistic to dynamically adjust the induced scalars to cut down the effect of uncertainties and to satisfy the realtime system performance.

For making up for the above-mentioned drawbacks, in the presence of unknown modeling errors, an adaptive fading KF (AFKF) for linear Gaussian systems has been proposed by introducing a forgetting factor, which can be adjusted adaptively, to suppress the influence of the uncertainties and a stronger tracking performance has also been achieved by comparison with the conventional KF in [21,22]. For nonlinear Gaussian systems, the corresponding AFEKF algorithm has been developed under the conventional EKF frame and some early research interests have been reported [23,24,25]. Moreover, a few practical engineering problems have been solved by utilizing such an adaptive algorithm [26,27]. For example, in Reference [26], a multiple fading factors-based adaptive $H_{\infty }$ KF algorithm has been proposed for the unmanned underwater vehicle navigation. In Reference [27], the speed and the load torque estimation problem has been investigated for induction motors by using the AFEKF. Until now, the localization problem of applying the AFEKF algorithm to constrain the impact of unknown modeling errors is still open, and this composes the principal focus of our present study.

Inspired by the above discussions, this paper aims to investigate the localization problem of the robot with unknown modeling errors by using the AFEKF algorithm. The main challenges we will encounter are identified as follows:

• How to characterize the inaccurate odometer measurement-induced unknown bias in the kinematic model of a type (2,0) robot?
• How to consider the inaccurate odometer measurement-induced unknown bias in the D–AR measurement model?
• How to developed a on-line localization algorithm such that the given localization performance can be achieved?

In this paper, we strive to meet the above-mentioned challenges and the main contributions are as follows:

• The inaccurate odometer measurement-induced unknown bias is considered in the kinematic model of a type (2,0) robot for the first time;
• The induced unknown bias is regarded in the D–AR measurement model;
• The AFEKF is adopted to reduce the impact of the modeling errors and achieving on-line localization;
• Thee comparative simulations have been conducted to testify the usefulness of the developed AFEKF by choosing three different types of modeling errors.

The remainder of this paper is arranged as follows. Section 2 states the considered robot localization problem. Section 3 shows the conventional EKF algorithm. In Section 4, the AFEKF-based localization algorithm is developed. Section 5 presents the stability analysis of the corresponding algorithm. The practicality of the provided AFEKF-based robot localization method is validated in Section 6 and some conclusions are drawn in Section 7.

2. Problem Formulation

In order to successfully carry out the subsequent analysis, the following assumptions are made.

Assumption A1.

The mobile robots considered in this paper are without the wheel skidding and slipping in modeling and simulation.

Assumption A2.

At each time instant, the Doppler–azimuth radar can collect all the measurements from the associated landmarks.

2.1. Conventional Robot Kinematic Model

In this paper, we consider a type (2,0) mobile robot, shown in Figure 1, whose kinematic model is described as follows [28]:

$$\left\{\begin{array}{c}\dot{x}=vcos\theta \hfill \\ \dot{y}=vsin\theta \hfill \\ \dot{\theta }=\omega ,\hfill \end{array}\right.$$

(1)

where $(x,y)$ is the position of the robot, $\theta$ denotes its orientation angle. The displacement velocity v and the angular velocity $\omega$ can be acquired via the odometer measurement. Then, by discretizing the continuous-time system (1), the following discrete-time system can be obtained:

$$\left\{\begin{array}{c}{x}_{k+1}=x_k+{\tau }_k{v}_{k}cos{\theta }_k\hfill \\ y_{k+1}=y_k+{\tau }_k{v}_{k}sin{\theta }_k\hfill \\ {\theta }_{k+1}={\theta }_k+{\tau }_k{\omega }_k,\hfill \end{array}\right.$$

(2)

where ${\tau }_k$ denotes the sampling interval between two successive time instants, k and $k+1$.

Denote robot’s state as $X_k={\left[\begin{array}{ccc}{x}_k& y_k& {\theta }_k\end{array}\right]}^T$ and treat $u_k={\left[\begin{array}{cc}{v}_k& {\omega }_k\end{array}\right]}^T$ as the input vector of the robot. Then, taking the system noise into consideration, system (2) can be transformed as:

$$X_{k+1}=f(X_k,u_k)+{\varphi }_k,$$

(3)

where $f(X_k,u_k)=X_k+h\left(X_k\right)u_k$, $h\left(X_k\right)=\left[\begin{array}{cc}{\tau }_{k}cos{\theta }_k& 0\\ {\tau }_{k}sin{\theta }_k& 0\\ 0& {\tau }_k\end{array}\right]$. Here, we assume that the system noise ${\varphi }_k$ is a white Gaussian stochastic sequence with covariance $R_k$.

By using the Taylor series to expand the nonlinear function $f(X_k,u_k)$ around the estimate ${\widehat{X}}_{k|k}$, (3) can be expanded as:

$$\begin{array}{c}\hfill X_{k+1}=f({\widehat{X}}_{k|k},u_k)+A_k(X_k-{\widehat{X}}_{k|k})+{\Delta }_k+{\varphi }_k\end{array}$$

(4)

where $A_k=\frac{\partial f(X_k,u_k)}{\partial X_k^T}{|}_{X_k={\widehat{X}}_{k|k}}=\left[\begin{array}{ccc}1& 0& -{\tau }_k{v}_{k}sin{\widehat{\theta }}_{k|k}\\ 0& 1& {\tau }_k{v}_{k}cos{\widehat{\theta }}_{k|k}\\ 0& 0& 1\end{array}\right]$, ${\Delta }_k$ denotes the higher order terms occurring in the expansion process.

2.2. Doppler-Azimuth Radar Measurement Model

We choose S known coordinates $M_l(x_{l,M},y_{l,M})$$(l=1,2,\dots ,S)$ as the positions of the landmarks. Each landmark can produce the Doppler frequency shift. The D–AR installed on the robot platform can collect its measurements. At time instant k, the produced Doppler frequency shift $d_{l,k}$ from the lth landmark and the azimuth angle ${\phi }_{l,k}$ are regarded as a set of measurements. In light of the definitions of the azimuth angle and the Doppler frequency shift in [6], the measurement model associated with the landmark l can be obtained as follows:

$$\begin{array}{c}\hfill z_{l,k}=g_{l,k}+{\delta }_{l,k},\end{array}$$

(5)

where $g_{l,k}=\left[\begin{array}{c}{d}_{l,k}\\ {\phi }_{l,k}\end{array}\right]=\left[\begin{array}{c}\frac{2f_c}{c}\frac{v_k\{(x_k-x_{l,M})cos{\theta }_k+(y_k-y_{l,M})sin{\theta }_k\}}{\sqrt{x_k-x_{l,M}{)}^2+{(y_k-y_{l,M})}^2}}\\ {\theta }_k-arctan\frac{y_{l,M}-y_k}{x_{l,M}-x_k}\end{array}\right]$, $f_c$ is the radar carrier frequency and c is the speed of light. Suppose that the measurement noise ${\delta }_{l,k}$ is a white Gaussian stochastic sequence with covariance $W_{l,k}$.

Taking the total S landmarks into account, the following overall measurement equation can be obtained:

$$\begin{array}{c}\hfill z_k=g_k+{\delta }_k,\end{array}$$

(6)

where $z_k={\left[\begin{array}{cccc}{z}_{1,k}^T& z_{2,k}^T& \cdots & z_{S,k}^T\end{array}\right]}^T$, $g_k={\left[\begin{array}{cccc}{g}_{1,k}^T& g_{2,k}^T& \cdots & g_{S,k}^T\end{array}\right]}^T$, and the augmented measurement noise ${\delta }_k={\left[\begin{array}{cccc}{\delta }_{1,k}^T& {\delta }_{2,k}^T& \cdots & {\delta }_{S,k}^T\end{array}\right]}^T$ is with zero mean and covariance $W_k=\mathrm{diag}\{W_{1,k},W_{2,k},\dots ,W_{S,k}\}$ where $\mathrm{diag}\{\cdots \}$ denotes a block-diagonal matrix.

By applying the Taylor series expansion again to expanding the nonlinear measurement model (6), one has:

$$\begin{array}{c}\hfill z_k={\widehat{g}}_{k|k-1}+C_k(X_k-{\widehat{X}}_{k|k-1})+{\Pi }_k+{\delta }_k,\end{array}$$

(7)

where

$$\begin{array}{cc}\hfill {\widehat{g}}_{k|k-1}& ={\left[\begin{array}{cccc}{g}_{1,k|k-1}^T& g_{2,k|k-1}^T& \cdots & g_{S,k|k-1}^T\end{array}\right]}^T{|}_{{\widehat{X}}_{k|k-1}}\hfill \\ \hfill & =\left[\begin{array}{c}\frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_{k|k-1}-x_{1,M})cos{\widehat{\theta }}_{k|k-1}+({\widehat{y}}_{k|k-1}-y_{1,M})sin{\widehat{\theta }}_{k|k-1}\}}{\sqrt{{\widehat{x}}_{k|k-1}-x_{1,M}{)}^2+{({\widehat{y}}_{k|k-1}-y_{1,M})}^2}}\\ {\widehat{\theta }}_{k|k-1}-arctan\frac{y_{1,M}-{\widehat{y}}_{k|k-1}}{x_{1,M}-{\widehat{x}}_{k|k-1}}\\ \frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_{k|k-1}-x_{2,M})cos{\widehat{\theta }}_{k|k-1}+({\widehat{y}}_{k|k-1}-y_{2,M})sin{\widehat{\theta }}_{k|k-1}\}}{\sqrt{{\widehat{x}}_{k|k-1}-x_{2,M}{)}^2+{({\widehat{y}}_{k|k-1}-y_{2,M})}^2}}\\ {\widehat{\theta }}_{k|k-1}-arctan\frac{y_{2,M}-{\widehat{y}}_{k|k-1}}{x_{2,M}-{\widehat{x}}_{k|k-1}}\\ ⋮\\ \frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_{k|k-1}-x_{S,M})cos{\widehat{\theta }}_{k|k-1}+({\widehat{y}}_{k|k-1}-y_{S,M})sin{\widehat{\theta }}_{k|k-1}\}}{\sqrt{{\widehat{x}}_{k|k-1}-x_{S,M}{)}^2+{({\widehat{y}}_{k|k-1}-y_{S,M})}^2}}\\ {\widehat{\theta }}_{k|k-1}-arctan\frac{y_{S,M}-{\widehat{y}}_{k|k-1}}{x_{S,M}-{\widehat{x}}_{k|k-1}}\end{array}\right]\hfill \\ \hfill C_k=& {\left[\begin{array}{cccc}{C}_{1,k}^T& C_{2,k}^T& \cdots & C_{S,k}^T\end{array}\right]}^T=\left[\begin{array}{ccc}\frac{\partial d_{1,k}}{\partial x_k}& \frac{\partial d_{1,k}}{\partial y_k}& \frac{\partial d_{1,k}}{\partial {\theta }_k}\\ \frac{\partial {\phi }_{1,k}}{\partial x_k}& \frac{\partial {\phi }_{1,k}}{\partial y_k}& \frac{\partial {\phi }_{1,k}}{\partial {\theta }_k}\\ \frac{\partial d_{2,k}}{\partial x_k}& \frac{\partial d_{2,k}}{\partial y_k}& \frac{\partial d_{2,k}}{\partial {\theta }_k}\\ \frac{\partial {\phi }_{2,k}}{\partial x_k}& \frac{\partial {\phi }_{2,k}}{\partial y_k}& \frac{\partial {\phi }_{2,k}}{\partial {\theta }_k}\\ ⋮& ⋮& ⋮\\ \frac{\partial d_{S,k}}{\partial x_k}& \frac{\partial d_{S,k}}{\partial y_k}& \frac{\partial d_{S,k}}{\partial {\theta }_k}\\ \frac{\partial {\phi }_{S,k}}{\partial x_k}& \frac{\partial {\phi }_{S,k}}{\partial y_k}& \frac{\partial {\phi }_{S,k}}{\partial {\theta }_k}\end{array}\right]{|}_{X_k={\widehat{X}}_{k|k-1}}\hfill \\ \hfill \frac{\partial d_{l,k}}{\partial x_k}=& \frac{2f_c}{c}\{\frac{v_{k}cos{\theta }_k}{\sqrt{{(x_k-x_{l,M})}^2+y_k-y_{l,M}{)}^2}}-\frac{v_k{(x_k-x_{l,M})}^{2}cos{\theta }_k}{{\left(\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\right)}^3}\hfill \\ \hfill & -\frac{v_k(y_k-y_{l,M})(x_k-x_{l,M})sin{\theta }_k}{{\left(\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\right)}^3}\}\hfill \\ \hfill \frac{\partial d_{l,k}}{\partial y_k}=& \frac{2f_c}{c}\{\frac{v_{k}sin{\theta }_k}{\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}}-\frac{v_k(x_k-x_{l,M})(y_k-y_{l,M})cos{\theta }_k)}{{\left(\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\right)}^3}\hfill \\ \hfill & -\frac{v_k{(y_k-y_{l,M})}^{2}sin{\theta }_k}{{\left(\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\right)}^3}\}\hfill \\ \hfill \frac{\partial d_{l,k}}{\partial {\theta }_k}=& \frac{-2f_c}{c}\frac{v_k\{(x_k-x_{l,M})sin{\theta }_k-(y_k-y_{l,M})cos{\theta }_k\}}{\sqrt{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}}\hfill \\ \hfill \frac{\partial {\phi }_{l,k}}{\partial x_k}=& \frac{-(y_{l,M}-y_k)}{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\hfill \\ \hfill \frac{\partial {\phi }_{l,k}}{\partial y_k}=& \frac{-(x_{l,M}-x_k)}{{(x_k-x_{l,M})}^2+{(y_k-y_{l,M})}^2}\hfill \\ \hfill \frac{\partial {\phi }_{l,k}}{\partial {\theta }_k}=& 1,\hfill \end{array}$$

${\Pi }_k$ denotes the higher order terms.

Remark 1.

From the robot system model (3) and the Doppler–azimuth radar’s measurement model (6), it can be discovered that the odometer measurements, $v_k$ and $w_k$, are not only the input state in (3) but are also the parameters involved in the measurement (6). Once the inaccurate odometer measurement-induced unknown modeling errors occur, both the system model and the measurement (6) would be affected. If the system and the measurement models, without considering the impact of the inaccurate odometer measurement-induced unknown bias, are directly used for localization, the localization performance will be attenuated.

This paper aims to develop a feasible scheme to locate the attitude of the robot in the presence of the unknown modeling errors and meanwhile guarantee the localization error exponentially bounded in mean square.

3. The Conventional EKF Algorithm

Under the frame of the conventional EKF, the robot kinematic model needs to be known exactly, and the higher order terms ${\Delta }_k$ and ${\Pi }_k$ in (4) and (7), respectively, are neglected. Consequently, the robot system and measurement models can be expressed as:

$$\begin{array}{cc}\hfill X_{k+1}=& f({\widehat{X}}_{k|k},u_k)+A_k(X_k-{\widehat{X}}_{k|k})+{\varphi }_k\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill z_k=& {\widehat{g}}_{k|k-1}+C_k(X_k-{\widehat{X}}_{k|k-1})+{\delta }_k.\hfill \end{array}$$

(9)

Then, the following conventional EKF algorithm can be given:

Prediction:

The one-step state prediction:

$$\begin{array}{c}\hfill {\widehat{X}}_{k+1|k}=f({\widehat{X}}_{k|k},u_k),\end{array}$$

(10)

the one-step predicted error covariance $P_{k+1|k}$ can be derived as:

$$\begin{array}{c}\hfill P_{k+1|k}=A_k{P}_{k|k}{A}_k^T+R_k.\end{array}$$

(11)

Then, by putting the one-step predicted state ${\widehat{X}}_{k+1|k}$ into the measurement model (9), the following measurement prediction can be obtained:

$$\begin{array}{c}\hfill {\widehat{z}}_{k+1}={\widehat{g}}_{k+1|k}.\end{array}$$

(12)

Update:

At sampling time instant $k+1$, the estimated robot’s state ${\widehat{X}}_{k+1|k+1}$ can be obtained via the following constructed filter:

$$\begin{array}{c}\hfill {\widehat{X}}_{k+1|k+1}={\widehat{X}}_{k+1|k}+K_{k+1}(z_{k+1}-{\widehat{g}}_{k+1|k}),\end{array}$$

(13)

where $K_{k+1}$ is the filter gain matrix, which can be derived according to:

$$\begin{array}{c}\hfill K_{k+1}=P_{k+1|k}{C}_{k+1}^T{[C_{k+1}{P}_{k+1|k}{C}_{k+1}^T+W_{k+1}]}^{-1}.\end{array}$$

(14)

The localization error covariance $P_{k+1|k+1}$ can be denoted as:

$$\begin{array}{c}\hfill P_{k+1|k+1}=[I-K_{k+1}{C}_{k+1}]P_{k+1|k}.\end{array}$$

(15)

Remark 2.

In the conventional EKF algorithm, the filtering performance highly depends on the system and the measurement models, which need to be known completely in advance. However, in practice, some unexpected factors may bring about modeling errors, such as the aging of electronic components, and so on. Moreover, [21] has indicated that the system model inaccuracy may deteriorate the filtering performance and even bring about the divergence of the filtering error. For the sake of working out this problem, the AFEKF method has been proposed to reduce the impact of incomplete modeling information and can maintain the system performance, which is suitable for the robot localization addressed in this paper.

4. Adaptive Fading EKF Algorithm

In this section, for making up the drawbacks of the conventional EKF algorithm, we employ the following AFEKF to cope with the system model’s inaccuracy induced by the inaccurate odometer measurement.

Prediction:

The one-step state prediction:

$$\begin{array}{c}\hfill {\widehat{X}}_{k+1|k}=f({\widehat{X}}_{k|k},u_k),\end{array}$$

(16)

obtain the innovation

$$\begin{array}{c}\hfill {\gamma }_{k+1}=z_{k+1}-{\widehat{g}}_{k+1|k},\end{array}$$

(17)

the one-step predicted error covariance $\overline{P_{k+1|k}}$ is

$$\begin{array}{c}\hfill \overline{P_{k+1|k}}={\alpha }_{k+1}{A}_k\overline{P_{k|k}}{A}_k^T+R_k,\end{array}$$

(18)

where ${\alpha }_{k+1}=max\{1,\mathrm{tr}\left({\Sigma }_{k+1}\right)/\mathrm{tr}\left(\overline{{\Sigma }_{k+1}}\right)\}$ is referred to as forgetting factor,

$$\begin{array}{c}\hfill {\Sigma }_{k+1}=\mathbb{E}\{{\gamma }_{k+1}{\gamma }_{k+1}^T\}-C_{k+1}{R}_k{C}_{k+1}^T-W_{k+1}\end{array}$$

(19)

and

$$\begin{array}{c}\hfill \overline{{\Sigma }_{k+1}}=C_{k+1}{A}_k\overline{P_{k|k}}{A}_k^T{C}_{k+1}^T.\end{array}$$

(20)

Measurement Update:

The filter gain matrix $\overline{K_{k+1}}$ can be computed according to:

$$\begin{array}{c}\hfill \overline{K_{k+1}}=\overline{P_{k+1|k}}{C}_{k+1}^T{(C_{k+1}\overline{P_{k+1|k}}{C}_{k+1}^T+W_{k+1})}^{-1},\end{array}$$

(21)

the estimated robot’s state ${\widehat{X}}_{k+1|k+1}$ can be obtained at sampling time instant $k+1$ via the following filter:

$$\begin{array}{c}\hfill {\widehat{X}}_{k+1|k+1}={\widehat{X}}_{k+1|k}+\overline{K_{k+1}}(z_{k+1}-{\widehat{g}}_{k+1|k}).\end{array}$$

(22)

The localization error covariance $\overline{P_{k+1|k+1}}$ can be updated by

$$\begin{array}{c}\hfill \overline{P_{k+1|k+1}}=[I-\overline{K_{k+1}}{C}_{k+1}]\overline{P_{k+1|k}}.\end{array}$$

(23)

Remark 3.

According to the AFEKF algorithm (16)–(23), the prominent difference between the conventional EKF and the AFEKF lies in a forgetting factor ${\alpha }_{k+1}$ being inserted into the Equation (18). Such an AFEKF algorithm has the following three features. First, the AFEKF has a unified filter frame and is effective for the filtering problem for nonlinear stochastic systems subject to inaccurate dynamics or measurement. Secondly, the acquisition of the forgetting factor needs a light computational complexity. Thirdly, this adaptive algorithm is suitable for applying to the realtime localization. In the next section, the stability will be analyzed.

5. Stability Analysis

In practical engineering, owing to the aging of electronic components, the odometer cannot always produce the exact measurements used for kinematic modeling. For characterizing such a case and for the convenience of analysis, we introduce an unknown vector $\Delta u_k={\left[\begin{array}{cc}\Delta v_k& \Delta {\omega }_k\end{array}\right]}^T$ to express the odometer measurements deviation. Then, the robot kinematic model (2) can be transformed as:

$$X_{k+1}=f(X_k,u_k)+\Delta f(X_k,\Delta u_k)+{\varphi }_k,$$

(24)

where $f(X_k,u_k)=X_k+h\left(X_k\right)u_k$, $\Delta f(X_k,\Delta u_k)=h\left(X_k\right)\Delta u_k$, $h\left(X_k\right)=\left[\begin{array}{cc}{\tau }_{k}cos{\theta }_k& 0\\ {\tau }_{k}sin{\theta }_k& 0\\ 0& {\tau }_k\end{array}\right]$.

For the convenience of the stability analysis of the developed AFEKF-based localization method, one-step formulation is considered in terms of an a priori variable. Therefore, by employing the Taylor series to expand the nonlinear function $f(X_k,u_k)$ around the estimate ${\widehat{X}}_k$, it has from (24) that:

$$\begin{array}{c}\hfill X_{k+1}=f({\widehat{X}}_k,u_k)+\Delta f(X_k,\Delta u_k)+A_k(X_k-{\widehat{X}}_k)+{\Delta }_k+{\varphi }_k\end{array}$$

(25)

where $A_k=\frac{\partial f(X_k,u_k)}{\partial X_k^T}{|}_{X_k={\widehat{X}}_k}=\left[\begin{array}{ccc}1& 0& -{\tau }_k{v}_{k}sin{\widehat{\theta }}_k\\ 0& 1& {\tau }_k{v}_{k}cos{\widehat{\theta }}_k\\ 0& 0& 1\end{array}\right]$; ${\Delta }_k$ expresses the higher order terms in the nonlinear function $f(X_k,u_k)$.

In the addressed robot localization problem, by taking the odometer measurement-induced unknown bias into account, the measurement model (6) can be written as:

$$\begin{array}{c}\hfill z_k=g_k+\Delta g_k+{\delta }_k,\end{array}$$

(26)

where

$$\begin{array}{c}\hfill \Delta g_k=\Delta v_k\left[\begin{array}{c}\frac{2f_c}{c}\frac{(x_k-x_{1,M})cos{\theta }_k+(y_k-y_{1,M})sin{\theta }_k}{\sqrt{x_k-x_{1,M}{)}^2+{(y_k-y_{1M})}^2}}\\ 0\\ \frac{2f_c}{c}\frac{(x_k-x_{2,M})cos{\theta }_k+(y_k-y_{2,M})sin{\theta }_k}{\sqrt{x_k-x_{2,M}{)}^2+{(y_k-y_{2,M})}^2}}\\ 0\\ ⋮\\ \frac{2f_c}{c}\frac{(x_k-x_{S,M})cos{\theta }_k+(y_k-y_{S,M})sin{\theta }_k}{\sqrt{x_k-x_{S,M}{)}^2+{(y_k-y_{S,M})}^2}}\\ 0\end{array}\right].\end{array}$$

The measurement model (6) can be expanded as follows:

$$\begin{array}{c}\hfill z_k={\widehat{g}}_k+\Delta g_k+C_k(X_k-{\widehat{X}}_k)+{\Pi }_k+{\delta }_k,\end{array}$$

(27)

where

$$\begin{array}{c}\hfill {\widehat{g}}_k=\left[\begin{array}{c}\frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_k-x_{1,M})cos{\widehat{\theta }}_k+({\widehat{y}}_k-y_{1,M})sin{\widehat{\theta }}_k\}}{\sqrt{{\widehat{x}}_k-x_{1,M}{)}^2+{({\widehat{y}}_k-y_{1,M})}^2}}\\ {\widehat{\theta }}_k-arctan\frac{y_{1,M}-{\widehat{y}}_k}{x_{1,M}-{\widehat{x}}_k}\\ \frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_k-x_{2,M})cos{\widehat{\theta }}_k+({\widehat{y}}_k-y_{2,M})sin{\widehat{\theta }}_k\}}{\sqrt{{\widehat{x}}_k-x_{2,M}{)}^2+{({\widehat{y}}_k-y_{2,M})}^2}}\\ {\widehat{\theta }}_k-arctan\frac{y_{2,M}-{\widehat{y}}_k}{x_{2,M}-{\widehat{x}}_k}\\ ⋮\\ \frac{2f_c}{c}\frac{v_k\{({\widehat{x}}_k-x_{S,M})cos{\widehat{\theta }}_k+({\widehat{y}}_k-y_{S,M})sin{\widehat{\theta }}_k\}}{\sqrt{{\widehat{x}}_k-x_{S,M}{)}^2+{({\widehat{y}}_k-y_{S,M})}^2}}\\ {\widehat{\theta }}_k-arctan\frac{y_{S,M}-{\widehat{y}}_k}{x_{S,M}-{\widehat{x}}_k}\end{array}\right],\end{array}$$

$C_k=\frac{\partial g_k}{\partial X_k^T}{|}_{X_k={\widehat{X}}_k}$, ${\Pi }_k$ denotes the higher order terms induced by the expansion of the nonlinear function $g_k$.

Definition 1.

An AFEKF filter is given as follows:

$$\begin{array}{cc}\hfill {\widehat{X}}_{k+1}=& f({\widehat{X}}_k,u_k)+\widehat{K_k}(z_k-{\widehat{g}}_k)\hfill \\ \hfill \widehat{P_{k+1}}=& {\alpha }_{k+1}(A_k\widehat{P_k}{A}_k^T-\widehat{K_k}{C}_k\widehat{P_k}{A}_k^T)+R_k\hfill \\ \hfill \widehat{K_k}=& A_k\widehat{P_k}{C}_k^T{(C_k\widehat{P_k}{C}_k^T+W_k)}^{-1}.\hfill \end{array}$$

(28)

In terms of Definition 1, letting the localization error be ${\rho }_k=X_k-{\widehat{X}}_k$, then, one has:

$$\begin{array}{cc}\hfill {\rho }_{k+1}=& X_{k+1}-{\widehat{X}}_{k+1}\hfill \\ \hfill =& \Delta f(X_k,\Delta u_k)+A_k(X_k-{\widehat{X}}_{k|k})+{\Delta }_k+{\varphi }_k\hfill \\ \hfill & -\widehat{K_k}(\Delta g_k+C_{k+1}(X_{k+1}-{\widehat{X}}_k)+{\Pi }_k+{\delta }_k)\hfill \\ \hfill =& (A_k-\widehat{K_k}{C}_k)(X_k-{\widehat{X}}_k)+o_k+{\varrho }_k+{\sigma }_k,\hfill \end{array}$$

(29)

where $o_k=\Delta f(X_k,\Delta u_k)-\widehat{K_k}\Delta g_k$, ${\varrho }_k={\Delta }_k-\widehat{K_k}{\Pi }_k$ and ${\sigma }_k={\varphi }_k-\widehat{K_k}{\delta }_k$.

Definition 2.

The localization error system (29) is exponentially bounded in the mean square sense, if there exist some positive scalars ζ, η and $0<\kappa <1$, for $k\ge 0$, such that:

$$\begin{array}{c}\hfill \mathbb{E}\{\parallel {\rho }_k{\parallel }^2\}\le \zeta \parallel {\rho }_0{\parallel }^2{\kappa }^k+\eta \end{array}$$

(30)

holds. The localization error system (29) is also said to be bounded with probability 1, if

$$\begin{array}{c}\hfill \underset{k\ge 0}{sup}\parallel {\rho }_k\parallel <\infty \end{array}$$

(31)

holds with probability 1.

From the expressions of $A_k$ and $C_k$, two scalars $\overline{a}$ and $\overline{c}$ can easily be found, such that $\parallel A_k\parallel \le \overline{a}$ and $\parallel C_k\parallel \le \overline{b}$. Moreover, we assume $\underline{p}{I}_{3\times 3}\le \widehat{P_k}\le \overline{p}{I}_{3\times 3}$, $W_k\ge \underline{w}{I}_{2S\times 2S}$ and $R_k\ge \underline{r}{I}_{3\times 3}$.

In (25) and (26), the uncertain terms $\Delta f(X_k,\Delta u_k)$ and $\Delta g_k$ induced by the odometer measurements are assumed to be bounded and fulfilled,

$$\begin{array}{c}\hfill \parallel \Delta f(X_k,\Delta u_k)\parallel \le \overline{f_{\Delta }},\parallel \Delta g_k\parallel \le \overline{g_{\Delta }},\end{array}$$

(32)

where $\overline{f_{\Delta }}$ and $\overline{g_{\Delta }}$ are two positive scalars.

We assume the high order terms ${\Delta }_k$ and ${\Pi }_k$ fulfilling:

$$\begin{array}{cc}\hfill \parallel {\Delta }_k\parallel & \le \overline{{\Delta }_f}{\parallel X_k-{\widehat{X}}_k\parallel }^2\le \overline{{\Delta }_f}{\overline{{\phi }_f}}^2\hfill \\ \hfill \parallel {\pi }_k\parallel & \le \overline{{\Pi }_g}{\parallel X_k-{\widehat{X}}_k\parallel }^2\le \overline{{\Pi }_g}{\overline{{\phi }_g}}^2,\hfill \end{array}$$

(33)

where $\overline{{\Delta }_f}$, $\overline{{\phi }_f}$, $\overline{{\Pi }_g}$ and $\overline{{\phi }_g}$ are four positive scalars.

Theorem 1.

Given the robot system (25), measurement (27) and the AFEKF algorithm shown in Definition 1, the localization error (29) is exponentially bounded in mean square and bounded with probability one under the following initial condition:

$$\begin{array}{c}\hfill \parallel {\rho }_0\parallel \le {\rho }^{*}\end{array}$$

(34)

and

$$\begin{array}{c}\hfill W_k\le \overline{w}{I}_{2S\times 2S},R_k\le \overline{r}{I}_{3\times 3},\end{array}$$

(35)

where ${\rho }^{*}>0$, ${\rho }^{*}=min(\overline{\phi },\frac{\chi }{2\overline{p}\overline{\kappa }})$, $\overline{\phi }=min(\overline{{\phi }_f},\overline{{\phi }_g})$, $\chi =\frac{\underline{w}}{[\overline{p}{(\overline{a}+\overline{k}\overline{c})}^2+\underline{w}]}$, $\overline{k}=\frac{\widehat{{\alpha }_k}\overline{\mathit{apc}}}{\widehat{{\beta }_k}\underline{r}}$, $\overline{\kappa }=\frac{{\overline{\kappa }}_{\Delta }}{\underline{p}}[2(\overline{a}+\overline{k}\overline{c})+{\overline{\kappa }}_{\Delta }\overline{\phi }]$, ${\overline{\kappa }}_{\Delta }=\overline{{\Delta }_f}+\overline{k}\overline{{\Pi }_g}$.

Proof. 

The proof of this theorem is similar to that in [23,29] and here omits it. □

Remark 4.

Until now, the localization problem of the robot with modeling errors has been solved by developing the AFEKF algorithm. Moreover, the stability has been discussed. In the next section, by considering three different types of modeling errors, three sets of comparison simulations are implemented to validate the effectiveness of the developed AFEKF-based localization scheme.

6. Simulation Results

In this section, compared with the conventional EKF, three sets of simulations are conducted to testify to the usefulness of the AFEKF-based localization scheme under three different types of modeling errors.

Set the sampling interval ${\tau }_k$ as ${\tau }_k=0.15$ s, and here we regard the sampling interval of the odometer and that of the Doppler Radar as the same. The displacement velocity $v_k$ is chosen as $v_k=0.1$ m/s and the angular velocity ${\omega }_k$ is set as ${\omega }_k=\frac{\pi }{18}$ rad/s, respectively. The system noise covariance $R_k$ is chosen as $R_k=0.005I$. Seven landmarks (i.e., $S=7$) are deployed in the simulation and the positions of the landmarks are $M_1(x_{1,M}=5\mathrm{m},y_{1,M}=0\mathrm{m})$, $M_2(x_{2,M}=-2\mathrm{m},y_{2,M}=-1\mathrm{m})$, $M_3(x_{3,M}=1\mathrm{m},y_{3,M}=1\mathrm{m})$, $M_4(x_{4,M}=3\mathrm{m},y_{4,M}=-1\mathrm{m})$, $M_5(x_{5,M}=0\mathrm{m},y_{5,M}=5\mathrm{m})$, $M_6(x_{6,M}=-1\mathrm{m},y_{6,M}=2\mathrm{m})$, and $M_7(x_{7,M}=5\mathrm{m},y_{7,M}=3\mathrm{m})$, respectively. The carrier frequency of the Doppler-a-zimuth radar is set as $f_c=1.0525\times {10}^6\mathrm{Hz}$ and the speed of light is set as $c=3\times {10}^8\mathrm{m}/\mathrm{s}$. The measurement noises covariance $W_k=\mathrm{diag}\{W_{1,k},W_{2,k},\dots ,W_{7,k}\}$ is set as $W_k=0.005\mathrm{diag}\{{\underbrace{I_{2\times 2},\dots ,I_{2\times 2}}}_7\}$. The initial state $X_0$ is a Gaussian stochastic variable with expectation ${\check{X}}_0=\mathbb{E}\{X_0\}={\left[\begin{array}{ccc}1& 1& 0.5\end{array}\right]}^T$ and covariance $\mathbb{E}\{(X_0-{\check{X}}_0){(X_0-{\check{X}}_0)}^T\}=0.001I_{3\times 3}$. The initial positive definite matrix $P_0$ is set as $P_0=0.001I$.

Based on the above parameter settings, under the following three different cases, the comparisons between the AFEKF-based algorithm and the conventional EKF method are made by implementing 500 simulation experiments.

For the first situation, the bias vector $\Delta u_k$ is chosen as a constant bias, that is, $\Delta u_k={\left[\begin{array}{cc}\Delta v_k& \Delta {\omega }_k\end{array}\right]}^T={\left[\begin{array}{cc}0.3& 0.1\end{array}\right]}^T$. In terms of the conventional EKF and the AFEKF algorithms, the simulation outcomes are depicted in Figure 2, Figure 3, Figure 4 and Figure 5, where Figure 2 displays the actual and estimated robot trajectories, Figure 3 depicts the actual and the estimated angles, and Figure 4 and Figure 5 depict the position and the angle errors, respectively.

For the second situation, we regard the bias vector $\Delta u_k$ as a set of white Gaussian stochastic vectors with variance $\mathbb{E}\{\Delta u_k\Delta u_k^T\}=0.3^2{I}_{2\times 2}$. The simulation results are listed in Figure 6, Figure 7, Figure 8 and Figure 9, where Figure 6 presents the actual and the estimated robot trajectories, the actual and the estimated angles are shown in Figure 7, and the position and the angle errors are depicted in Figure 8 and Figure 9, respectively.

For the third situation, the bias vector $\Delta u_k$ is set as an uncertainty satisfying $\Delta u_k\Delta u_k^T\le \mathrm{diag}\{0.3^2,0.1^2\}$. The simulation results are given in Figure 10, Figure 11, Figure 12 and Figure 13. The actual and the estimated trajectories are shown in Figure 10. Figure 11 depicts the actual angle and the estimated angle, and Figure 12 and Figure 13 depict the position and the angle errors, respectively.

From the simulation results, it can be seen that the developed AFEKF-based robot localization scheme presents a good localization performance. Specifically, the influence of the modeling errors on the localization performance can be resisted owing to the introduction of the forgetting factor. Moreover, the simulations show that the conventional EKF algorithm has an unsatisfied localization performance, which implies the conventional EKF has poor robustness against the modeling errors.

7. Conclusions

In this paper, in the featured environment with the known association between the sensor and the landmarks, we have studied the localization problem of a mobile robot by developing an AFEKF-based localization scheme. A type of odometer measurement-induced modeling error has been considered for this AFEKF-based robot localization problem. To mitigate the influence of the induced modeling errors and to enhance the robot localization, an AFEKF-based localization scheme has been developed. Three sets of comparative simulations have been conducted to testify to the feasibility of the developed AFEKF-based localization scheme under three sets of different biases, respectively.

In the future, we will solve the robot localization problem by considering simultaneously the energy constraint and outliers [30,31].