Data-Driven Kalman Consensus Filtering for Connected Vehicle Speed Estimation in a Multi-Sensor Network

Abstract

The autonomous traffic system has imposed higher requirements on the speed estimation of connected vehicles, where the speed of connected vehicles, as one of the control conditions for refined traffic management, plays a crucial role in the evaluation and optimization of network performance. In this paper, we propose a multi-source speed measurement sensor network consensus filtering (MSCF) algorithm based on information weight for the problem of optimal speed consistency estimation for connected vehicles. Specifically, we first utilize dynamic linearization techniques and data-driven parameter identification algorithms to handle the derived state equations of connected vehicles. We then establish observation models for four different types of sensors and construct distributed direct and indirect measurement models by dynamically adjusting the information weights of sensor nodes. Following this, we combine the Kalman consistency filtering algorithm to derive the speed state estimation update rate and design a new state estimator to achieve the optimal consistent convergence estimation for connected vehicles’ speed. The MSCF algorithm can solve the problem of consistency filtering for noisy sensor data under observation- and communication-constrained conditions, enabling each sensor node to obtain a consistent convergence estimation value for the speed of the connected vehicle. The convergence of the algorithm is proved using the Lyapunov function. Through numerical simulation, the results are verified, indicating that compared to existing methods, this method can achieve a higher precision speed estimation effect.

1. Introduction

In the epoch of connected intelligent transportation, a higher degree of precision in estimating vehicular speed is becoming increasingly critical to accommodate the necessities posed by the evolution of autonomous transportation [1]. Intelligent connected vehicles necessitate paramount safety, of which an accurate speed estimation serves as an integral part. This precise estimation aids the vehicular system to adeptly respond to emergencies, ensuring prompt braking, evasion, and similar maneuvers, thus safeguarding the security of both drivers and pedestrians [2,3]. The autonomous driving function of intelligent connected vehicles demands high-precision speed estimation, facilitating superior control over the vehicle’s speed and direction, thereby actualizing the principles of safety, efficiency, and precision intrinsic to autonomous driving [4]. Furthermore, a high-precision estimation of vehicular speed augments the tracking of the vehicle’s location and driving direction, thereby enhancing the navigational accuracy of the connected vehicle. This improvement assists the vehicle system in better controlling energy consumption, thereby enhancing the vehicle’s energy efficiency [5,6].

With the development of measurement and manufacturing technologies, it is difficult for a single sensor to estimate the target state to meet the needs of future vehicle networking and autonomous transportation. The technique of multi-sensor information fusion can augment the perception of environmental variations in the vehicle speed estimation system, providing more accurate cognizance, and further facilitating optimal estimation and decision-making. This technique has already found extensive application in fields such as SLAM [7], industrial processing [8], and biomedical science [9], and it also holds considerable potential in vehicle speed estimation and positioning. The consistent coordination algorithm for observing target nodes in a sensor network composed of multiple sensors has achieved good results. Consistency coordination is an important concept in distributed computing, used to achieve a consistent view or state among nodes in a distributed network, even in the event of some node failures. Consistency coordination algorithms are widely used in UAV coordination control, vehicle formation control, distributed power grid, satellite attitude control, etc. [10,11,12,13]. Through multi-sensor information fusion technology, a vehicle speed estimation system can simultaneously exploit data from a variety of sensors, such as the Global Navigation Satellite Sensor (GNSS), inertial sensors, visual sensors, and LiDAR (Light Laser Detection and Ranging) [14]. These data can be collaboratively merged, reducing overall uncertainty, and enhancing the precision of speed estimation. Moreover, this technology can rectify errors inherent in the sensors, thereby further bolstering the system’s reliability and stability [15,16,17,18].

In the field of system state estimation, Kalman filtering is one of the most widely applied methods. Kalman filtering is an efficient recursive filtering algorithm capable of estimating the state of a dynamic system from a series of incomplete and noisy measurements [19]. For a dynamic linear system, Kalman filtering uses a prediction model to anticipate its next state, and based on measurement data, this prediction is corrected, yielding a more accurate estimation of the system state. For systems with non-linear characteristics or non-Gaussian noise, numerous researchers have developed extensions to the Kalman filter, including the Extended Kalman Filter (EKF) [20,21,22] and the Unscented Kalman Filter (UKF) [23], building upon its foundational methodology. A probability-weighted fusion method based on the Extended Kalman Filter was proposed in reference [24]. However, due to the higher-order stage error introduced by Taylor linear approximation, its estimation results might not be optimal and could even diverge [25]. Reference [26] put forward an Unscented Kalman Filter algorithm to estimate the speed of dynamic targets. This method can avoid the high-order terms in the linearization process, but it entails a substantial computational load. It is noteworthy that scholars have also proposed the Interacting Multiple Model (IMM) algorithm [27]. The IMM constructs multiple models of a target, synthesizing information from different models via the update of probability weights. It can effectively enhance the tracking performance [28], but it increases the computational overhead. Moreover, its performance also depends on whether the selected collection of models can adequately describe all possible motions of the target [29,30,31]. In addition, many existing fusion algorithms require a fusion center to obtain dynamic estimations of the target. Therefore, in recent years, scholars have also proposed Kalman consistency filtering methods such as the Distributed Extended Kalman Consensus Filter (DEKCF) [32] and Distributed Unscented Consensus Kalman Filter (DUKCF) [33,34] algorithms. These filters accomplish distributed target dynamic estimation through local information fusion among adjacent sensors. Combining the Kalman filter algorithm with the consistency coordination algorithm, the distributed communication interaction of the information flow improves the estimation accuracy of the entire sensor network for the target and improves the robustness of the system [35,36]. However, these consistency filters require precise modeling of the system, making the estimation process relatively complex and computationally expensive.

Furthermore, particle filters are another class of system state estimation methods that merit attention. They are a type of non-parametric and non-linear Bayesian filter, applicable to any type of system (linear or non-linear, Gaussian or non-Gaussian). Particle filters use a set of random samples (particles) to represent the state space of the system and drive the evolution of these particles in prediction and update steps. However, when dealing with high-dimensional systems, using a large number of particles to accurately represent the state space could lead to substantial computational costs [37,38,39].

Apart from model-based methods, deep neural networks have also achieved satisfactory results in target tracking and estimation. Target tracking can provide the preconditions for speed estimation and can be transformed into a category of state estimation problems. Reference [40] proposes a method based on Recurrent Neural Networks (RNNs) to handle the uncertainty of target movement. It can still guarantee a good tracking performance when sudden changes occur in the dynamic behavior of the target.

In summary, to satisfy the requirements of proactive safety control functions of vehicles, it is essential to enhance the precision of state observation under the strong non-linear characteristics of vehicles. Multi-sensor information fusion technology can augment the accuracy and reliability of a vehicle speed estimation system, holding significant value in applications related to autonomous driving and safe travel. However, data-driven consensus filtering is seldom applied to the speed estimation of connected vehicles.

This study focuses on a non-linear, discrete connected vehicle motion system. It selects an information flow topology sensor network composed of multiple types of sensors and proposes a distributed fusion algorithm based on information weight. This realizes that each sensor node can obtain a true estimation of the speed of the connected vehicles under observation and communication constraints. The main contributions of this paper are as follows:

(1)

By introducing the Full Form Dynamic Linearization (FFDL) dynamic linearization technique, the non-linear, discrete connected vehicle motion model is transformed into a quasi-linear state space model.

(2)

A multi-source speed measurement sensor network consensus filtering algorithm (MSCF) is proposed. This algorithm enables each sensor to make an optimal estimate of the speed of connected vehicles, which consistently converges to the true target value.

(3)

Based on Lyapunov techniques, a convergence analysis of the MSCF algorithm was conducted, providing a rigorous mathematical proof.

(4)

Through numerical simulation, the MSCF algorithm proposed in this paper shows superior estimation accuracy for the speed of connected vehicles compared to the DEKCF and DUKCF algorithms. This remains true in the presence of process noise, measurement noise, and communication noise.

The structure of this paper is as follows: In Section 2, the dynamic model of connected vehicles is established. In Section 3, the multi-source sensor speed measurement network and the MSCF algorithm are constructed. Section 4 provides the convergence analysis of the state estimators. Section 5 verifies the effectiveness of the proposed method through simulations. Section 6 puts forth the conclusions and future work.

2. Vehicle Dynamics Modeling

2.1. Discrete-Time Dynamics Modeling

Vehicular motion, a highly intricate process involving multiple degrees of freedom, embodies both complexity and uncertainty. A connected vehicle, during its travel, can be viewed as a rigid single particle, simplified by the vehicle longitudinal dynamics model in reference [41], and its dynamics can be expressed as:

$$\{\begin{array}{l}M\cdot \frac{dv(t)}{dt}=F(t)-f_B(t)-f_O(t,v)\\ f_O(t,v)=f_{m,s}(t)+f_{m,a}(t)+f_s(v)\\ f_s(v)=\frac{1}{2}\rho (t)d(t)A{v}^2(t)+\mu (t)v(t)\end{array}$$

(1)

Here, $t$ denotes the continuous time index, $v(t)$ is the vehicle’s velocity (m/s), $M$ is the vehicle’s mass (kg), $a(t)$ is the vehicle’s acceleration (m/s²), $F(t)$ is the tractive force provided by the vehicle engine (N), $f_B(t)$ is the resistance due to the vehicle’s braking system or engine brake, $f_O(t,v)$ is the total resistance the vehicle experiences (N), $f_m(t)$ is the resistance encountered by the vehicle when stationary, $f_{m,s}(t)=l(t)\sin \left(\theta (t)\right)$ denotes the additional resistance due to the gravity component, $\theta$ is the slope angle of the road surface where the vehicle travels, $f_{m,a}(t)$ represents the vehicle’s mechanical drive resistance, $f_s(v)$ is the aerodynamic drag and rolling resistance of the tires experienced during motion, $\rho (t)$ is the air density (kg/m³), $d(t)$ is the drag coefficient also known as the aerodynamic drag coefficient, $A$ is the vehicle’s frontal area (m²), and $\mu (t)$ is the rolling friction coefficient between the tire and road surface, which is related to tire material and structure, road condition, and weather.

In practical control systems, in order to reduce computational complexity, linear and quadratic terms are merged, and Euler’s theorem is used to sample continuous time state variables to obtain a discrete time dynamic model of connecting vehicles:

$$\{\begin{array}{l}s(k+1)=s(k)+\Delta v(k)\\ v(k+1)=v(k)+\Delta [(F(k)-f_B(k)-c_0(k)-c_1(k)v(k)-c_2(k)v^2(k)-l(k)\sin \left(\theta (t){\displaystyle \sum _{j=1}^k\Delta }\cdot v(k)\right)]\begin{array}{c}\\ \end{array}\end{array}$$

(2)

Here, $k$ denotes the sampling moment, and $k\in [0,1,\dots ,K]$ signifies the sampling period (s). Therefore, the total travel time $T$ of the connected vehicle can be expressed as $T=\Delta K$.

It is evident that the speed of connected vehicles is influenced and interfered with by many factors, and accurately representing vehicle speed through mechanism modeling is challenging. Therefore, data-driven methods are widely used for the speed estimation of networked vehicles, which can effectively handle unmodeled dynamic factors and improve the accuracy of speed estimation.

2.2. Data-Driven Vehicle Speed Modelling

Given the modeling uncertainties in the operation of connected vehicles, we establish the following nonlinear discrete system model:

$$v(k+1)=f(v(k),\cdots ,v(k-S_y),F(k),\cdots ,F(k-S_u))+\vartheta \overline{\omega }(k)$$

(3)

Here, $v(k)\in ℛ$ denotes the output of the connected vehicle system at moment $k$, that is, the vehicle’s speed; $F(k)\in ℛ$ denotes the control input of the vehicle; $f(\cdot ):{ℛ}^{S_u+S_y+2}\to ℛ$ denotes an unknown nonlinear function, with positive integers $S_u$ and $S_y$ representing the unknown orders of the system’s input and output; $\overline{\omega }(k)$ denotes the system’s uncertainty, following a normal distribution with zero mean and covariance $S$; and $\vartheta$ denotes the noise coefficient.

In order to transform the complex nonlinear system to a linear characteristic, this paper utilizes FFDL technology [42], which is conducive to studying the complex operating characteristics of connected vehicles.

Given the length constants for control output linearization as $L_y(0\le L_y\le S_y)$ and control input linearization as $L_u(1\le L_u\le S_u)$, when satisfying $\Vert \Delta T_{L_y,L_u}(k)\Vert \ne 0$, the full-format dynamic linearization data model can be obtained as:

$$\Delta v(k+1)={\varphi }_{f,L_y,L_u}^T(k)\Delta T_{L_y,L_u}(k)+\vartheta \omega (k)$$

(4)

Herein, the time-varying parameter vector ${\varphi }_{f,L_y,L_u}^T(k)$ is referred to as the Pseudo Gradient (PG), where $\Delta T_k(L_y,L_u)=T_k(L_y,L_u)-T_{k-1}(L_y,L_u)$, $\omega (k)=\overline{\omega }(k)-\overline{\omega }(k-1)$, ${\varphi }_{f,L_y,L_u}=\left[\varphi (1),\cdots ,\varphi (L_y)\varphi (L_y+1),\cdots ,\varphi (L_y+L_u)\right]$.

Following the time-varying autoregressive model conversion method in reference [43], through parameter separation, the above equation can yield the time-varying autoregressive (TVARX) model of order $(n+2)$:

$${\displaystyle \sum _{i=0}^{n+1}{a}_i}(k)v(k-i+1)={\displaystyle \sum _{j=0}^{n+1}{b}_j}(k)F(k-j+1)+\vartheta \omega (k)$$

(5)

Herein, $n=max\{L_y,L_u\}$, $a_i(k)$ represents the time-varying autoregressive coefficient, satisfying $a_0(k)=1$, and when $L_y+1<i\le n+1$, satisfying $a_0(k)=0$. $b_i(k)$ refers to the time-varying exogenous input excitation coefficient, which satisfies $b_j(k)=0$ when $j=0$ or $L_u+1<j\le n+1$. Hence, the nonlinear characteristics of connected vehicles are transformed into time-varying coefficients $a_i(k)$ and $b_i(k)$, requiring identification of these two time-varying coefficients.

Rewriting the TVARX model, we obtain the following parameter identification equation:

$$\{\begin{array}{l}v(k+1)={\mathsf{\Xi }}^T(k)\mathsf{\Theta }(k)+\vartheta \omega (k)\\ \mathsf{\Theta }(k)={[-a_1(k),-a_2(k),\cdots ,-a_{n+1}(k),b_1(k),b_2(k),b_{n+1}(k),\cdots ,b_{n+1}(k)]}^T\\ \mathsf{\Xi }(k)=[v(k),\cdots ,v(k-n),F(k),\cdots ,F(k-n)]\end{array}$$

(6)

where $\omega (k)~(0,S)$ and $\mathsf{\Theta }(k)$ denote the unknown time-varying parameter vectors, and $\mathsf{\Xi }(k)$ is the time-varying status vector (speed and tractive force) of the continuous system input and output.

Based on the model given above, the following data-driven parameter identification algorithm is proposed:

$$\begin{array}{c}P(k)=\left(I-K(k){\mathsf{\Xi }}^T(k)\right)P(k-1)\\ K(k)=P(k-1)\mathsf{\Xi }(k){(FS{F}^T+\mathsf{\Xi }(k{)}^{T}P(k-1)\mathsf{\Xi }(k))}^{-1}\\ \widehat{\mathsf{\Theta }}(k+1)=\widehat{\mathsf{\Theta }}(k)+K(k)\left(y(k+1)-{\mathsf{\Xi }}^T(k)\widehat{\mathsf{\Theta }}(k)\right)\end{array}$$

(7)

Equivalent transformation of the equation, the state equation, and output equation of the connected vehicle system can be derived:

$$\{\begin{array}{l}V(k+1)=A(k)V(k)+B(k)\mathit{F}(k)+\vartheta \mathit{\omega }(k)\\ v(k)=CV(k)\end{array}$$

(8)

wherein $V(k)={[V_1(k),V_2(k),\cdots ,V_{n+1}(k)]}^T\in {ℛ}^{n+1}$, $\mathit{F}(k)={[F(k),F(k-1),\cdots ,F(k-n)]}^T$, $\mathit{\omega }(k)={[0,\cdots ,0,\omega (k)]}^T\in {ℛ}^{n+1},C=[0\cdots 01]$,

$$A(k)=\left[\begin{array}{cccc}0& 1& \cdots & 0\\ 0& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 1\\ -a_{n+1}(k)& -a_n(k)& \cdots & -a_1(k)\end{array}\right],B(k)=\left[\begin{array}{cccc}0& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 0\\ b_1(k)& b_2(k)& \cdots & b_{n+1}(k)\end{array}\right]$$

Thus, the time-varying parameters $A(k)$ and $B(k)$ of the system can be estimated, thereby obtaining the discrete-time system state equation for the connected vehicle.

3. Design of State Estimator

3.1. Multi-Source Sensors Modeling

Given a network composed of $N$ interconnected sensor nodes, its topological structure can be represented as $𝒢=(𝒱,ℰ,𝒜)$. Here, $𝒱=\{1,2,\dots ,N\}$ denotes the set of nodes composed of sensor nodes $N$, $ℰ\subseteq 𝒱\times 𝒱$ is the set of edges composed of the communication channels between the sensors, and $𝒜=[a(i,j)]\in {ℛ}^{N\times N}$ is the adjacency matrix of the multi-sensor nodes. For the elements in the adjacency matrix 𝒜, if $(i,j)\in ℰ$, then $a(i,j)=1$, otherwise $a(i,j)=0$. $d(i)={\displaystyle \sum _{j=1}^{N}a(i,j)}$ is the in-degree of sensor $i$, and $D=diag\left\{d_i\right\}\in {ℛ}^{N\times N}$ is the in-degree matrix of the graph $𝒢$. The Laplacian matrix of the sensor network is $ℒ=D-𝒜$. The set of neighbors that can communicate with sensor node $i$ is defined as $\mathcal{N}(i)=\left\{j\right|(j,i)\in ℰ,\forall j\ne i\}$. $E\{\cdot \}$ represents the mathematical expectation, $diag\{\cdot \}$ refers to the diagonal matrix, and the mathematical symbol $\otimes$ denotes the Kronecker product operation.

The distributed connected vehicle speed sensor network is shown in the Figure 1. The connected vehicle travels on the road network, and the sensor nodes are used to measure the speed of the vehicle. Connected vehicles can also be equipped with sensors. Its observation range and corresponding communication topology are shown in Figure 1.

When the connected vehicle is within the effective observation range of the sensor node, the sensor node can communicate with neighboring sensors and estimate the vehicle speed based on its own and other sensors’ measurements; when the connected vehicle is outside of the observation range of the sensor node, it estimates the vehicle speed through the communication topology of neighboring sensors.

In order to ensure the safe operation of connected vehicles, this paper chooses four speed sensors based on different principles: inertial navigation sensors (INS), Global Navigation Satellite System (GNSS), visual image sensors (VS), and laser radar (LR). The measurement error model can be written as follows:

$${\mathit{Z}}_i(k)=H_i(k)V(k)+v_i(k)$$

(9)

where ${\mathit{Z}}_i(k)={[z_i(k-n),z_i(k-n+1),\dots ,z_i(k)]}^T\in {ℛ}^{n+1}$, $H_i(k)=diag(h_i(k))\in {ℛ}^{(n+1)\times (n+1)}$, $V(k)={[v(k-n),v(k-n+1),\dots ,v(k)]}^T\in {ℛ}^{n+1}$, ${\mathit{v}}_i(k)={[v_i(k-n),v_i(k-n+1),\dots ,v_i(k)]}^T\in {ℛ}^{n+1}$, $Z_i(k)$ represents the measurement vector of the speed of the connected vehicle by the sensor node $i$, $H_i(k)$ is the measurement matrix of sensor node $i$, and $v_i(k)$ is the measurement error and noise of the sensor node $i$,

$$h_i(k)=\{\begin{array}{ll}1& i=INS\\ 1& i=GNSS\\ (1+G_{vs})& i=VisualSensor\\ (1-G_{lr})& i=LaserRadar\end{array}, v_i(k)=\{\begin{array}{ll}({\delta }_{acc}+{\delta }_{ins})& i=INS\\ ({\delta }_{dely}+{\delta }_{me}+{\delta }_{gnss})& i=GNSS\\ ({\delta }_{env}+{\delta }_{motion}+{\delta }_{vi})& i=VS\\ ({\delta }_{echo}+{\delta }_{t\arg et}+{\delta }_{ir})& i=LR\end{array},$$

$v(k)$ is the real driving speed of the connected vehicle; $Z_{INS}$, $Z_{GNSS}$, $Z_{VS}$, $Z_{LR}$ are the speed measurement value of each sensors; ${\delta }_{acc}$, ${\delta }_{ins}$ are the measured cumulative error and other errors of INS. ${\delta }_{dely}$, ${\delta }_{me}$, ${\delta }_{gnss}$ are the atmospheric delay error, multipath effect error and other errors of GNSS. $G_{vi}$, ${\delta }_{env}$, ${\delta }_{motion}$, ${\delta }_{vs}$ are the correction parameters for the camera lens distortion, environmental noise error, motion blur error, and other errors of VS. $G_{lr}$, ${\delta }_{echo}$, ${\delta }_{t\arg et}$, ${\delta }_{ir}$ are the optical distortion correction coefficient, reflectance of the material error, multi-target measurement error, and other errors of VR. $G_{lr}$, ${\delta }_{echo}$, ${\delta }_{t\arg et}$, ${\delta }_{ir}$ are the optical distortion correction coefficient, reflectance of the material error, multi-target measurement error, and other errors of LR.

Combining data from multiple sensors, the generalized measurement model of the sensor network can be expressed as:

$$Z(k)=H(k)\otimes V(k)+v(k)$$

(10)

wherein $Z(k)={[Z_1{(k)}^T,Z_2{(k)}^T,\dots ,Z_N{(k)}^T]}^T$, $H(k)={[H_1^T(k),H_2^T(k),\dots ,H_N^T(k)]}^T$, and $v(k)={[v_1{(k)}^T,v_2{(k)}^T,\dots ,v_n{(k)}^T]}^T$.

3.2. Distributed Measurement Model of Connected Vehicle Speed

The distributed multi-sensor fusion architecture adopted in this paper allows each sensor node in the sensor network to communicate through a consistency strategy to achieve the optimal estimation of fusion. First, two basic assumptions of the sensor network are given:

Assumption 1. 

The multi-sensor network consists of a spanning tree, and the speed of the connected vehicle can be observed by at least one root sensor node.

Assumption 2. 

The real driving speed $v(t)$ of the connected vehicle can be observed by the overall multi-sensor network, that is, $H(k)={[H_1^T(k),\dots ,H_i^T(k),\dots H_N^T(k)]}^T$is observable.

For the multi-sensor speed measurement network system of connected vehicles, at the sensor node $i$, introducing the Fisher information matrix, we consider the following form of distributed measurement model based on the sensor network topology:

$$\begin{array}{cc}\hfill z_i(i)& =\left(\begin{array}{l}{z}^{dir}(i,k)\\ z^{ind}(i,k)\end{array}\right)\hfill \\ & =\left(\begin{array}{l}g(i_0)\left(\tilde{H}(i,k)V(i,k)+\tilde{\mathit{v}}(i,k)\right)\\ {\displaystyle \sum _{j=1}^{N}a(i,j)\gamma (i)\left[{(P^{-}(j,k))}^{-1}\left({\stackrel{⌢}{V}}^{-}(j,k)+\mathsf{\Omega }(i,j,k)\right)-{(P^{-}(i,k))}^{-1}{\stackrel{⌢}{V}}^{-}(i,k)\right]}\end{array}\right)\hfill \\ & \equiv H(i,k)V(k)+\mathit{v}(i,k)\hfill \end{array}$$

(11)

wherein $H(i,k)={\left(g(i_0)(\tilde{H}(i,k) d(i)\right)}^T$, $d(i)={\displaystyle \sum _{j=1}^{N}a(i,j)}$,

$$v(i,k)=\left(\begin{array}{l}g(i_0)\tilde{\mathit{v}}(i,k)\\ {\displaystyle \sum _{j=1}^N{a}_g(i,j)\left\{\gamma (i)\left[{(P^{-}(j,k))}^{-1}\left({\stackrel{⌢}{V}}^{-}(j,k)+\mathsf{\Omega }(ij,k)\right)-{(P^{-}(j,k))}^{-1}{\stackrel{⌢}{V}}^{-}(i,k)\right]-V(k)\right\}}\end{array}\right)$$

(12)

wherein $\tilde{H}(i,k)$ is the distributed observation matrix of the sensor node $i$, ${\stackrel{⌢}{V}}^{-}(i,k)$ is the a priori estimation value of the sensor node $i$ itself, $P^{-}(i,k)=E\left\{\left(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right){\left(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right)}^T\right\}$ is the a priori covariance matrix of the estimation error of the vehicle speed of the sensor node $i$, $\tilde{\mathit{v}}(i,k)$ is the measurement noise of the sensor node $i$, $\mathsf{\Omega }(i,j,k)$ is the communication noise between the sensor node $i$ and the sensor node $j$ and satisfies $\mathsf{\Omega }(i,j,k)~\left(0,\mathsf{\Pi }(i,j)\right)$. $\gamma (i)$ is the learning parameter and $\gamma (i)>0$. $g(i_0)$ indicates whether the sensor node can directly observe the speed of the connected vehicle. If it can measure its speed information, then $g(i_0)=1$, $\tilde{H}(i,k)\ne 0$; otherwise $g(i_0)=0$ and $\tilde{H}(i,k)=0$. As known from Assumption 1, at least one sensor node $i$ in the multi-sensor network measuring the speed of connected vehicles can observe the vehicle speed, i.e., it satisfies $g(i_0)\ne 0$. ${(P^{-}(r,k))}^{-1},r=i,j$ is the Fisher information matrix, representing the weight matrix of the consistent filter estimation of the sensor network, that is, the inverse of the speed error covariance matrix.

Theorem 1. 

Given a discrete-time nonlinear connected vehicle system, given the multi-sensor for speed estimation of connected vehicles (10), the optimal estimate of the driving speed of the connected vehicle can be obtained through the multi-source speed measurement sensor network consensus filtering (MSCF) algorithm.

The MSCF algorithm of the multi-sensor for the speed estimation of connected vehicles includes direct and indirect Kalman consistent gain (15) and (16), the speed estimation error covariance propagation of the sensor node (17), and the updates of the speed estimation of the sensor node (18), as follows:

$$N^{dir}(i,k)=g(i_0)P(i,k){\left(g^2(i_0)\tilde{H}(i,k)P(i,k){\tilde{H}}^T(i,k)+R^{dir}(i,k)\right)}^{-1}$$

(13)

$$N^{ind}(i,k)=d(i)P(i,k){\left(d^2(i)P(i,k)+R^{ind}(i,k)\right)}^{-1}$$

(14)

$$K^{dir}(i,k)=\left(I-d(i)N^{ind}(i,k)\right){\left[{\left(N^{dir}(i,k)\right)}^{-1}-g(i_0)d(i)\tilde{H}(i,k)N^{ind}(i,k)\right]}^{-1}$$

(15)

$$K^{ind}(i,k)=\left(I-g(i_0)N^{dir}(i,k)\tilde{H}(i,k)\right){\left[{\left(N^{ind}(i,k)\right)}^{-1}-g(i_0)d(i)N^{dir}(i,k)\tilde{H}(i,k)\right]}^{-1}$$

(16)

$$P(i,k+1)=A(i,k)M(i,k)P(i,k)A^T(i,k)+\vartheta (k)S(k){\vartheta }^T(k)$$

(17)

$$\begin{array}{ll}\stackrel{⌢}{V}(i,k+1)& =A(k)\stackrel{⌢}{V}(i,k)+B(k)F(k)+g(i_0)A(i,k)K^{dir}(i,k)[\tilde{H}(i,k)\\ & \cdot \left(V(k)-\stackrel{⌢}{V}(i,k)\right)+\stackrel{⌢}{\mathit{v}}(i,k)]+\gamma (i)A(i,k)K^{ind}(i,k){\displaystyle \sum _{j=1}^{N}a(i,j)}\\ & \cdot P^{-1}(j,k)\left(\stackrel{⌢}{V}(j,k)-\stackrel{⌢}{V}(j,k)+\mathsf{\Omega }(i,j,k)\right)\end{array}$$

(18)

wherein $R^{dir}(i,k)=\omega (i)$, $R^{ind}(i,k)={\gamma }^2(i){\displaystyle \sum _{j=1}^{N}a(i,j)P^{-1}(j,k)\mathsf{\Pi }(i,j,k)P^{-1}(i,k)}$, $\mathsf{\Pi }(i,j,k)=E\left\{\omega (i,j,k){\omega }^T(i,j,k)\right\}$, $M(i,k)=I-g(i_0)K^{dir}(i,k)\tilde{H}(i,k)-d(i)K^{ind}(i,k)$, $A(i,k)=A(k){\left[I-d(i)K^{ind}(i,k)+\gamma (i)K^{ind}(k){\displaystyle \sum _{j=1}^{N}a(i,j)(P^{-1}(j,k)-P^{-1}(i,k))}\right]}^{-1}$.

Proof. 

According to the multi-sensor measurement model (11), its direct and indirect connected vehicle speed measurement models can be written as follows.

$$z^{dir}(i,k)=H^{dir}(i)V(k)+v^{dir}(i,k)$$

(19)

$$z^{ind}(i,k)=H^{ind}(i)V(k)+v^{ind}(i,k)$$

(20)

wherein $H^{dir}(i)=g(i_0)\tilde{H}(i)$, $H^{ind}(i)=d(i)I$, $v^{dir}(i,k)=g(i_0)\tilde{v}(i)$, $v^{ind}(i)={\displaystyle \sum _{j=1}^{N}a(i,j)\left\{\gamma (i)\left[{\left(P^{-}(i)\right)}^{-1}\left({\stackrel{⌢}{V}}^{-}(j)+\mathsf{\Omega }(i,j)\right)-{\left(P^{-}(i)\right)}^{-1}{\stackrel{⌢}{V}}^{-}(i)\right]-V\right\}}$.

Knowing the discrete-time system state equation of the connected vehicle from Equation (8), and the direct and indirect measurement models of multiple sensors from Equations (19) and (20), the speed estimator of the connected vehicle can be made:

$$\begin{array}{c}\stackrel{⌢}{V}(i,k)={\stackrel{⌢}{V}}^{-}(i,k)+g(i_0)K^{dir}(i,k)\left(\tilde{H}(i)(V(k)-{\stackrel{⌢}{V}}^{-}(i,k))+\tilde{v}(i,k)\right)\\ +K^{ind}(i,k)\left({\displaystyle \sum _{j=1}^{N}a(i,j)\left(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right)+v^{ind}(i,k)}\right)\end{array}$$

(21)

$$\stackrel{⌢}{V}(i,k+1)=A(i,k){\stackrel{⌢}{V}}_i(k)+B(k)\mathit{F}(k)$$

(22)

where $A(i)=A{\left[I-d(i)K^{ind}(i)+\gamma (i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)\left({\left(P^{-}(j)\right)}^{-1}-{\left(P^{-}(i)\right)}^{-1}\right)}\right]}^{-1}$.

We let the state error covariance, the a priori estimation covariance, the posterior state estimation error, and the a priori state estimation error be as follows:

$$P(i,k)=E\left\{\tilde{V}(i,k){\tilde{V}}^T(i,k)\right\},P^{-}(i,k)=E\left\{{\tilde{V}}^{-}(i,k){\left({\tilde{V}}^{-}(i,k)\right)}^T\right\}$$

(23)

$$\tilde{V}(i,k)=V(k)-{\stackrel{⌢}{V}}_i(k),{\tilde{V}}^{-}(i,k)=V(k)-{\stackrel{⌢}{V}}^{-}(i,k)$$

(24)

Substituting Equation (21) into (24), we obtain:

$$\begin{array}{cc}\hfill \tilde{V}(i,k)& =V(k)-{\stackrel{⌢}{V}}^{-}(i,k)-g(i_0)K^{dir}(i,k)\left(\tilde{H}(i)\left(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right)+\tilde{v}(i,k)\right)\hfill \\ & -K^{ind}(i,k)\left({\displaystyle \sum _{j=1}^{N}a(i,j)\left(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right)+v^{ind}(i,k)}\right)\hfill \\ & =M(i,k){\tilde{V}}^{-}(i,k)-g(i_0)K^{dir}(i,k)\tilde{v}(i,k)-K^{ind}(i,k)v^{ind}(i,k)\hfill \end{array}$$

(25)

wherein $M(i,k)=I-g(i_0)K^{dir}(i,k)\tilde{H}(i)-d(i)K^{ind}(i,k)$.

In Equation (25), since the posterior estimation error $\tilde{V}(i,k)$ is only correlated to the measurement noise $\tilde{v}(i,k)$ of the sensor and is irrelevant to the rest of ${\tilde{V}}^{-}(i,k)$, ${\stackrel{⌢}{V}}^{-}(i,k)$, and $V(k)$; therefore, their product expectation is zero.

In this context, the covariance matrices of process noise, communication noise, direct measurement noise, and indirect measurement noise are defined as follows:

$$S(k)=E\left\{w(k)w^T(k)\right\}$$

(26)

$$\mathsf{\Pi }(i,j,k)=E\left\{\mathsf{\Omega }(i,j,k){\mathsf{\Omega }}^T(i,j,k)\right\}$$

(27)

$$R^{dir}(i,k)=E\left\{g^2(i_0)\tilde{v}(i,k){\tilde{v}}^T(i,k)\right\}=\omega (i)$$

(28)

$$R^{ind}(i,k)=E\left\{v^{ind}{\left(v^{ind}\right)}^T\right\}={\gamma }^2(i){\displaystyle \sum _{j=1}^{N}a(i,j)}{\left(P^{-}(j)\right)}^{-1}\mathsf{\Pi }(i,j){\left(P^{-}(j)\right)}^{-1}$$

(29)

Substituting Equation (25) into Equation (23), $P(i,k)$ can be expressed as:

$$\begin{array}{cc}\hfill P(i,k)& =E\left\{\tilde{V}(i,k){\tilde{V}}^T(i,k)\right\}\hfill \\ & =M(i,k)P^{-}(i,k)M^T(i,k)+K^{dir}(i,k)R^{dir}(i,k){\left(K^{dir}(i,k)\right)}^T\hfill \\ & +K^{ind}(i,k)R^{ind}(i,k){\left(K^{ind}(i,k)\right)}^T\hfill \end{array}$$

(30)

wherein $P(i,0)=E\left\{\tilde{V}(i,0){\tilde{V}}^T(i,0)\right\}$, $R^{ind}(i,k)={\gamma }^2(i){\displaystyle \sum _{j=1}^{N}a(i,j)}{\left(P^{-}(j)\right)}^{-1}\mathsf{\Pi }(i,j){\left(P^{-}(j)\right)}^{-1}$.

To obtain the direct and indirect Kalman gains $K^{dir}$ and $K^{ind}$, we minimize the trace of $P(i,k)$, and the direct and indirect Kalman gains can be written as:

$$K^{dir}(i)=(I-d(i)N^{ind}(i,k)){\left[{\left(N^{ind}(i,k)\right)}^{-1}-g(i_0)d(i)\tilde{H}(i)N^{ind}(i,k)\right]}^{-1}$$

(31)

$$K^{ind}(i)=(I-g(i_0)N^{dir}(i)\tilde{H}(i)){\left[{\left(N^{ind}(i,k)\right)}^{-1}-g(i_0)d(i)N^{dir}(i)\tilde{H}(i)\right]}^{-1}$$

(32)

wherein $N^{dir}(i,k)=g(i_0)P^{-}(i,k){\left(g^2(i_0)\tilde{H}(i)P^{-}(i,k){\tilde{H}}^T(i)+R^{dir}(i)\right)}^{-1}$,$N^{ind}(i,k)=d(i)P^{-}(i,k){\left(d^2(i)P^{-}(i,k)+R^{ind}(i,k)\right)}^{-1}$.

By substituting the covariance matrix $P(i,k)$ into the a priori covariance matrix $P^{-}(i,k)$ in the invertible matrices $N^{dir}$ and $N^{ind}$, we can obtain the direct and indirect Kalman consistent gain from Equations (15) and (16).

Substituting the derivative result of the Kalman gain into the covariance Equation (30), we obtain:

$$\begin{array}{cc}\hfill P(i,k)& =M(i,k)P^{-}(i,k)M^T(i,k)\hfill \\ & +g(i_0)M(i,k)P^{-}(i,k){\tilde{H}}^T(i,k){\left(K^{dir}(i,k)\right)}^T+d(i)M(i,k)P^{-}(i,k){\left(K_i^{ind}(i,k)\right)}^T\hfill \\ & =M(i,k)P^{-}(i,k)\left[M^T(i,k)+g(i_0){\left(K^{dir}(i,k)\tilde{H}(i,k)\right)}^T+d(i){\left(K^{ind}(i,k)\right)}^T\right]\hfill \end{array}$$

(33)

Substituting $M(i,k)=I-g(i_0)K^{dir}(i)\tilde{H}(i,k)-d(i)K^{ind}(i,k)$ into the Equation (33), we can obtain:

$$P(i,k)=M(i,k)P^{-}(i,k)$$

(34)

We redefine the distributed state transition matrix $A(i)$ as:

$$A=A(i)-d(i)A(i)K^{ind}(i)+\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left({\left(P^{-}(j)\right)}^{-1}-{\left(P^{-}(i)\right)}^{-1}\right)$$

(35)

Substituting Equations (8), (21), and (35) into Equation (24), we obtain:

$$\begin{array}{ll}{V}^{-}(i,k+1)& =A(i,k)\tilde{V}(i,k)+\vartheta (k)\mathit{\omega }(k)-A(i,k)[d(i)K^{ind}(i)-\gamma (i)\begin{array}{c}\\ \end{array}\\ & \cdot K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left({\left(P^{-}(j)\right)}^{-1}-{\left(P^{-}(i)\right)}^{-1}\right)]V(k)\end{array}$$

(36)

In Equation (36), both $\tilde{V}(i,k)$ and $V(k)$ are not correlated with $\mathit{\omega }(k)$, only $V^{-}(i,k+1)$ correlated with $\mathit{\omega }(k)$, so the expected value of their product is zero; that is, $E\left\{\tilde{V}(i,k)\mathit{\omega }(k)\right\}=E\left\{\mathit{\omega }(k){\tilde{V}}^T(i,k)\right\}=0$, $E\left\{V(k)\mathit{\omega }(k)\right\}=E\left\{\mathit{\omega }(k)V^T(k)\right\}=0$.

Therefore, based on Equation (36), the prior state error covariance matrix $P^{-}(i,k+1)$ can be simplified as:

$$\begin{array}{cc}\hfill P^{-}(i,k+1)& =E\left\{{\tilde{V}}^{-}(i,k+1){\left(V^{-}(i,k+1)\right)}^T\right\}\hfill \\ & =E\left\{A(i,k)\tilde{V}(i,k)V^T(i,k)A^T(i,k)\right\}+E\left\{\vartheta (k)\mathit{\omega }(k){\mathit{\omega }}^T(k){\vartheta }^T(k)\right\}\hfill \\ & =A(i,k)P(i,k)A^T(i,k)+\vartheta (k)S(k){\vartheta }^T(k)\hfill \end{array}$$

(37)

wherein $P^{-}(i,0)=E\left\{{\tilde{V}}^{-}(i,0){\left(V^{-}(i,0)\right)}^T\right\}$, $S(k)=E\left\{\mathit{\omega }(k){\mathit{\omega }}^T(k)\right\}$.

Substituting Equation (34) into the above Equation (37), we can obtain:

$$P^{-}(i,k+1)=A(i,k)M(i,k)P^{-}(i,k)A^T(i,k)+\vartheta (k)S(k){\vartheta }^T(k)$$

(38)

By replacing the prior covariance matrix $P^{-}(i,k)$ with the covariance matrix $P(i,k)$, the expression for the sensor node speed estimation error covariance updates $P(i,k+1)$ can be obtained:

$$P(i,k+1)=A(i,k)M(i,k)P(i,k)A^T(i,k)+\vartheta (k)S(k){\vartheta }^T(k)$$

(39)

Substituting Equation (21) into Equation (22) and combine like terms, we can obtain:

$$\begin{array}{cc}\hfill {\stackrel{⌢}{V}}^{-}(i,k+1)& =A(i)\left[I-d(i)K^{ind}(i)+\gamma (i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left({\left(P^{-}(j)\right)}^{-1}-{\left(P^{-}(i)\right)}^{-1})\right)\right]{\stackrel{⌢}{V}}^{-}(i,k)\hfill \\ & +B\mathit{F}(k)+g(i_0)A(i)K^{dir}(i)\left(\tilde{H}(i)(V(k)-{\stackrel{⌢}{V}}^{-}(i,k)\right)+\tilde{v}(i))\hfill \\ & +\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}{\left(P^{-}(j)\right)}^{-1}\left({\stackrel{⌢}{V}}^{-}(j,k)-{\stackrel{⌢}{V}}^{-}(i,k)+\mathsf{\Omega }(i,j)\right)\hfill \end{array}$$

(40)

Then, according to Equation (35), we replace the prior covariance matrix $P^{-}(i,k)$ with the covariance matrix $P(i,k)$ and replace the prior state estimate ${\stackrel{⌢}{V}}^{-}(i,k)$ with the state estimate $\stackrel{⌢}{V}(i,k)$. It is known that the speed estimation update $\stackrel{⌢}{V}(i,k+1)$ for each sensor node is:

$$\begin{array}{cc}\hfill \stackrel{⌢}{V}(i,k+1)& =A(k)\stackrel{⌢}{V}(i,k)+B(k)\mathit{F}(k)+g(i_0)A(i,k)K^{dir}(i,k)\left(\tilde{H}(i,k)\left(V(k)-\stackrel{⌢}{V}(i,k)\right)+\tilde{v}(i,k)\right)\hfill \\ & +\gamma (i)A(i,k)K^{ind}(i,k){\displaystyle \sum _{j=1}^{N}a(i,j)}{P}^{-1}(j,k)\left(\stackrel{⌢}{V}(j,k)-\stackrel{⌢}{V}(i,k)+\mathsf{\Omega }(i,j,k)\right)\hfill \end{array}$$

(41)

Proof is completed. □

4. Convergence Analysis of State Estimators

In dealing with discrete linear time-varying systems in Equation (8) and distributed state measurement models in Equation (11), Theorem 1 introduces the MSCF algorithm. This part will conduct rigorous mathematical analysis on the consistency convergence of the MSCF algorithm. Here are some lemmas about the Laplace matrix $L$, which will provide the necessary conditions for the proof of convergence.

Lemma 1. 

If and only if the directed graph ${𝒢}_g$ has a spanning tree, then the rank of the corresponding Laplacian matrix $L$ is $rank(L)=E-1$ , that is, $L$ has and only has one nonzero eigenvalue ${\mathsf{\lambda }}_1=0$ , and the rest of the non-zero eigenvalues have positive real parts.

Under the condition of Lemma 1, ${\mathsf{\lambda }}_1=0$ exists and is unique, satisfying $L\overline{1}m-0$, where $\overline{1}$ is a vector composed of $N$ ones, satisfying $\overline{1}={\left[1\cdots 1\right]}^T\in {ℛ}^N$, and $m$ is any constant. At the same time, the Laplacian matrix $L$ is a singular $M$ matrix, so the following Lemma 2 also applies.

Lemma 2. 

If the Laplacian matrix $L$ is a singular $M$ matrix, there always exists a positive vector $s={[s(1),s(2),\cdots ,s(N)]}^T$, where $s(1)>0,\forall i=1,2,\dots ,N$, such that $s^{T}L\ge 0$, and there exists $s(i){\displaystyle \sum _{i=1}^{N}a(i,j)}\ge {\displaystyle \sum _{i=1}^{N}s(j)s(j,i)}\ge 0,\forall i=1,2\cdots ,N$.

These two lemmas will be used for the proof of convergence analysis.

Theorem 2. 

Under the conditions of Assumptions 1 and 2, for a discrete-time nonlinear system of connected vehicles with process noise, given the sensor node $i$ and target speed measurement model, if the process noise $\omega (k)$, measurement noise $\tilde{v}(i,k)$, and communication noise $\mathsf{\Omega }(i,j,k)$ are all zero, using the distributed MSCF algorithm (13)–(18) can ensure that the state estimation $\overline{V}(i,k)$ of each sensor node consistently converges to the actual speed state $V(k)$ of the connected vehicle; that is, when $k\to \infty$, the distributed speed state estimation error $\tilde{V}(i,k)=V(k)-\stackrel{⌢}{V}(i,k)\in {ℛ}^n$ will eventually converge to zero.

Proof. 

Since the system process noise $w(k)=0$, the velocity estimation error covariance update rate $P(i,k+1)$ of sensor node $i$ is:

$$P(i,k+1)=A(i,k)M(i,k)P(i,k)A^T(i,k)$$

(42)

Based on Assumption 2, $\left(A(k),H(k)\right)$ is observable, and $P(i,k)$ is symmetrically positive definite. Therefore, $P(i,k)$ is invertible, satisfying:

$$\begin{array}{cc}\hfill P^{-1}(i,k+1)& =A^{-T}(k)P^{-1}(i,k)M^{-1}(i,k)A^{-1}(k)\hfill \\ & =A^{-T}(k)M^{-T}(i,k)P^{-T}(i,k)A^{-1}(k)\hfill \end{array}$$

(43)

Based on Assumption 1, the speed of connected vehicles can be observed by at least one sensor node $i$, satisfying $g(i_0)=1$. When not considering measurement noise, according to the distributed direct speed measurement model and indirect speed measurement model in Equation (11), the prior measurement noise model can be obtained as:

$$z^{dir}(i,k)=g(i_0)\tilde{H}(i)V(k)$$

(44)

$$z^{ind}(i,k)={\displaystyle \sum _{j=1}^{N}a(i,j)}\gamma (i)\left(P^{-1}(j,k){\stackrel{⌢}{V}}^{-}(j,k)-P^{-1}(i,k){\stackrel{⌢}{V}}^{-}(i,k)\right)$$

(45)

When the measurement noise $\tilde{v}(i)$ of multiple sensors is 0, the velocity estimation update in Equation (8) of sensor node $i$ is:

$$\begin{array}{cc}\hfill \stackrel{⌢}{V}(i,k+1)& =A(i)\stackrel{⌢}{V}(i,k)+B\mathit{F}(k)+g(i_0)A(i)K^{dir}(i)\tilde{H}(i)\left(V(k)-\stackrel{⌢}{V}(i,k)\right)\hfill \\ & +\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j)\stackrel{⌢}{V}(j,k)-P^{-1}(i)\stackrel{⌢}{V}(i,k)\right)\hfill \\ & -d(i)A(i)K^{ind}(i)\stackrel{⌢}{V}(i,k)\hfill \end{array}$$

(46)

When the process noise $\omega (k)$ is zero, the state equation of the connected vehicle driving system (8) can be written as:

$$V(k+1)=AV(k)+B\mathit{F}(k)$$

(47)

Combining Equations (46) and (47), the prior speed state estimation error dynamics $\tilde{V}(i,k+1)$ can be written as:

$$\begin{array}{cc}\hfill \tilde{V}(i,k+1)& =V(k+1)-\stackrel{⌢}{V}(i,k+1)\hfill \\ & =AV(k)+B\mathit{F}(k)-A(i)\stackrel{⌢}{V}(i,k)-B\mathit{F}(k)-g(i_0)A(i)K^{dir}(i)\tilde{H}(i)\left(V(k)-\stackrel{⌢}{V}(i,k)\right)\hfill \\ & +d(i)A(i)K^{ind}(i)\stackrel{⌢}{V}(i,k)-\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j)\stackrel{⌢}{V}(j,k)-P^{-1}(i)\stackrel{⌢}{V}(i,k)\right)\hfill \end{array}$$

(48)

Substituting the equation $A=A(i)-d(i)A(i)K^{ind}(i)+\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j)-P^{-1}(i)\right)$ into the above equation, we obtain:

$$\begin{array}{cc}\hfill \tilde{V}(i,k+1)& =A(i)\tilde{V}(i,k)-g(i_0)A(i)K^{dir}(i)\tilde{H}(i)\tilde{V}(i,k)-d(i)A(i)K^{ind}(i)\tilde{V}(i,k)\hfill \\ & +\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j)\tilde{V}(j,k)-P^{-1}(i)\tilde{V}(i,k)\right)\hfill \\ & =A(i)M(i)\tilde{V}(i,k)+\gamma (i)A(i)K^{ind}(i){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j)\tilde{V}(j,k)-P^{-1}(i)\tilde{V}(i,k)\right)\hfill \end{array}$$

(49)

When the communication noise $\mathsf{\Omega }(i,j,k)=0$, its covariance matrix $\mathsf{\Pi }(i,j)=0$, so that $R^{ind}(i,k)=0$. Substituting into Equation (14), we can get $N^{ind}(i,k)=1/d_i$, and then substitute it into Equation (16); thus, the distributed indirect Kalman gain matrix $K^{ind}(i,k)=1/d(i)$ can be updated.

A discrete-time Lyapunov function candidate can be defined as:

$$ℒ\left(\tilde{V}(k)\right)={\displaystyle \sum _{i=1}^N\frac{d(i)s(i)}{\gamma (i)}}{\tilde{V}}^T(i,k)P^{-1}(i,k)\tilde{V}(i,k)$$

(50)

where $s(i)$ is the positive component of the positive vector $s$ defined in Lemma 2, satisfying $s(i)>0$. Based on Assumption 2, $(A,H)$ is observable, $P(k)=diag\left(P^{-1}(i)\right)$ is positive definite, and $ℒ\left(\tilde{V}(k)\right)>0$ can be known by $d(i)>0$ and $\gamma (i)>0$.

Calculate the first order difference of $ℒ\left(\tilde{V}(k)\right)$ and substituting Equations (49) and (50), we obtain:

$$\begin{array}{cc}\hfill \Delta ℒ\left(\tilde{V}(k)\right)& ={\displaystyle \sum _{i=1}^N\frac{d(i)s(i)}{\gamma (i)}}{\tilde{V}}^T(i,k)\left\{M^T(i)A^T(i)P^{-1}(i,k+1)A(i)M(i)-P^{-1}(i,k)\right\}\tilde{V}(i,k)\hfill \\ & +2{\displaystyle \sum _{i=1}^{N}s(i){\tilde{V}}^T(i,k)}{P}^{-T}(i,k){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)\hfill \\ & +{\displaystyle \sum _{i,j=1}^N\frac{\gamma (i)s(i)}{d(i)}}a(i,j)[{\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)}^T\hfill \\ & \cdot P^{-1}(i,k)M^{-1}(i,k)\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)]\hfill \end{array}$$

(51)

It is known from Lemma 2 that the second term on the right side of Equation (51) needs to satisfy the following conditions:

$$\begin{array}{l}2{\displaystyle \sum _{i=1}^{N}s(i)}{\tilde{V}}^T(i,k)P^{-T}(i,k){\displaystyle \sum _{j=1}^{N}a(i,j)}\left(P(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)\\ \le -{\displaystyle \sum _{i,j=1}^{N}s(i)}a(i,j){\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)}^T\\ \cdot \left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)\end{array}$$

(52)

Based on Equation (52) and Lemma 6 in [44], the first-order difference $\Delta ℒ\left(\tilde{V}(k)\right)$ can be updated to:

$$\begin{array}{cc}\hfill \Delta ℒ\left(\tilde{V}(k)\right)& ={\displaystyle \sum _{i=1}^N\frac{d(i)s(i)}{\gamma (i)}}{\tilde{V}}^T(i,k)\left\{M^T(i)A^T(i)P^{-1}(i,k+1)A(i)M(i)-P^{-1}(i,k)\right\}\tilde{V}(i,k)\hfill \\ & -{\displaystyle \sum _{i,j=1}^{N}s(i)}a(i,j)\{{\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)}^T\hfill \\ & \cdot \left(I-\frac{\gamma (i)}{d(i)}{P}^{-1}(i,k)M^{-1}(i,k)\right)\left(P^{-1}(j,k)\tilde{V}(j,k)-P^{-1}(i,k)\tilde{V}(i,k)\right)\}\hfill \end{array}$$

(53)

Therefore, the stable convergence of the connected vehicle driving discrete system needs to satisfy the following two necessary conditions:

$$M^T(i,k)A^T(i,k)P^{-1}(i,k+1)A(i,k)M(i,k)-P^{-1}(i,k)<0$$

(54)

$$I-\frac{\gamma (i)}{d(i)}{P}^{-1}(i,k)M^{-1}(i,k)>0$$

(55)

The proof of condition in Equation (54) is shown as follows.

When the process noise is zero, it is known from Equations (30) and (37) that the velocity estimation error covariance matrix $P(i,k+1)$ can be represented as:

$$\begin{array}{cc}\hfill P(i,k+1)& =A(i,k)M(i,k)P(i,k)M^T(i,k)A^T(i,k)\hfill \\ & +A(i,k)K^{dir}(i,k)R^{dir}(i,k){\left(K^{dir}(i,k)\right)}^T{A}^T(i,k)\hfill \\ & +A(i,k)K^{ind}(i,k)R^{ind}(i,k){\left(K^{ind}(i,k)\right)}^T{A}^T(i,k)\hfill \end{array}$$

(56)

It can be seen from Equations (31) and (32) that:

$$K^{dir}(i,k)=g(i_0)M(i,k)P^{-}(i,k){\tilde{H}}^T(i){\left(R^{dir}(i,k)\right)}^{-1}$$

(57)

$$K^{ind}(i,k)=d(i)M(i,k)P^{-}(i,k){\left(R^{ind}(i,k)\right)}^{-1}$$

(58)

Then, substituting Equations (57) and (58) and into (56), we obtain:

$$\begin{array}{cc}\hfill P(i,k+1)& =A(i,k)M(i,k)P(i,k)M^T(i,k)A^T(i,k)+g^2(i_0)A(i,k)M(i,k)\hfill \\ & \cdot P(i,k){\tilde{H}}^T(i,k){\left(R^{dir}(i,k)\right)}^{-1}\tilde{H}(i,k)P^T(i,k)M^T(i,k)A^T(i,k)\hfill \\ & +d^2(i_0)A(i,k)M(i,k)P(i,k){\left(R^{ind}(i,k)\right)}^{-1}\hfill \\ & \cdot P^T(i,k)M^T(i,k)A^T(i,k)\hfill \end{array}$$

(59)

Substituting Equation (59) into Equation (54), we obtain:

$$\begin{array}{l}{M}^T(i,k)A^T(i,k)P^{-1}(i,k+1)A(i,k)M(i,k)-P^{-1}(i,k)\\ ={\left(P(i)+P(i)\psi (k)P^T(i)\right)}^{-1}-P^{-1}(i)\end{array}$$

(60)

where $\psi (k)=g^2(i_0){\tilde{H}}^T(i){\left(R^{dir}(i)\right)}^{-1}\tilde{H}(i)+d^2(i_0){\left(R^{ind}(i)\right)}^{-1}$.

Based on the inverse lemma of the matrix:

$${(A+BCD)}^{-1}=A^{-1}-A^{-1}B{(C^{-1}+D{A}^{-1}B)}^{-1}D{A}^{-1}$$

(61)

Equation (60) can be updated to:

$$M^T(i,k)A^T(i,k)P^{-1}(i,k+1)A(i,k)M(i,k)-P^{-1}(i,k)=-\left({\psi }^{-1}+P(i)\right)$$

(62)

$R^{dir}(i,k)$ and $R^{ind}(i,k)$ are both positive definite symmetric; therefore, $\psi (k)$ and $P(i)$ are both positive definite symmetric matrices, therefore satisfying $-\left({\psi }^{-1}+P(i)\right)<0$. Therefore, the proof of Equation (54) is completed. □

The proof of the condition in Equation (55) is shown as follows.

Equation (55) can be equivalently transformed into:

$$\left(M(i,k)P(i,k)-\frac{\gamma (i)}{d(i)}I\right)P^{-1}(i,k)M^{-1}(i,k)>0$$

(63)

Based on Equation (43), we obtain,

$$P^{-1}(i,k)M^{-1}(i,k)=A^T(i,k)P^{-1}(i,k+1)A(i,k)>0$$

(64)

Therefore, $P^{-1}(i,k)M^{-1}(i,k)$ is symmetrically positive definite, and when $0<{\gamma }_i<{\gamma }_i^{*}<d(i){\mathsf{\lambda }}_{min}\left(M(i,k)P(i,k)\right)$, the condition in Equation (45) is also true. The two necessary conditions are satisfied, the first-order forward difference satisfies $\Delta ℒ\left(\tilde{V}(k)\right)<0$.

Thus, $\Delta ℒ\left(\tilde{V}(k)\right)$ is the Lyapunov function. In the multi-sensor speed measurement network of the connected vehicle, assuming the process noise $\omega (k)$, measurement noise $\tilde{v}(i,k)$, and communication noise $\mathsf{\Omega }(i,j,k)$ are all zero, the speed estimation error dynamics (49) obtained by applying the MSCF algorithm in Equations (13)–(18) are globally asymptotically stable, and when $k\to \infty$, the distributed speed state estimation error ${\tilde{V}}_i(k)$ eventually converges to zero.

Based on reference [45], if the process noise $w(k)$, measurement noise $\tilde{v}(i,k)$, and communication noise $\mathsf{\Omega }(i,j,k)$ are all non-zero, the estimation error of sensor node $i$ for the speed of the connected vehicle is uniformly ultimately bounded (UUB). This means that the speed estimation of node $i$ for the connected vehicle falls within a bounded region of the real speed, and the size of this bounded region depends on the amplitude of the noise.

5. Numerical Simulation

To verify the effectiveness of the algorithm, simulation verification is carried out on the MATLAB platform. Assuming the connected vehicle is operating on a designated road section, the operating time is 1800 s, the sampling time is 0.1s, and the operating parameters of the vehicle are set as follows:

$$\{\begin{array}{l}{c}_0(k)=37.6*\sin (0.0052k)+211\\ c_1(k)=1.8*\sin (0.0049k)+25.17\\ c_2(k)=0.036*\sin (0.0037k)+0.2023\end{array}$$

(65)

Due to environmental interference on the given road section, the connected vehicle will be subject to the following time-varying interference resistance:

$$f_m(t)=\{\begin{array}{l}-0.04m,t\subset [150,400]\\ 0.08m,t\subset [450,680]\\ -0.07m,t\subset [1030,1400]\\ 0.075m,t\subset [1500,1700]\\ 0,t=else\end{array}$$

(66)

This simulation uses the adaptive iterative learning control AILC) in reference [46] to simulate the driving state of the connected vehicle. According to multi-sensor modeling, a speed measurement network composed of four types of sensors is established to estimate the actual speed of the connected vehicle. The connection topology of the sensor network is shown in Figure 2, where Node 0 represents the actual speed of the connected vehicle, and Nodes 1 to 4 are INS, GNSS, VS, and LiDAR sensors, respectively.

During the vehicle’s driving, it is assumed that the sensor Node 4 fails and cannot directly measure the actual state of target Node 0 but can communicate with other sensor nodes to achieve indirect measurement; other nodes can directly observe the actual speed of the target Node 0.

According to the connection relationship of the sensor network, the adjacency matrices between the sensor nodes and between the sensor nodes and the target nodes, and the degree matrix are defined as:

$${\mathcal{A}}_g=\left[\begin{array}{llll}0\hfill & 0\hfill & 0\hfill & 1\hfill \\ 0\hfill & 0\hfill & 0\hfill & 1\hfill \\ 0\hfill & 0\hfill & 0\hfill & 1\hfill \\ 1\hfill & 1\hfill & 1\hfill & 0\hfill \end{array}\right],{𝒢}_g=\left[\begin{array}{llll}1\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill \end{array}\right],{𝒟}_g=\left[\begin{array}{llll}1\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 3\hfill \end{array}\right]$$

The parameters value of the sensors is set as

Table 1. Parameters value of each sensor.

Sensor Type	Parameters Value
INS	$v_{INS}(k)=0.03randn\left(1\right)$
GNSS	$v_{GNSS}(k)=0.02randn\left(1\right)$
Visual image	$G_{VS}=0.01$ $v_{VS}(k)=0.04randn\left(1\right)$
Laser radar	$G_{LR}=0.01$ $v_{LR}(k)=0.01randn\left(1\right)$

:

During the operation of the connected vehicle, the process noise and the measurement noise of each sensor are shown in Figure 3.

In order to simulate vehicles driving on bumpy roads, the expected speed curve undergoes significant changes due to significant and rapid changes in speed, which could better test and verify the performance of the MSCF algorithm.

The estimation effect of the proposed MSCF algorithm on the actual speed of the connected vehicle and its errors are shown in Figure 4.

The results show that all four sensors can achieve the real-time tracking of the speed of the connected vehicle. The laser radar sensor, which cannot directly observe the speed of the connected vehicle, can also obtain speed estimation, and the speed estimation error can be controlled at an ideal level, and its estimation is consistent and ultimately bounded.

To verify the advancement of the proposed MSCF algorithm, a comparative simulation verification of connected vehicle speed estimation is carried out with the DEKCF and the DUKCF algorithms.

The results in Figure 5 and Figure 6 show that both algorithms can achieve a consistent convergence of the speed estimation of connected vehicle, but in terms of the speed estimation error effect, the proposed multi-sensor speed sensor network consensus filtering algorithm has a better estimation effect.

The mean square error (MSE) is used as the evaluation index to quantify the speed estimation effect of the three algorithms. The calculation method of the MSE is:

$$MSE=\frac{1}{N}{\displaystyle \sum _{i=1}^{N}M}S{E}_i, MS{E}_i=\frac{1}{k}{\displaystyle \sum _{i=1}^k{\left(v(k)-\stackrel{⌢}{v}(i,k)\right)}^2}$$

(67)

The MSE of the proposed MSCF algorithm and DEKCF and DU KCF algorithms are calculated, and the results are shown in the table below.

Compared with the DEKCF and DUKCF algorithms, the speed estimation MSE of the proposed MSCF algorithm dropped by 74.77% and 58.15%, respectively. The results are shown in

Table 2. Comparison of different algorithms.

Algorithm	Speed Estimation Bias MSE
DEKCF	0.0013
DUKCF	0.000798
MSCF	0.000334

. Therefore, the MSCF is more precise in estimating the speed of the connected vehicle and can meet the speed estimation needs in complex scenarios.

6. Conclusions

This work focuses on the speed estimation of a connected vehicle in a network of different types of speed sensors. Using dynamic linearization techniques, a data-driven dynamic model of the connected vehicle is established and then distributed between the direct and indirect measurement models by dynamically adjusting the information weights of sensor nodes. Combining the Kalman consistency filtering algorithm, we designed a new state estimator to achieve optimal consistent convergence estimation for the speed of connected vehicles. The convergence of the algorithm is strictly proven by introducing the Lyapunov function, and in the case where the process noise, measurement noise, and communication noise are all zero, the speed estimation error dynamics obtained by using the MSCF algorithm are globally asymptotically stable, and the distributed speed state estimation error will eventually converge to zero. The effectiveness of the algorithm has been proven through simulation.

The MSCF algorithm provides a new solution for the speed estimation problem of a connected vehicle in autonomous transportation systems, which can not only effectively improve the accuracy of speed estimation, but also has a good real-time performance and robustness and is suitable for a variety of complex traffic scenarios. In future work, we will investigate the problem of vehicle speed estimation in time-varying topologies, which can be combined with deep neural network-based methods and IMM algorithms to improve the tracking performance of challenging dynamic targets and meet the needs of autonomous transportation development.