Fuzzy-Logic-Based, Obstacle Information-Aided Multiple-Model Target Tracking

Abstract

Incorporating obstacle information into maneuvering target-tracking algorithms may lead to a better performance when the target when the target maneuver is caused by avoiding collision with obstacles. In this paper, we propose a fuzzy-logic-based method incorporating new obstacle information into the interacting multiple-model (IMM) algorithm (FOIA-MM). We use convex polygons to describe the obstacles and then extract the distance from and the field angle of these obstacle convex polygons to the predicted target position as obstacle information. This information is fed to two fuzzy logic inference systems; one system outputs the model weights to their probabilities, the other yields the expected sojourn time of the models for the transition probability matrix assignment. Finally, simulation experiments and an Unmanned Aerial Vehicle experiment are carried out to demonstrate the efficiency and effectiveness of the proposed algorithm.

1. Introduction

The Multiple-Model (MM) algorithm is an effective approach to maneuvering target tracking in many real-world applications [1], that works by regarding the maneuvering as the transition to motion modes and describes them by a finite number of kinematic models. The MM algorithm can be categorized into three generations: autonomous, cooperative, and variable-structured. For the first generation, all model filters in a fixed-structure model set work independently without interacting with each other. Its representative is the Static Multiple Model (SMM) algorithm pioneered by Magill [2]. The second generation reinitializes each filter with a weighted sum of the updated estimates from every model in the set and merges their results. Its popularization and further development have been spearheaded by the interacting MM (IMM) algorithm, which was proposed by Bar-Shalom and Blom [3]. The performance of the IMM algorithm suffers when there are too many motion models that overlap and compete with each other [4,5,6]. To solve this problem, X. Rong Li proposed the third-generation MM algorithm, i.e., the variable-structure multiple-model (VSMM) algorithm [7]. The model set within this model can be adaptively adjusted according to the changes in the motion mode of the target [8,9,10].

The target motion mode may change to avoid obstacles. For example, an automated guided vehicle (AGV) will change the way forward to bypass buildings and objects on the ground; a general aviation aircraft will change direction to avoid no-fly zones, bad meteorological zones along a route, etc. For the type of maneuvering that is performed in order to avoid all obstacles, existing research shows that a combination of the obstacle information and the MM algorithm can achieve a better tracking performance [11,12,13].

According to the description of the obstacles, incorporating the available obstacle information into the MM algorithms can be divided into two categories: implicit and explicit. In the implicit category, instead of obstacles, a permitted range of motion is given. For example, road information is used to adjust the model set in References [13,14,15], in which areas that are off of the road are considered to be obstacles. In addition to model set adjustment, a modification of the model probability (MP) and the model transition probability matrix (TPM) are presented in references [16] and [17], where guard conditions on waypoints that must be flown over, are used to alter the update of the MP and/or the TPM. In the explicit category, the range of obstacles is given directly. For example, circular obstacles are presented in the state-dependent variations of the interacting multiple-model (SD-IMM) algorithm by Rastgoufard [11]. The SD-IMM algorithm uses the distances between the target position and the obstacles as auxiliary information to modify the MP in the updating step and the TPM in the mixing step of the IMM algorithm. Its performance is always clearly better than that of the traditional version [12].

Using circles to describe obstacles is attractive because of its simplicity in computing, but it may mistake non-obstacle areas as obstacle areas. Furthermore, the SD-IMM algorithm gives the same adjustment value for the same distance between a target and obstacle circles with different radiuses. However, for obstacles with different sizes, evasive maneuvering should be different, since the target may require slighter maneuvers to bypass small obstacle circles than it does to bypass large obstacle circles.

To solve the problems mentioned above, we use polygons to describe the obstacles and introduce a new piece of obstacle information, i.e., the field angle. Fuzzy inference systems are applied to simplify the complex relationship between the obstacle information and the update of the MP and TPM; thus, a fuzzy-logic-based, obstacle information-aided multiple-model (FOIA-MM) algorithm is presented.

The rest of this paper is structured as follows: a brief introduction to the stochastic model and the improvement of the SD-IMM algorithm is presented in Section 2. In Section 3, the obstacle information description is given first, followed by the method to adjust MP and TPM, through their utilization. In Section 4, the performance of the proposed algorithm is compared with that of the SD-IMM algorithm by simulation and is illustrated by a UAV experiment, the results of which show that the proposed algorithm is effective and efficient. Finally, the conclusions are presented in Section 5.

2. Outline of the SD-IMM Algorithm

This section presents the stochastic model and the improvement of the SD-IMM algorithm.

2.1. Stochastic Model

In the SD-IMM algorithm, the maneuvering target is typically modeled through “hybrid systems”, which means the target state is a continuous process, while its motion modes are described as a finite model set. Assuming that there are r models matching the motion mode currently in effect, $M_k=\left\{m_k^1,m_k^2,m_k^3,\dots m_k^r\right\}$, the dynamics equation and the measurement equation are, respectively, defined by:

$${\mathit{x}}_{k+1}={\mathit{F}}_k^i({\mathit{m}}_k^i){\mathit{x}}_k+{\mathit{w}}_k^i({\mathit{m}}_k^i)$$

(1)

$${\mathit{z}}_k={\mathit{H}}_k^i({\mathit{m}}_k^i){\mathit{x}}_k+{\mathit{v}}_k^i({\mathit{m}}_k^i)$$

(2)

where $\mathit{x}$ denotes an n-dimensional state vector, and z denotes an m-dimensional measurement vector; ${\mathit{F}}_k^i(·)$ and ${\mathit{H}}_k^i(·)$ are the state transition function and the measurement function, respectively; $i\in \left\{1,2,\cdots ,r\right\}$, $m_k^i$ is the i-th adopted model at time k. The random variables ${\mathit{w}}_k^i$ and ${\mathit{v}}_k^i$ represent the process noise and the measurement noise, respectively. They are mutually independent zero-mean Gaussian white noise with covariance $\mathrm{cov}\left({\mathit{w}}_k^i\right)={\mathit{Q}}_k^i$ and $\mathrm{cov}\left({\mathit{v}}_k^i\right)={\mathit{R}}_k^i$.

The transition between different models is typically regarded to as Markov chain. The model transition probability $p_{ji}$ is given by:

$$p_{ji}=P(m_k^i|m_{k-1}^j)$$

(3)

where $m_{k-1}^j$ is the j-th adopted model at time k − 1.

2.2. Improvement in the SD-IMM Algorithm

The SD-IMM algorithm improves the IMM algorithm by applying the state-dependent obstacle information to adjust the MP and TPM. Suppose that there are P circular obstacles, each with the radius $r_l$ and the center $x_l,y_l,l\in \left\{1,2,\cdots ,P\right\}$; $d_l\left(\mathit{x}\right)$, the distance between the position of the state x and the i-th obstacle, can be calculated as:

$$d_l\left(\mathit{x}\right)=\sqrt{{(x-x_l)}^2+{(y-y_l)}^2}-r_l$$

(4)

where (x, y) is the position of the elements in x.

The influence level of the l-th obstacle circle on the state x is defined as:

$$S(\mathit{x},l)=\frac{1}{1+e^{-\beta (d_l(\mathit{x}))}}$$

(5)

where $\beta$ is the shape of the parameter. Thus, the obstacle information, $S\left(\mathit{x}\right)$, can be obtained by:

$$S(\mathit{x})=\underset{1\le l\le p}{\mathbf{min}}S(\mathit{x},l)$$

(6)

Unlike the traditional IMM, the SD-IMM algorithm adjusts the update of MP ${\mu }_k^i$ and TPM = [$p_{ji,k}]$ as follows:

$${\mu }_k^i=\frac{{\mu }_{k|k-1}^i{\mathsf{\Lambda }}_k^i{s}_k^i}{\sum _j{\mu }_{k|k-1}^j{\mathsf{\Lambda }}_k^j{s}_k^j},s_k^i=S({\hat{\mathit{x}}}_{k|k-1}^i)$$

(7)

$$p_{ji,k}=\frac{p_{ji,k-1}{s}_{k|k-1}^{ji}}{\sum _j{p}_{ji,k-1}{s}_{k|k-1}^{ji}},s_{k|k-1}^{ji}=S({\hat{\mathit{x}}}_{k|k-1}^{ji})$$

(8)

where ${\mathsf{\Lambda }}_k^i$ is the likelihood of model $m_k^i$; ${\widehat{\mathit{x}}}_{k|k-1}^i$ is the predicted state of model $m_k^i$; ${\widehat{\mathit{x}}}_{k|k-1}^{ji}$ is the updated state of model $m_k^i$. This adjustment incorporates the obstacle information into the IMM algorithm and thus makes the SD-IMM algorithm achieve a better tracking performance [12].

3. FOIA-MM Algorithm

Although the SD-IMM algorithm outperforms the traditional IMM algorithm in the obstacle scene, it has two main shortages: one is the mistake of viewing the non-obstacle areas as obstacle areas by using circles to describe them. As shown in Figure 1, the shaded parts are non-obstacle areas that are mistaken as obstacle areas. The crosses represent the true positions of the target, while the small circles show the estimates made by the SD-IMM algorithm. It shows that the SD-IMM algorithm makes several wrong estimates for taking a triangular obstacle as a circular obstacle. This shortage may lead to large errors in the estimation or to unnecessary maneuvers in the navigation. The other shortage is the insufficiency of utilizing only the distance between a target and obstacle circles in order to adjust the MP and TPM. Since a target’s evasive maneuvering for avoiding small obstacles must be slighter than that for large obstacles, the target needs a big turn rate to bypass a large circular obstacle and a little turn rate to bypass a small circular obstacle. As shown in Figure 2, Target (1) takes a greater turn rate than Target (2) ($\beta \left(1\right)>(\beta \left(2\right)$) to bypass obstacles of different sizes when they are at the same distance from the obstacles. The angles $\beta \left(1\right)$ and $\beta \left(2\right)$ are defined as the field angles, which are relative to Area (1) and Area (2).

To overcome the above two shortages, we applied polygons to describe the obstacles and used a new metric, the field angle (the angular extent of the obstacle viewing at target position), which was designed to reflect the size of the obstacles. Both the distance and the field angle were used to adjust the MP and the TPM in the proposed algorithm. For targets at the same distance to obstacles with different sizes, the bigger the field angle, the stronger the target maneuvers, and vice versa. It is hard to express this qualitative relationship mathematically, so fuzzy inference was employed. The fuzzy inference has been successfully applied to target tracking problems and has achieved good performance [18,19]. On the basis of the improvements mentioned above, we propose the fuzzy-logic-based obstacle information-aided multiple-model (FOIA-MM) algorithm.

Figure 3 shows the schematic block diagram of the proposed algorithm. First, the obstacle information, i.e., the distance $S_k$ and the field angle $B_k$, are calculated according to the predicted target position. Next, the obstacle information is fed into two fuzzy logic inference systems: one system outputs the model weights to their probabilities, the other yields the expected sojourn time of the models for the assignment of TPM. At last, the target state estimation is obtained through MM filtering. More details can be found in the following section.

3.1. Obstacle Information Descriptions

In practical applications, many obstacles have various shapes. However, in order to avoid a too large computational load, the FOIA-MM algorithm simplifies the shape of the obstacle into a polygonal shape. A concave polygon will produce abrupt changes in practical applications, which will greatly affect the tracking performance and even reduce the tracking performance. If it is a concave polygon, it must first be filled as a convex polygon. The convex polygon used in this algorithm is enough to cover the shape of obstacles. To simplify, for obstacles with a non-linear shape, this study used a convex polygon, which can best cover their shape. The FOIA-MM algorithm will declare all polygons as convex polygons.

Suppose there are P obstacle convex polygons in the motion scene. The predicted target position is ${\mathit{x}}_{k|k-1}$. The distance D(j) from ${\mathit{x}}_{k|k-1}$ to the j-th obstacle Area(j) is calculated as follows:

$$D(j)=\underset{\mathit{y}\in Area(j)}{\inf }\Vert {\mathit{x}}_{k|k-1}-\mathit{y}\Vert$$

(9)

where ‖ ‖ is the Euclidean norm. The field angle, $\beta \left(j\right)$, relative to D(j), is half the sum of the angular extent of all the sides of Area(j) viewing at ${\mathit{x}}_{k|k-1}$, $\beta \left(j\right)=\frac{1}{2}\sum _i{\beta }_{ji}$ (${\beta }_{ji}$ is the angular extent of the i-th side of Area(j)). The following rules can be obtained according to geometric knowledge. An example of the scene is shown in Figure 4. It has three obstacle areas; the dark point T is ${\mathit{x}}_{k|k-1}$. $\beta \left(3\right)$ is the biggest field angle between ${\mathit{x}}_{k|k-1}$ and the three obstacle areas.

$$\{\begin{array}{l}\beta (j)=\pi ,{\mathit{x}}_{k|k-1}\mathrm{is}\mathrm{inside}\mathrm{the}\mathrm{obstacle}\mathrm{area}\\ \beta (j)<\pi ,{\mathit{x}}_{k|k-1}\mathrm{is}\mathrm{outside}\mathrm{the}\mathrm{obtascle}\mathrm{area}\end{array}$$

(10)

As the target moves far away from the obstacle area, $\beta \left(j\right)$ approaches zero. Based on the distance D(j) and the field angle $\beta \left(j\right)$, the obstacle information $B_k$ and $S_k$ can be calculated as follows:

$$B_k=\beta (j*)S_k=D(j*)$$

(11)

where

$$j\ast =\arg \underset{1\le j\le P}{\min }D(j)$$

(12)

An example of the scene is shown in Figure 4. It has three obstacle areas; the dark point T is ${\mathit{x}}_{k|k-1}$. D(3) is the shortest distance between ${\mathit{x}}_{k|k-1}$ and three obstacle areas. Therefore, the obstacle information is $S_k$ = D(3) and $B_k$=$\beta \left(3\right)$.

3.2. MP Update

Suppose $J_k^i$ is the weight of model $m_k^i$, which reflects the influence of the obstacles on the MP. The weighted model probability of ${\mu }_k^i$ is [11]:

$${\mu }_k^i=\frac{1}{\mathrm{C}}{\mathsf{\Lambda }}_k^i{J}_k^i{\displaystyle \sum _{j=1}^r{p}_{ji}}{\mu }_{k-1}^j$$

(13)

where C is a normalizing factor, and ${\mu }_{k-1}^j$ is the model probability at the time k − 1.

The relationship between $J_k^i$ and the obstacle information ($S_k$, $B_k$) is not easy to quantify, so $J_k^i$ is acquired by using fuzzy inference in the proposed algorithm. The fuzzy inference system has two inputs, $S_k$ and $B_k$, and one output, $J_k^i$. The universe of discourse of $S_k$ is mapped into three fuzzy sets: near (NE), medium (ME), and far (FA), as shown in Figure 5. That of $B_k$ is also divided into three fuzzy sets: small positive (SP), medium positive ($\overline{\mathrm{MP}}$), and large positive (LP), as shown in Figure 6. The universe of discourse of $J_k^i$ is partitioned into five fuzzy sets, labeled in the linguistic terms of zero (ZE), low (LOW), medium (ME), high (HI), and very high (VH), as shown in Figure 7.

The fuzzy inference rules for $J_k^i$ are described as follows. When ${\mathit{x}}_{k|k-1}$ is near the obstacle areas, $S_k$ is small and $J_k^i$ should be small. When ${\mathit{x}}_{k|k-1}$ is not near the obstacle areas, if $S_k$ increases, $J_k^i$ should increase. If $B_k$ increases, which means that no matter how big an obstacle is, target tends to require higher maneuvers to avoid hitting the target, then $J_k^i$ should decrease.

3.3. TPM Update

The performance of the MM algorithm is related to the choice of the TPM [20], which is easily affected by the obstacle areas. Let ${\tau }_k^i$ be the expected sojourn time of the i-th model. The diagonal elements $p_{ii}$ of the TPM are given by [21]:

$$\begin{array}{l}{p}_{ii}=1-\frac{SI}{{\tau }_k^i}{\tau }_k^i\ge SI\\ i\in \left\{1,2,\cdots ,r\right\}\end{array}$$

(14)

where SI is the sampling interval. After $p_{ii}$ has been determined, the off-diagonal elements $p_{ji}$ of the TPM are given by:

$$\begin{array}{l}{p}_{ji}=\frac{1-p_{ii}}{r-1}\\ i,j\in \left\{1,2,\cdots ,r\right\}\end{array}$$

(15)

The expected sojourn time, ${\tau }_k^i$, of a model is the amount of time the model is expected not to change. It is apparently affected by obstacles or by $(S_k$, $B_k$). When ${\mathit{x}}_{k|k-1}$ is near the obstacle areas, $S_k$ is small, and ${\tau }_k^i$ should be small. When the target moves away from the obstacle areas, if $S_k$ increases, ${\tau }_k^i$ should also increase. If $B_k$ increases, which means that no matter how big an obstacle is, target tends to require higher maneuvers to avoid hitting the target, then ${\tau }_k^i$ should decrease.

According to the relationship between ${\tau }_k^i$ and ($S_k$, $B_k$) stated above, ${\tau }_k^i$ is acquired by using fuzzy inference in the proposed algorithm. For both $S_k$ and $B_k$, the universe of discourse is mapped into three fuzzy sets, as shown in Figure 5 and Figure 6, respectively. The acquired ${\tau }_k^i$ is partitioned into five fuzzy sets, labeled in the linguistic terms of very short (VS), short (SH), medium (ME), long (LO), and very long (VL), as shown in Figure 8.

3.4. The Iterative Process of the FOIA-MM Algorithm

The FOIA-MM algorithm combines the obstacle information into the MM algorithm. The iterative process of the FOIA-MM algorithm is given in

Table 1. The iterative process of the FOIA-MM algorithm.

1. Conditioned (re)initialization

Predicted MP: ${\mu }_{k|k-1}^i=p\left\{m_k^i\right|{\mathit{M}}_k,{\mathit{M}}_{k-1},{\mathit{Z}}^{k-1}\}={\displaystyle \sum p_{ji}}{\mu }_{k-1}^j$

Mixing MP: ${\mu }_{k-1}^{j|i}=p\left\{m_{k-1}^j\right|m_k^i,{\mathit{M}}_{k-1},{\mathit{{\rm Z}}}^{k-1}\}=p_{ji}{\mu }_{k-1}^j/{\mu }_{k|k-1}^i$

Mixing state: ${\overline{\mathit{x}}}_{k-1}^i=E[{\mathit{x}}_{k-1}|m_k^i,{\mathit{Z}}^{k-1}]={\displaystyle \sum {\widehat{\mathit{x}}}_{k-1|k-1}^j}{\mu }_{k-1}^{j|i}$

Mixing covariance: ${\overline{\mathit{P}}}_{k-1}^i=\sum \{{\mathit{P}}_{k-1|k-1}^j+[{\overline{\mathit{x}}}_{k-1}^i-{\widehat{\mathit{x}}}_{k-1|k-1}^j]{[{\overline{\mathit{x}}}_{k-1}^i-{\widehat{\mathit{x}}}_{k-1|k-1}^j]}^T\}{\mu }_{k-1}^{j|i}$

2. Kalman filtering

Predicted state: ${\widehat{\mathit{x}}}_{k|k-1}^i=E({\mathit{x}}_k|m_k^i,{\mathit{M}}_{k-1},{\mathit{Z}}^{k-1})={\mathit{F}}_{k-1}^i{\overline{\mathit{x}}}_{k-1}^i$

Predicted covariance: $P_{k|k-1}^i=F_{k-1}^i{\overline{\mathit{P}}}_{k-1}^i{(F_{k-1}^i)}^T+{\mathit{Q}}_{k-1}^i$

Measurement residual: ${\overline{\mathit{z}}}_k^i={\mathit{z}}_k-E({\mathit{z}}_k|m_k^i,{\mathit{M}}_{k-1},{\mathit{Z}}^{k-1})={\mathit{z}}_k-{\mathit{H}}_k^i{\widehat{\mathit{x}}}_{k|k-1}^i-{\overline{\mathit{v}}}_k^i$

Residual covariance: ${\mathit{S}}_k^i={\mathit{H}}_k^i{\mathit{P}}_{k|k-1}^i{({\mathit{H}}_k^i)}^T+{\mathit{R}}_k^i$

Filter gain: ${\mathit{K}}_k^i={\mathit{P}}_{k|k-1}^i{({\mathit{H}}_k^i)}^T{({\mathit{S}}_k^i)}^{-1}$

Updated state: ${\widehat{\mathit{x}}}_{k|k}^i=E({\mathit{x}}_k|m_k^i,{\mathit{M}}_{k-1},{\mathit{Z}}^{k-1})={\widehat{\mathit{x}}}_{k|k-1}^i+{\mathit{K}}_k^i{\overline{\mathit{z}}}_k^i$

Updated covariance: ${\mathit{P}}_{k|k}^i={\mathit{P}}_{k|k-1}^i-{\mathit{K}}_k^i{S}_k^i{({\mathit{K}}_k^i)}^T$

3. updated MP and TPM

Model likelihood: ${\mathsf{\Lambda }}_k^i=p({\overline{\mathit{z}}}_{}^i|m_k^i,{\mathit{M}}_{k-1},{\mathit{Z}}^{k-1})\stackrel{assume}{=}N[{\overline{\mathit{z}}}_k^i;0,{\mathit{S}}_k^i]$

Model weight: $B_k,S_k\stackrel{Fuzzy}{\to }{J}_k^i$

Updated MP: ${\mu }_k^i=\frac{1}{C}{\mathsf{\Lambda }}_k^i{J}_k^i{\displaystyle \sum _i^{}{p}_{ji}}{\mu }_{k-1}^j$

Expected sojourn time: $B_k,S_k\stackrel{Fuzzy}{\to }{\tau }_k^i$

Updated TPM: $p_{ii}=1-\frac{T}{{\tau }_k^i}{p}_{ij}=\frac{1-p_{ij}}{r-1}$

4. Combination

Overall state: ${\widehat{\mathit{x}}}_{k|k}=E({\mathit{x}}_k|{\mathit{M}}_k,{\mathit{M}}_{k-1},{\mathit{Z}}^k)={\displaystyle \sum {\widehat{\mathit{x}}}_{k|k}^i{\mu }_k^i}$

Overall covariance: ${\mathit{P}}_{k|k}={\sum }_{}[{\mathit{P}}_{k|k}^i+({\widehat{\mathit{x}}}_{k|k}-{\widehat{\mathit{x}}}_{k|k}^i){({\widehat{\mathit{x}}}_{k|k}-{\widehat{\mathit{x}}}_{k|k}^i)}^T]{\mu }_k^i$

. Filters in it are Kalman filters.

4. Experimental Results and Analysis

In this section, simulation experiments were carried out to compare the performances of the FOIA-MM algorithm and the SD-IMM algorithm. A UAV experiment was also carried out to demonstrate the effectiveness of the proposed algorithm in real applications.

4.1. Simulation Scenario

The position and velocity were chosen as state elements in the target-tracking simulation. That is:

$$\mathit{x}=[x,\dot{x},y,\dot{y}]$$

(16)

where $x,y$ are the target coordinates, and $\dot{x},\dot{y}$ are the velocities. The scenario was designed as shown in Figure 9. Three obstacles are labeled Area (1), Area (2), and Area (3). The thick black line indicates the boundary of the obstacle areas, while the thin black line shows the true target trajectory. Area (3) is relatively small with regard to Area (1) and Area (2). Position (1) and Position (2) have the same distance from the nearest obstacle Area (2) and Area (3). Position (1) has a larger field angle than Position (2), which leads to a larger turning maneuver to avoid hitting the obstacle. The initial state of the target is given by ${\mathit{x}}_{\mathbf{0}}$ = (180 m, 0 m/s, 49 m, 30 m/s). The target trajectory has nine phases, as shown in

Table 2. Motion models and durations of the target’s trajectory.

Model	CV	$\mathrm{CT}+7°/s$	CV	$\mathrm{CT}+7°/s$	$\mathrm{CT}-5°/s$
Time (s)	$1~22$	$23~27$	$28~46$	$47~60$	$61~67$
Model	$\mathrm{CT}+7°/s$	CV	$\mathrm{CT}+3°/s$	CV
Time (s)	$68~75$	$76~85$	$86~95$	$96~110$

. These three obstacles were simplified into six circular obstacle areas in the SD-IMM algorithm, as shown in Figure 10, each with radius $r_l$ and center $x_l,y_l,l\in \left\{1,2,\cdots ,6\right\}$. In this paper, the FOIA-MM algorithm simplified the obstacle into a convex polygon, and the SD-IMM algorithm simplified the obstacle into a circle. If an obstacle is covered with multiple circles, the number of circles needed is relatively large, which will greatly increase the computational complexity of the multiple-model algorithm with heavy computational load, and the real-time performance of the algorithm is difficult to be guaranteed. The target trajectory crossed over the circular obstacle areas several times. In the SD-IMM algorithm, when the target state estimation falls into the circular obstacle area, the weight of a model reaches the minimum.

The initial model set of the proposed algorithm and the SD-IMM algorithm consists of five models: a constant-velocity (CV) model and four coordinated-turn (CT) models with disparate turn rates: ${\mathsf{\omega }}_1=+3°/s,{\mathsf{\omega }}_2=+5°/s,{\mathsf{\omega }}_3=+7°/s,{\mathsf{\omega }}_4=-5°/s$. The initial values of MP were [0.2, 0.2, 0.2, 0.2, 0.2]. The dynamics matrix of the CT model is expressed by:

$$\begin{array}{c}{\mathit{F}}_k^i=\left[\begin{array}{cccc}1& \frac{\sin {\omega }_{i}T}{{\omega }_i}& 0& -\frac{1-\cos {\omega }_{i}T}{{\omega }_i}\\ 0& \cos {\omega }_{i}T& 0& -\sin {\omega }_{i}T\\ 0& \frac{1-\cos {\omega }_{i}T}{{\omega }_i}& 1& \frac{\sin {\omega }_{i}T}{{\omega }_i}\\ 0& \sin {\omega }_{i}T& 0& \cos {\omega }_{i}T\end{array}\right]\\ i=1,2,3,4\cdots \end{array}$$

(17)

The initial values of TPM were

$$\left[\begin{array}{ccccc}0.9& 0.025& 0.025& 0.025& 0.025\\ 0.05& 0.8& 0.05& 0.05& 0.05\\ 0.05& 0.05& 0.8& 0.05& 0.05\\ 0.05& 0.05& 0.05& 0.8& 0.05\\ 0.05& 0.05& 0.05& 0.05& 0.8\end{array}\right]$$

(18)

The measurements included the zero-mean Gaussian noises with standard deviations of ${\mathsf{\sigma }}_1=30m$ and ${\mathsf{\sigma }}_2=40m$ in the simulation trajectory.

Table 3. Fuzzy rules for $S_k$, $B_k$ and $J_k^i$.

${\mathit{J}}_{\mathit{k}}^{\mathit{i}}$	${\mathit{S}}_{\mathit{k}}$
${\mathit{B}}_{\mathit{k}}$		VN	ME	FA
	ZE	ZE	LOW	HI
	$\overline{\mathrm{MP}}$	ZE	ME	HI
	LP	ZE	LOW	LOW

and

Table 4. Fuzzy rules for $S_k$, $B_k$ and ${\tau }_k^i$.

${\mathit{\tau }}_{\mathit{k}}^{\mathit{i}}$	${\mathit{S}}_{\mathit{k}}$
${\mathit{B}}_{\mathit{k}}$		VN	ME	FA
	ZE	VS	ME	VL
	$\overline{\mathrm{MP}}$	VS	LO	VL
	LP	VS	VS	SH

are the fuzzy rules used in the simulation.

4.2. Performance Comparison

To evaluate the performance of the FOIA-MM algorithm, the root-mean-square error (RMSE) in the position and velocity was used. The results are the average of 50 Monte Carlo simulations. Figure 11 and Figure 12 show the RMSE in position and velocity estimation, with measurement noise standard deviations of ${\mathsf{\sigma }}_1=30$ and ${\mathsf{\sigma }}_2=40m$, respectively. Meanwhile, Figure 11 and Figure 12 also show the distribution of the distance between the target’s true positions and the obstacle areas. They demonstrate that most RMSE of the estimation of the SD-IMM algorithm were larger than those of the proposed algorithm, especially when the target was close to the edge of the obstacle areas. When the measurement noise level was bigger, the performance of the proposed algorithm was better than that of the SD-IMM algorithm. The MP at Position (1) and Position (2) are shown in

Table 5. Model probabilities of Position (1) and Position (2).

	Model	CV	$CT+3°/s$	$CT+5°/s$	$CT-5°/s$	$\mathit{C}\mathit{T}\mathbf{+}\mathbf{7}\mathbf{°}\mathbf{/}\mathit{s}$
Position (1)	FOIA-MM	0.09	0.13	0.11	0.04	0.63
	SD-IMM	0.14	0.16	0.20	0.14	0.36
	Model	CV	$\mathit{C}\mathit{T}\mathbf{+}\mathit{3}\mathbf{°}\mathbf{/}\mathit{s}$	$CT+5°/s$	$CT-5°/s$	$CT+7°/s$
Position (2)	FOIA-MM	0.15	0.60	0.16	0.05	0.04
	SD-IMM	0.21	0.25	0.24	0.10	0.20

The dark columns are real-model and the estimated-model probabilities by FOIA-MM and SD-IM. The table shows that FOIA-MM gave higher correct model probabilities than SD-IMM.

. The real-motion model at Position (1) is $CT+7°/s$, and that at Position (2) is $CT+3°/s$. As shown in Table 5, the FOIA-MM algorithm gave a more correct probability of the real models than the SD-IMM algorithm. This validated the effect of the field angle. In addition, as shown in

Table 6. Number of positions located in the obstacle areas.

	Measured	Estimated	Improvement
FOIA-MM	655	5	99.24%
SD-IMM	655	490	25.2%

, the proposed algorithm decreased the extent of the wrong estimation compared with the SD-IMM algorithm through statistical analysis. This result is the sum of 50 tests.

Computational complexity is an important index that measures whether an algorithm has real-time processing capability, which is especially important in tracking systems with high real-time requirements. In the field of tracking, many algorithms with high tracking accuracy are limited because of their high computational complexity. In this paper, the parameters of the simulation platform are: CPU Intel i3 2.0 GHz, 4 GB of memory, and operating environment MATLAB R2009b, which can perform 100 Monte Carlo simulations. The average CPU running time of FOIA-MM algorithm and SD-IMM algorithm was 0.234 s and 0.181 s, respectively. Because fuzzy inference requires a certain amount of time, the FOIA-MM algorithm ran longer than the SD-IMM algorithm.

4.3. Field Experiment and Results Analysis

In this experiment, the performance of the FOIA-MM algorithm was verified for tracking a UAV in a real scenario, as shown in Figure 13. The thick black line indicates the boundary of two high-rise buildings. The black asterisks indicate measurements collected by the GPS sensor during the flight of the UAV. The thin black line shows the FOIA-MM algorithm filtering results. About 10% measurements fell in the obstacle areas due to the GPS system measurement error. They were wrong measurements. Figure 13 shows that the FOIA-MM algorithm could successfully track the UAV and filter out all the wrong measurements.

5. Conclusions

In this paper, we presented the FOIA-MM algorithm for incorporating the obstacle information to the multiple model algorithm. We used convex polygons to describe obstacles to avoid mistaking non-obstacle areas as obstacle areas. For obstacles with different sizes, evasive maneuvering is different. We introduced a new term, the field angle, in the obstacle information, which can be considered as a relative distance between the target and the obstacles. The proposed algorithm regards the distance and the field angle as the obstacle information and analyses this relationship by mapping them to a set of fuzzy rules, which will modify the TPM and the MP for the state estimate. The effectiveness is evaluated by simulation experiments and a UAV experiment. These experiments have shown that the proposed algorithm has a good performance when compared to the SD-IMM algorithm. The fuzzy rules used in this paper can improve the performance currently achieved through their adjustment. Fuzzy rules are difficult to adjust, and erroneous rules will hamper improving the tracking performance. How to determine appropriate fuzzy inference rules is a critical problem for future works. The proposed algorithm can be extended to solve robot localization problems, drone tracking and localization, and other practical application problems.