Localization in Structured Environments with UWB Devices without Acceleration Measurements, and Velocity Estimation Using a Kalman–Bucy Filter

Abstract

In this work, a novel scheme for velocity and position estimation in a UWB range-based localization system is proposed. The suggested estimation strategy allows to overcome two main problems typically encountered in the localization systems. The first one is that it can be suitable for use in environments where the GPS signal is not present or where it might fail. The second one is that no accelerometer measurements are needed for the localization task. Moreover, to deal with the velocity estimation problem, a suitable Kalman–Bucy filter is designed and it is compared, experimentally, with a particle filter by showing the features of the two algorithms in order to be used in a localization context. Additionally, further experimental tests are carried out on a suitable developed test setup in order to confirm the goodness of the proposed approach.

1. Introduction

Real-time localization of mobile objects and people has gained increasing interest and popularity because of the growing demand of location-based services such as in smart industries, warehouse management, healthcare industry, transportation and logistics, defense, mobile robot guidance, and autonomous drive and flight. For these reasons, the localization estimation problem is a challenge in the academic communities and industrial sectors, especially for those involved in autonomous flight applications design [1,2,3] where the controller, in order to implement real-time state-feedback control laws, has to know the position, linear and angular speed of the object, and its attitude. Since the position and velocity vectors, estimated by means of integrations of the accelerations measured by IMU, suffer uncertainties and disturbances introduced by the sensor itself, the aim of this paper is the estimation of the position of a moving point P and that of the velocity without acceleration measurements. In outdoor environments, where geostationary satellites are in LOS (line-of-sight) condition, the GPS signal is used to obtain information about position and velocity of a mobile object (see, for example, [4,5,6,7,8,9]), together with a data fusion system [10] which is able to handle the different nature and frequency of IMU measures and GPS signals. In other contexts, where the GPS signal becomes unreliable, for example, in indoor environments [11,12] or outdoor scenarios such as forests or urban canyons [13], it is necessary to use alternative sensor devices and methods to face the localization problem [14,15,16,17]. A common way to approach this kind of problem is to perform the navigation task in a structured environment where it is possible to take information about the distance between the moving object and some fixed reference points. This can be achieved, for example, by installing some UWB radio transceivers both on the mobile device and in correspondence of the fixed reference points, which are called anchors. The localization method is based on the measurement of the distances between the onboard module and the anchors, and makes use of a suitable algorithm to extract the position of the center of gravity of the moving object [18,19,20]. Starting from $n\ge 3$ distance measurements and applying a trilateration algorithm [21,22,23,24], it is possible to obtain an estimation of the position vector. With regards to the implementation of the trilateration algorithm, in order to calculate the best approximation of the mobile object position, some recursive procedures have been explored and proposed in the literature, generally based on linear and nonlinear least-square technique [25,26], or the use of Cayley–Menger determinant [27,28]. Other approaches based on Kalman filtering have also been investigated. For example, in [29], a Kalman filter based on data fusion algorithm was designed to improve the localization performance in an indoor scenario. In [30], a multi-sensor fusion algorithm for underwater vehicle localization was presented, making use of neural network and obtaining better navigation and localization results with respect to the classic implementation based on the error-state Kalman filter. In [31], a deep learning framework with multiple long short-term memory modules combined with an extended Kalman filter was proposed to predict the increment of the vehicle position in a localization task during GPS outages.

This paper represents an extension of the work presented in [32], where a localization scheme derived from the parallel robot context and based on the estimation of the direct kinematics is illustrated. In particular, assuming the gravity center of the mobile object as the end effector of a parallel robot and its distances from n anchors as the prismatic links, the estimation of the position vector is obtained by means of inversion of the known inverse kinematics. The distances from the anchors are measured at low frequency using the UWB radio transceivers DWM1000 integrated circuit (IC) from Decawave manufacturer [33]. The estimator consists of a closed-loop control scheme based on a PI action driven by the error between the measured distance vector and the estimated one, and affected by the distance vector derivative acting as a disturbance. Since this disturbance is calculated by means of IMU integration and a projection through the online-computed inverse kinematics, it is affected by errors due to drift caused by IMU biases. In order to cope with this effect, high values of the estimator gains have to be employed, as suggested in [34].

Since this solution appears to be unsatisfactory, in this paper, the estimator presented in [32] is modified, replacing the derivative of the distance vector with an estimation of this derivative carried out by means of an exact first-order Levant’s differentiator [35]. In this work, it is shown that it is possible to eliminate the accelerometer and all problems connected to it, such as systematic errors given by imprecise scaling factors, axes misalignments, non-uniform cross-axis sensitivities, non-zero biases, and temperature dependence. Moreover, from the theoretical point of view, it is possible to ensure the convergence to zero of the position estimation error in the case with three anchors, while if more than three anchors are employed, the convergence can be proved only locally. Finally, with the aim of also estimating the velocity of the mobile object, a Kalman–Bucy filter, driven by the estimated position, is introduced. It is important to highlight that the Kalman–Bucy filter input variables are computed by the proposed localization algorithm and, consequently, that they do not present drift problems caused by the double integration of the accelerometer measurement.

Experimental results show the effectiveness of the proposed localization scheme. In particular, it is shown that it is possible to approximate the first derivatives of the distances by using a suitable developed differentiator. This leads to a slightly noisier estimate, but allows to eliminate the accelerometer. Moreover, the Kalman–Bucy filter is compared with the particle filter, and it is shown that the best results are obtained using the differentiator and the Kalman–Bucy filter for velocity estimation, both in term of amplitude and variance of the estimation error.

The structure of the paper is as follows. In Section 2, the localization scheme for the position estimation without accelerometer measurements is illustrated by differentiating the case with three anchors (Section 2.3) and with more than three anchors (Section 2.4). The experimental results for this scheme are given in Section 2.5. In Section 3, we discuss three different schemes to estimate the velocity, also showing the experimental results and giving a comparison among them in Section 3.3. Finally, conclusions are given in Section 4. Additionally, the experimental setup implemented to prove the effectiveness of the proposed architectures is described in Appendix A.

2. Localization without Accelerometers

A trilateration algorithm, in both indoor and outdoor environments, allows to extract the information about the coordinates of a point P in a chosen reference frame by measuring the distances of P from some fixed devices whose coordinates $A_i,i=1,\cdots ,n$ are a priori known. Since the aim of this paper is to propose a method for the localization of a point of a mobile moving object in indoor environments, where the GPS signals do not arrive, the electronic devices chosen to perform range-based measurements are ultra-wideband radio transceivers (see Section 1). The proposed method is based on the kinematics inversion algorithm highlighted in the parallel robotics field. In this context, differently from the one for serial robots, the structure of the inverse kinematics in a closed form is known, so it is necessary to determine the direct kinematics. Due to the nonlinear nature of the problem, this computation does not exist in a closed form. As already stated, the principal objective is that of performing the localization algorithm without any knowledge of accelerometer signals.

The inverse kinematics structure describing the relationship between the range measures $q_i$ and the coordinates $\mathit{p}$ is given by the following equation:

$$\mathit{q}={\mathcal{K}}^{-1}\left(\mathit{p}\right)$$

(1)

where the generic component of ${\mathcal{K}}^{-1}\left(\mathit{p}\right)$, $q_i$, is given by:

$$q_i=\sqrt{{(x_i-p_x)}^2+{(y_i-p_y)}^2+{(z_i-p_z)}^2}\forall i\in S,$$

(2)

where $x_i$, $y_i$, and $z_i$ denote, respectively, the components of the position of the ith anchor $A_i$ referred to the reference frame; $p_x$, $p_y$, and $p_z$ denote the coordinates of the mobile point P; $q_i$ are the distance between P and the ith anchor; and $S=\{1,2,\cdots n\}$ denotes the discrete set of integer numbers representing the anchors. In the case under study, $n\ge 3$. In (1), the components of $\mathit{q}$ and $\mathit{p}$ represent, respectively, the prismatic link lengths and the coordinates of the reference point P of the parallel robot end-effector. If these coordinates ($p_x$, $p_y$, and $p_z$) are known, it is possible to obtain $\mathit{q}$ by means of (2). However, the aim of this paper is to estimate the components of P by measuring the distances between P itself and the $A_i,i=1,\cdots ,n$ anchors, and then the relationship to be used is the following one:

$$\mathit{p}=\mathcal{K}\left(\mathit{q}\right)$$

(3)

where $\mathcal{K}$ is not known and has to be determined.

In [22], it is shown that by using three anchors located on the ground plane, it is possible to obtain a closed-form solution of the direct kinematics problem with an uncertainty on the sign of the component along the z-axis. This uncertainty can be eliminated if the motion of the mobile object is restricted to one side of the plane on which the three anchors lie.

2.1. Determination of the Direct Kinematics for $n\ge 3$ Anchors

When more than three anchors are used, Equation (1) consists of n equations and three unknown quantities that are the coordinates of P. In this case, the problem is solved by means of the left pseudo-inverse algorithm. More precisely, deriving with respect to the time Equation (1), the following can be obtained:

$$\dot{\mathit{q}}={\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)\dot{\mathit{p}},$$

(4)

where ${\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)\in R^{n\times 3}$ is the Jacobian matrix of the vectorial function ${\mathcal{K}}^{-1}\left(\mathit{p}\right)$, given by:

$${\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)=\frac{\partial {\mathcal{K}}^{-1}\left(\mathit{p}\right)}{\partial \mathit{p}}.$$

(5)

In order to obtain $\dot{\mathit{p}}$ from (4), for $n=3$ it is sufficient to apply the relationship $\dot{\mathit{p}}={\mathit{J}}_{{\mathcal{K}}^{-1}}^{-1}\left(\mathit{p}\right)\dot{\mathit{q}}$ because the Jacobian matrix is invertible, whereas for $n>3$, it is common practice to minimize the 2-norm of the difference of the first and the second term of (4). This leads to an optimal solution ${\dot{\mathit{p}}}_{opt}$, given by:

$${\dot{\mathit{p}}}_{opt}=\underset{\dot{\mathit{p}}}{min}{∥{\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)\dot{\mathit{p}}-\dot{\mathit{q}}∥}_2^2={\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left({\mathit{p}}_{opt}\right)\dot{\mathit{q}},$$

(6)

where ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\mathit{p}\right)$ is the left pseudo-inverse matrix of ${\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)$, given by:

$${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\mathit{p}\right)={\left({\mathit{J}}_{{\mathcal{K}}^{-1}}^T\left(\mathit{p}\right){\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)\right)}^{-1}{\mathit{J}}_{{\mathcal{K}}^{-1}}^T\left(\mathit{p}\right),$$

(7)

which exists because the Jacobian matrix ${\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\mathit{p}\right)$ is full-rank column.

2.2. A New Closed-Loop Localization Algorithm

Equation (6) suggests to determine the position vector by means of the following iterative algorithm:

$$\dot{\widehat{\mathit{p}}}={\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\widehat{\mathit{p}}\right)\left(\dot{\widehat{\mathit{q}}}+{\mathit{K}}_{P}e+{\mathit{K}}_I{\int }_0^t\mathit{e}\left(\tau \right)d\tau \right).$$

(8)

where $\mathit{e}=(\mathit{q}-\widehat{\mathit{q}})$. The corresponding closed-loop block scheme of the localization method is given in Figure 1.

This algorithm differs from that shown in [32] for the presence of the derivative of the computed feedback distance vector, $\dot{\widehat{\mathit{q}}}$, instead of the derivative of the true distances, $\dot{\mathit{q}}$. While the computed feedback distance vector is a continuous function, and consequently numerically derivable, the measured distance vector is a vector whose elements are piecewise constant functions, and consequently cannot be numerically derivable. For this reason, the computation of $\dot{\mathit{q}}$ requires the use of accelerometers, as discussed in [32]. In order to compute a numeric derivative of $\widehat{\mathit{q}}$ it is possible to use the robust first-order Levant’s differentiator [35], given by:

$$\begin{array}{c}\hfill {\dot{\widehat{q}}}_i=u_i-\lambda \sqrt{\left|\right(x_i-q_i\left)\right|}\mathrm{sgn}(x_i-q_i),\end{array}$$

(9)

$$\begin{array}{c}\hfill {\dot{u}}_i=-\alpha \mathrm{sgn}(x_i-q_i),\\ \hfill i=1,\cdots ,n,\end{array}$$

(10)

where sgn denotes the signum function.

2.3. Using Three Anchors

When the localization is performed using three anchors, it is easy to prove the following theorem.

Theorem 1.

Given a mobile object, let the point P be the center of gravity. Assume that three anchors are employed for the localization of the mobile object, i.e., $\mathit{q}\in R^3$. Then (8) is an estimator of the coordinates of the point P, where ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\widehat{\mathit{p}}\right)={\mathit{J}}_{{\mathcal{K}}^{-1}}^{-1}\left(\widehat{\mathit{p}}\right)$$\mathit{e}\in R^3$ and ${\mathit{K}}_P,{\mathit{K}}_I\in R^{3\times 3}$, with ${\mathit{K}}_P=k_P{\mathit{I}}_3$ and ${\mathit{K}}_I=k_I{\mathit{I}}_3$, provided that $\frac{k_I}{k_P}>0$.

Proof. Since $\dot{\widehat{\mathit{q}}}={\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\dot{\widehat{\mathit{p}}}\right)$ (cf. (4)), (8) becomes:

$${\mathit{J}}_{{\mathcal{K}}^{-1}}^{-1}\left(\widehat{\mathit{p}}\right)\left({\mathit{K}}_{P}e+{\mathit{K}}_I{\int }_0^t\mathit{e}\left(\tau \right)d\tau \right)=0.$$

(11)

since ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{-1}\left(\widehat{\mathit{p}}\right)$ is a square full-rank matrix, (11) implies that:

$$k_P\mathit{e}+k_I{\int }_0^t\mathit{e}\left(\tau \right)d\tau =0.$$

(12)

It follows that $\mathit{e}\left(t\right)$ converges asymptotically to zero when t diverges to infinity, provided that $\frac{k_I}{k_P}>0$. This implies that $\widehat{\mathit{q}}$ converges to $\mathit{q}$ and, consequently, $\widehat{\mathit{p}}=\mathcal{K}\left(\widehat{\mathit{q}}\right)$ converges to $\mathit{p}=\mathcal{K}\left(\mathit{q}\right)$.

2.4. Using $n>3$ Anchors

When $n>3$ anchors are employed for the localization task, the left pseudo-inverse ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\widehat{\mathit{p}}\right)$ is a $3\times n$ matrix having rank three. In this case (8) does not imply the convergence of $\mathit{e}\left(t\right)$ to zero; in fact, since the n columns of ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\widehat{\mathit{p}}\right)$ are linearly dependent, Equation (8) can be also satisfied for $e_i$, $i=1,\dots ,n$ different from zero. However, since the elements of the the Jacobian matrix ${\mathit{J}}_{{\mathcal{K}}^{-1}}\left(\widehat{\mathit{p}}\right)$ are bounded, the elements of ${\mathit{J}}_{{\mathcal{K}}^{-1}}^{\#}\left(\widehat{\mathit{p}}\right)$ are bounded as well, and, consequently, ${\parallel \mathit{e}\parallel }_2$ is bounded. This implies that ${\parallel {\mathcal{K}}^{-1}\left(\widehat{\mathit{p}}\right)-{\mathcal{K}}^{-1}\left(\mathit{p}\right)\parallel }_2$ is bounded and, since ${\mathcal{K}}^{-1}\left(\mathit{p}\right)$ is a continuous function in $R^3$, $\parallel \widehat{\mathit{p}}{-\mathit{p}\parallel }_2$ is bounded.

2.5. Experimental Results Using Four Anchors

For the localization experimental tests, the setup described in the Appendix A was used. In this case, where only position estimation was addressed (no speed estimation), the results were compared with the previous results obtained with the algorithm proposed in [32]. In order to highlight the differences between the two estimators, the tests were named as shown in

Table 1. Experimental tests for the localization without speed estimation.

Test Name	Estimation Algorithm
Test 1	The one already proposed in [32].
Test 2	Localization without accelerometers as in Equation (8).

.

In particular, the reference motion of the point P, corresponding to the onboard mobile anchor, is that depicted in Figure 2. This motion occurs in a scenario in which four fixed anchors are placed in positions $A_1=(4.58,4.58,1.60)$, $A_2=(4.58,0,2.20)$, $A_3=(0,0,1)$, and $A_4=(0,4.58,2.80)$, referred to a conventional Cartesian reference frame. The distances $q_i,i=1,\dots ,4$ are measured at a frequency rate of 12 Hz from the onboard mobile anchor and these four fixed anchors $A_i,i=1,\dots ,4$.

These measured distances represent the components of the distances vector $\mathit{q}$ applied as input to the closed loop localization scheme of Figure 1. In this localization scheme, the measured distances $\mathit{q}$ are staircase waveforms, whereas the feedback distances $\widehat{\mathit{q}}$ are continuous, and then differentiable waveforms, which track, on average, the input distance vector components.

The parameters of the localization scheme are $k_P=10$ and $k_I=25$. The components of the closed-loop localization scheme output vector, i.e., the coordinates of the point P, are given in Figure 3 together with the components of the reference trajectory of P during the motion. In the same figure, the corresponding tracking errors for both Test 1 and Test 2 are given together with a zoom relative to the initial transient. For both Test 1 and Test 2, the measured distances are reported in Figure 4 in black color. Examination of these figures shows that the new closed loop localization scheme is able to estimate the components of the point P with an error that, after a transient, presents a maximum value of 15 cm. The estimated position error for the new algorithm Test 2, obtained without the use of acceleration measurement, appears to be a little bit noisier than the one obtained with the previous localization algorithm Test 1. The estimated distances $\widehat{\mathit{q}}$ for both tests are given in Figure 4. Note that, from Figure 4, it is evident that the two algorithms produce almost the same output and for this reason there is an overlap between the two waveforms (Test 1 and Test 2).

In

Table 2. Mean value and variance of the position estimation error in Test 1 and Test 2.

	Mean Value [m]	Variance [m${}^2$]
Test 1	0.045	0.00910
Test 2	0.053	0.01187

, the mean value and the variance of the norm of the position estimation error for both Test 1 and Test 2 are reported. Data in Table 1 highlight that mean and variance of the errors relative to Test 2 are greater than those in Test 1. This confirms that using the differentiator instead of the acceleration measurements leads to a small loss of performance which, in turn, is insignificant with respect to the advantage connected to the elimination of the accelerometer itself.

Remark 1.

Note that when the accelerometer is removed from the observer scheme, the algorithm workload is a little bit higher because of the additional computational power needed to implement the differentiator. However, the removal of the accelerometer gives some advantages. In fact, even if accelerometers are very common and not expensive, they are generally affected by systematic errors given by imprecise scaling factors, axes misalignments, non-uniform cross-axis sensitivities, non-zero biases, and temperature dependence that decrease accuracy in the measurements. For these reasons, a system that makes use of accelerometers needs to be initialized with a calibration procedure before use.

3. Velocity Estimation

In order to obtain information about the velocity of P, a Kalman–Bucy filter is designed with the aim of obtaining the state estimation of the model, i.e., the position and velocity of P.

The strap-down model is given by:

$$\dot{\mathit{p}}=\mathit{v},$$

(13)

$$\dot{\mathit{v}}=\mathit{a},$$

(14)

$$\mathit{y}=\mathit{p}$$

(15)

where $\mathit{a}$ is the acceleration vector, $\mathit{p}$ and $\mathit{v}$ are the position and velocity vectors, and $\mathit{y}$ is the output vector. The state space model corresponding to (13)–(15) is given by:

$$\dot{\mathit{x}}=\mathit{A}\mathit{x}+\mathit{B}\mathit{a},$$

(16)

$$\mathit{y}=\mathit{C}\mathit{x}.$$

(17)

where $\mathit{x}={\left[{\mathit{p}}^T{\mathit{v}}^T\right]}^T$ is the state vector, and the matrices appearing in (16)–(17) are given by:

$$\mathit{A}=\left[\begin{array}{cc}{\mathbf{0}}_3& {\mathit{I}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right],\mathit{B}=\left[\begin{array}{c}{\mathbf{0}}_3\\ {\mathit{I}}_3\end{array}\right],\mathit{C}=\left[\begin{array}{cc}{\mathit{I}}_3& {\mathbf{0}}_3\end{array}\right],$$

where ${\mathbf{0}}_3$ and ${\mathit{I}}_3$ are the null and identity $3\times 3$ matrices. It easy to verify that model (16)–(17) is observable and, consequently, it is possible to employ an observer to obtain the state variables. Due to the linearity of the model (16)–(17) and taking into account that the acceleration, measured by the accelerometers, is affected by a large amount of noise, it is convenient to design a continuous-time Kalman–Bucy filter (KBF) estimator. Moreover, a particle filter is considered for comparison reasons.

With regard to the velocity estimation, three different implementation schemes were considered, as reported in

Table 3. Experimental tests for the localization with speed estimation.

Test Name	Estimation Algorithm
Test 3	Differentiator and Kalman–Bucy filter (cf. Figure 5).
Test 4	Estimation of $\dot{\widehat{\mathit{q}}}$ by Kalman–Bucy filter (cf. Figure 6).
Test 5	Differentiator and particle filter (cf. Figure 7).

. In all the three schemes, it is assumed that the position estimation is that given by the localization scheme of Figure 1. In particular, in Figure 5, a Kalman–Bucy filter is added to the localization scheme of Figure 1 with the aim of obtaining the velocity estimation. In Figure 6, the estimated velocity is used for estimating $\dot{\widehat{\mathit{q}}}$ without the use of the differentiator. In Figure 7, a particle filter is used instead of the Kalman–Bucy filter together with the differentiator.

3.1. Kalman–Bucy Filter

In order to design the KBF corresponding to the acceleration model, it is necessary to associate with the deterministic model (16)–(17) a stochastic model which takes into account the noise generated by the accelerometer ${\mathit{w}}_a$ together with the true acceleration $\overline{\mathit{a}}$ and the measurement noise ${\mathit{w}}_p$ at the output of the localization scheme $\widehat{\mathit{p}}$. Then, the stochastic model of the accelerometer is given by:

$$\dot{\mathit{x}}=\mathit{A}\mathit{x}+\mathit{B}\overline{\mathit{a}}+\mathit{B}{\mathit{w}}_a,$$

(18)

$$\mathit{y}=\mathit{C}\mathit{x}+{\mathit{w}}_p,$$

(19)

where ${\mathit{w}}_a$ and ${\mathit{w}}_p$ are uncorrelated Gaussian white noises, $\overline{\mathit{a}}$ is the true acceleration, and $\mathit{y}$ is the output, having, respectively, covariance matrices $\mathit{Q}$ and $\mathit{R}$. The optimal KBF estimator is therefore given by:

$$\dot{\widehat{\mathit{x}}}=\mathit{A}\widehat{\mathit{x}}+\mathit{B}\overline{\mathit{a}}+{\mathit{K}}_F\left(t\right)[\mathit{y}-\mathit{C}\widehat{\mathit{x}}],$$

(20)

where ${\mathit{K}}_F\left(t\right)=\mathit{P}\left(t\right){\mathit{C}}^T{\mathit{R}}^{-1}$ and $\mathit{P}\left(t\right)$ is the covariance matrix of the estimation error given by $\mathit{P}\left(t\right)=E\{[\mathit{x}\left(t\right)-\widehat{\mathit{x}}\left(t\right)]{[\mathit{x}\left(t\right)-\widehat{\mathit{x}}\left(t\right)]}^T\}$, which is the solution of the following Riccati equation:

$$\dot{\mathit{P}}\left(t\right)=\mathit{A}\mathit{P}\left(t\right)+\mathit{P}\left(t\right){\mathit{A}}^T+\mathit{B}\mathit{Q}{\mathit{B}}^T-{\mathit{K}}_F\left(t\right)\mathit{R}{\mathit{K}}_F^T\left(t\right),$$

(21)

corresponding to $\mathit{P}\left(0\right)=E\{[\mathit{x}\left(0\right)-\widehat{\mathit{x}}\left(0\right)]{[\mathit{x}\left(0\right)-\widehat{\mathit{x}}\left(0\right)]}^T\}$.

To better clarify how the Kalman–Bucy filter is implemented for the velocity estimation, in Figure 8, the corresponding block diagram is reported.

In this way, the localization is performed without using the accelerometers, whereas the velocity estimation requires to employ the signals generated by the accelerometers and a Kalman–Bucy filter. Now, it is interesting to evaluate the use of the estimated velocity for determining the feedback variable $\dot{\widehat{\mathit{q}}}$, with the aim of eliminating the differentiator.

Observing (4), it is possible to obtain an estimation of $\dot{\widehat{\mathit{q}}}$ starting from the estimation of $\widehat{\mathit{v}}$ given by the Kalman–Bucy filter. Obviously, this approach should imply the use of the accelerometer signals also for the localization of the point P. It is assumed to again use the position estimated by means of the localization system for obtaining the feedback distance vector $\widehat{\mathit{q}}$.

3.2. Particle Filter as Alternative to the Kalman–Bucy Filter

In this subsection, the opportunity of using a particle filter instead of a Kalman–Bucy filter is evaluated for velocity estimation of the point P. The particle filter is tuned according to the discrete-time model associated with the strap-down model given by (13) and (14). This model is obtained by integrating Equation (14) and, successively, Equation (13) from $t_0$ to t, starting from the initial conditions $p\left(t_0\right)$ and $v\left(t_0\right)$, and assuming $p\left(t\right)=p\left(k{T}_s\right)$, $v\left(t\right)=v\left(k{T}_s\right)$ and ${\mathit{a}}_k=\mathit{a}\left(k{T}_s\right)$ for $t\in [k{T}_s,(k+1)T_s)$. Then, the discrete-time model is obtained, placing $t_0=k{T}_s$ and $t=(k+1)T_s$ where $k=1,2,\dots$. The model in question is represented as follows [24]:

$${\mathit{x}}_{k+1}={\mathit{A}}_d{\mathit{x}}_k+{\mathit{B}}_d{\overline{a}}_k+{\mathit{B}}_d{\mathit{w}}_{a,k},$$

(22)

$${\mathit{y}}_k=\mathit{C}{\mathit{x}}_k+{\mathit{w}}_{p,k},$$

(23)

where ${\mathit{a}}_k={\overline{\mathit{a}}}_k+{\mathit{w}}_{a,k}$, with ${\overline{\mathit{a}}}_k$ the true acceleration and ${\mathit{w}}_{a,k}$ the superimposed accelerometer noise, and

$${\mathit{A}}_d=\left[\begin{array}{cc}{\mathit{I}}_3& T_s{\mathit{I}}_3\\ {\mathbf{0}}_3& {\mathit{I}}_3\end{array}\right];{\mathit{B}}_d=\left[\begin{array}{c}0.5T_s^2{\mathit{I}}_3\\ T_s{\mathit{I}}_3\end{array}\right]$$

(24)

Placing $\mathbf{ϵ}={\mathbf{B}}_d{\mathbf{w}}_{a,k}$, the covariance matrix ${\mathit{Q}}_{ϵ}=E\left\{\mathbf{ϵ}{\mathbf{ϵ}}^T\right\}$ is given by:

$${\mathit{Q}}_{ϵ}=\left[\begin{array}{cc}\frac{1}{4}{T}_s^4& \frac{1}{2}{T}_s^3\\ \frac{1}{2}{T}_s^3& T_s^2\end{array}\right]\otimes {\mathit{\Sigma }}_3,$$

(25)

where ${\mathit{\Sigma }}_3=diag({\sigma }_x^2,{\sigma }_y^2,{\sigma }_z^2)$ and ${\sigma }_x$, ${\sigma }_y$, and ${\sigma }_z$ are standard deviations of the accelerometer noise along the axes (x, y, and z).

The particle filter considered here is shown in [36] (the one available in the MATLAB environment). It requires the deterministic discrete-time plant model (22)–(23) without noises ${\mathbf{w}}_{a,k}$ and ${\mathbf{w}}_{p,k}$, and with ${\mathbf{a}}_k$ instead of ${\overline{\mathbf{a}}}_k$, and the construction of two functions, the state transition and the likelihood function.

The state transition function computes a $6\times Np$ matrix where $Np$ is the particles number, and the jth column is the state value corresponding to the jth particle. The likelihood function computes the likelihood of each particle, starting from the output error given by the difference of the measured output and the output computed with the state associated to the particle itself. The output is then computed starting from the state of the particle having the maximum likelihood.

3.3. Experimental Results for the Velocity Estimation Schemes

To properly design the KBF it is necessary to obtain the covariance matrices $\mathit{Q}$ and $\mathit{R}$. To determine $\mathit{Q}$, the following procedure is performed. The three components of the acceleration vector, referred to in the conventional frame of Figure 2 and acquired during the experiment described in the previous section, are passed offline through three anticausal filters in order to split each acceleration component into the true acceleration and the noise signal superimposed to them. The anticausal filters are based on a continuous-time third-order Butterworth filter, having different bandwidths so that the mean value of the three noise signals is almost the same and is as low as possible. Then, the three matrix elements of $\mathit{Q}$, chosen diagonal, are computed as the covariances of the above noise signals. The covariance matrix, computed assuming the three elements of $\mathit{Q}$ equal to the maximum value of the covariance along the three axes, is given by $\mathit{Q}=diag(0.2244,0.2244,0.2244)$. The covariance matrix $\mathit{R}$ is chosen equal to ${\mathit{I}}_3$.

To design the particle filter, the distribution function is assumed Gaussian with a null mean and covariance ${\left[\begin{array}{cccccc}0.0001& 0.0001& 0.0001& 0.2244& 0.2244& 0.2244\end{array}\right]}^T$, and the particle number is assumed to be $N_p=10000$. The first three components of the covariance vector represent the covariances of the position component of P, whereas the remaining three components represent the covariances of the three acceleration components. The sample time is that corresponding to the frequency of 100 Hz, as in the previous experimental results.

In Figure 9, the estimated position, relative to Test 3, 4 and 5, is reported while the experimental results relative to the velocity estimation are given in Figure 10. Examination of this figure shows that the KBF is able to give an acceptable estimation of the velocity of point P in both cases of presence of the differentiator (cf. Figure 5), and direct feedback of the estimated velocity (cf. Figure 6). Instead, the particle filter is not able to estimate the velocity of P or considerably increase the number of particles. Examination of this figure shows that the effects of the velocity feedback on the localization are those of introducing noise in the estimated position of P together with a variation of the mean value of the estimation error and its variance, as is confirmed by

Table 4. Mean value and variance of the position estimation error in Test 3, Test 4, and Test 5.

	Error Mean Value [m]	Error Variance [m${}^2$]
Test 3	0.045	0.00911
Test 4	0.053	0.01187
Test 5	0.064	0.04008

. On the other end, even if the particle filter is able to reproduce the position with an acceptable mean error (greater than the one obtained using the KBF), the variance is four times higher.

Finally, in Figure 11, the norm of the distance estimation error is shown for Test 3 and 4. It appears that results from Test 4 present a better performance in terms of mean value and variance with respect to the one in Test 3.

In conclusion, from the experimental tests, it is possible to affirm that the best results are obtained using the differentiator and the Kalman–Bucy filter for velocity estimation, as shown in Figure 5, Test 3. In fact, this structure allows to obtain, especially in the position estimation task, a lower mean and variance for the position estimation error (see Table 4) while it behaves in a similar way to Test 4 and Test 5 for the velocity estimation (see

Table 5. Mean value and variance of the speed estimation error in Test 3, Test 4, and Test 5.

	Error Mean Value [m]	Error Variance [m${}^2$]
Test 3	0.125	0.04860
Test 4	0.125	0.04877
Test 5	0.198	0.20147

). Alternatively, it is possible to use the Kalman–Bucy filter alone, and feed back the velocity estimated by the filter, avoiding the use of the differentiator, accepting an increasing of the mean value and the variance of the position estimation error. The scheme with the particle filter, Test 5, is instead to be excluded because in both cases (position and speed estimation) it behaves worse than the other two solutions proposed in Test 3 and Test 4.

4. Conclusions

In this work, a novel observer for a range-based localization system is proposed for applications in environments where the GPS fails. Two aspects were developed. Firstly, a closed-loop localization scheme, which uses the measured distances as input variable, was designed. Secondly, a Kalman–Bucy filter was used to estimate the velocity components of P, and it was compared with the particle filter. Experimental results show the effectiveness of the proposed localization scheme. In particular, it was shown that it is possible to approximate the first derivatives of the distances by using a suitable developed differentiator. This leads to a slightly noisier estimate, but allows to eliminate the accelerometer and all problems connected to it. Moreover, the Kalman–Bucy filter results as the best-performing filter for estimating the velocity of point P, compared with the particle filter. Finally, it was shown that the velocity estimated by the Kalman–Bucy filter can be fed back to estimate the derivative of the distance vector, avoiding the use of a differentiator to implement the proposed localization algorithm.