Castillo Zamora

José De Jesús

Aguilera Álvarez

Juan Pablo

Escareño Castro
email: juan.escareno-castro@unilim.fr

Juan Antonio

Boussaada Islam
email: islam.boussaada@centralesupelec.fr

Martínez Nolasco

Juan José

Jiménez Garibay

Alonso Alejandro

A PEDAGOGICAL APPROACH TO DATA FUSION AND KALMAN FILTER

Introduction

The current paper is addressed to under graduated students interested in sensors and data fusion techniques. This work is written in an easy scientific language in order to be understood by the students. We propose the usage of the Kalman Filter as a first approach to data fusion as it is widely used in different research fields and applications as cited by [START_REF] Sasiadek | Sensor data fusion using Kalman filter[END_REF]] and [START_REF] Gao | Some remarks on Kalman filters for the multisensor fusion[END_REF].

In this matter, [START_REF] Liggins | Multisensor data fusion[END_REF] define Data fusion as the combination of the data from multiple sensors to achieve specific inferences that could not be achieved by the usage of one single sensor. Different techniques can be applied to carry out the aforementioned task, nevertheless Kalman Filtering is one of the most significant due to the robustness of the approach, the easy-reasoning-and-understanding of the filtering principle, among other benefits and advantages discussed by [START_REF] Gan | Comparison of two measurement fusion methods for Kalman-filter-based multisensor data fusion[END_REF] and [START_REF] Sasiadek | Sensor data fusion using Kalman filter[END_REF] who also mention several variations of this filter which improve the performance of the data fusion operation and/or estimation.

The literature offers a wide amount of papers and research concerning the application of Kalman Filtering techniques in data fusion, to mention some, [START_REF] Sasiadek | Sensor data fusion using Kalman filter[END_REF] used the Kalman Filter to improve the performance of autonomous robots to accomplish the navigation and trajectory-tracking tasks.

[ [START_REF] Zheng | Data fusion based multi-rate Kalman filtering with unknown input for on-line estimation of dynamic displacements[END_REF] compute the dynamic displacement of a given structure based on on-line multi-rate data fusion of high-sampling rate acceleration with time-varying bias and low-sampling rate displacement measurements. In addition, a linear Kalman Filter is implemented by [START_REF] Castillo-Zamora | Disturbances and Coupling Compensation for Trajectory Tracking of a Multi-link Aerial Robot[END_REF] to meet the trajectory tracking specifications of a multi-link unmanned aerial system. More detailed examples can be find in the literature.

The methodology used in the paper consists in a full pedagogical discussion and representation about the Kalman filter for data fusion followed by the sensors characterization and the dynamics modelling of the systems to be studied in the examples. Simulations were carried out to illustrate the behavior of the systems with and without the Kalman filter. The conclusions obtained ae lastly exposed.

Methodology

To understand the Kalman filtering algorithm, let us consider our linear system to be described in a discrete domain by the expressions:

𝑥 𝑘 = 𝐴𝑥 𝑘-1 + 𝐵𝑢 𝑘 + 𝑤 𝑘 ( 1
)
𝑦 𝑘 = 𝐻𝑥 𝑘 + 𝑣 𝑘 (2)
Equation 1 establishes that the current state of the system 𝑥 𝑘 ∈ ℝ 𝑛 is given in some proportion (𝐴 ∈ ℝ 𝑛×𝑛 and 𝐵 ∈ ℝ 𝑛×𝑚 ) by the previous state 𝑥 𝑘-1 ∈ ℝ 𝑛 and the control input 𝑢 𝑘 ∈ ℝ 𝑚 . The term 𝑤 𝑘 ∈ ℝ 𝑛 is called process noise and represents the influence of unmodeled phenomena. Equation 2 defines the measurement value 𝑦 𝑘 ∈ ℝ 𝑝 as a linear combination of the current state (in some proportion 𝐻 ∈ ℝ 𝑝×𝑛 ) and the measurement noise 𝑣 𝑘 ∈ ℝ 𝑝 which is normal distributed, i.e. Gaussian. One should notice that the better the noise parameters are defined, the better estimates one gets.

Based on equation 1 and 2, the Kalman Filter equations are conceived such that these are divided in two sets: Time Update (prediction) and Measurement Update (correction). The first set is formed by the expressions:

𝑥 ̂𝑘 -= 𝐴𝑥 ̂𝑘-1 + 𝐵𝑢 𝑘 (3)
𝑃 𝑘 -= 𝐴𝑃 𝑘-1 𝐴 𝑇 + 𝑄 (4)

𝐾 𝑘 = 𝑃 𝑘 -𝐻 𝑇 (𝐻𝑃 𝑘 -𝐻 𝑇 + 𝑅) -1 (5)
While the correction set is defined by:

𝑦 𝑘 = 𝑠𝑒𝑛𝑠𝑜𝑟 𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑚𝑒𝑛𝑡 (6)
𝑥 ̂𝑘 = 𝑥 ̂𝑘 -+ 𝐾 𝑘 (𝑦 𝑘 -𝐻𝑥 ̂𝑘 -) ( 7)

𝑃 𝑘 = (𝐼 -𝐾 𝑘 𝐻)𝑃 𝑘 - ( 8
)
Both sets are applied at each step or iteration in strict order, moreover, 𝐼 ∈ ℝ 𝑛×𝑛 stands for the identity matrix, 𝑃 𝑘 ∈ ℝ 𝑛×𝑛 is the error covariance matrix and, 𝑅 ∈ ℝ 𝑝×𝑝 and 𝑄 ∈ ℝ 𝑛×𝑛 are the covariance matrices of the environment noise and the process noise, respectively. Often, these ones result difficult to compute.

To start the process, we need to know the estimate of 𝑥 and 𝑃 at the beginning of the process, these values are called initial conditions and are represented as 𝑥 ̂0 and 𝑃 0 , respectively.

The Filter explained above in equations 3-8 is often used for estimation tasks, in order to implement it as a data fusion technique some modifications and assumptions need to be taken into account.

Let us consider, a mechanical system whose states are the position and velocity, i.e. 𝑥 and 𝑥, and a pair of sensors at our disposal for knowing the position and acceleration, an ultrasonic sensor and an accelerometer, for instance.

The position captured by the ultrasonic sensor is written as 𝑥 𝑢 ∈ ℝ that can be derivate w.r.t. time to obtain the velocity 𝑥u ∈ ℝ and the acceleration 𝑥ü ∈ ℝ. By integrating the acceleration signal coming out from the accelerometer 𝑥ä ∈ ℝ, also the velocity 𝑥ȧ ∈ ℝ and the position 𝑥 𝑎 ∈ ℝ can be known.

To estimate the states of the system with more accuracy, 𝑥̈𝑎 is used at the Prediction stage of the Kalman Filter and 𝑥 𝑢 is used in the correction stage, as shown next.

By simple kinematics the position and the velocity of the system can be obtained [Castillo-Zamora, 2016], i.e.

𝑥 = 𝑥 0 + 𝑥̇𝑡 + 1 2 𝑥̈𝑡 2 (9)
𝑥̇= 𝑥̇0 + 𝑥̈𝑡 [START_REF] Castillo-Zamora | Comparison of PD, PID and sliding-mode position controllers for V-tail quadcopter stability[END_REF] Where 𝑥 0 ∈ ℝ and 𝑥0 ∈ ℝ are the initial position and velocity of the system, 𝑡 ∈ ℝ + stands for the time. The discrete representation of equations 9 and 10 is given as:

𝑥 𝑘 = 𝑥 𝑘-1 + 𝑥k -1 𝑑𝑡 +
In our case, the vector 𝑈 𝑘 will contain 𝑥ä measured at the instant 𝑘.The fact that we can observe the position by means of the ultrasonic sensor implies an observation equation as follows:

𝑌 𝑘 = 𝐻 𝑋 𝑋 𝑘 ; 𝐻 𝑋 = [ 1 0 ] ( 15
)
The information provided by equations 13, 14 and 15 allows us to rewrite the Kalman filter equations 3-8 as shown below.

𝑋 ̂𝑘 -= 𝐴 𝑋 𝑋 ̂𝑘-

Notice that now, the signals of the ultrasonic sensor and the accelerometer take part of the Kalman filter equations also it is worth to mention that at the output of the filter, the vector 𝑋 ̂𝑘 contains the states of the system computed from the measurement of both sensors thus it is used to close the control loop as explained later. Further and detailed information about the Kalman Filter theory is available in the references cited in this work.

Example 1: Mini car robot

The figure 1 For sake of simplicity, we are interested only in the translational linear movement of the car. Notice that in figure 1, two different frames have been defined: the inertial reference frame (𝑂 𝐼 defined by the axis 𝑥 𝐼 and 𝑦 𝐼 ) which is fixed to some point in the space and the body-fixed reference frame (𝑂 𝑏 with the axis 𝑥 𝑏 and 𝑦 𝑏 ) which is located at the center of gravity of the vehicle. The position of the vehicle is described by 𝑥 ∈ ℝ and measured as shown, additionally, the velocity and the acceleration of the car are represented as 𝑥̇∈ ℝ and 𝑥̈∈ ℝ, respectively defined by the first and second time derivatives.

In order to control the vehicle, it is important to have the mathematical description of it. The dynamic model can then be obtained by the Newton Euler formalism or by the Euler Lagrange formalism [Castillo-Zamora, 2016]. For the simplest case of our robot, by both procedures, the dynamics is found to be:

𝑚 𝑐 𝑥̈= 𝐹 𝑥 (22)
Notice that the friction force between the wheels of the car and the floor has been omitted. 𝐹 𝑥 ∈ ℝ represents the force applied over the vehicle along the 𝑥 𝐼 . The mass of the vehicle is expressed as 𝑚 𝑐 ∈ ℝ + .

Let us assume that the force 𝐹 𝑥 can be easily manipulated, this allows us to define such force as the control input 𝑢 𝑥 ∈ ℝ, i.e.:

𝐹 𝑥 = 𝑢 𝑥 (23)
The main goal of the controller in this example is to make the vehicle go to a desired specific point in the space located at a distance 𝑥 𝑑 ∈ ℝ in the 𝑥 𝐼 axis. To do so, we can apply different control techniques however, we are going to apply a PD controller [Castillo-Zamora et al., 2018]:

𝑢 𝑥 = 𝐾 𝑝 𝑥 𝑒 𝑥 + 𝐾 𝑣 𝑥 𝑒ẋ ( 24
)
where 𝐾 𝑝 𝑥 and 𝐾 𝑣 𝑥 (both ∈ ℝ + ) are the proportional and derivative gains of the controller, meanwhile 𝑒 𝑥 = 𝑥 𝑑 -𝑥 ∈ ℝ is the position error and 𝑒̇𝑥 = 𝑥̇𝑑 -𝑥̇= -𝑥̇∈ ℝ is the velocity error. Notice that as the desired position is a fixed point in the space, i.e. it is a constant, its time derivative is always 0.

Based on equations 23 and 24, equation 22 can be rewritten such that:

𝑚 𝑐 𝑥̈= 𝐾 𝑝 𝑥 𝑒 𝑥 -𝐾 𝑣 𝑥 𝑥̇ (25)
Equation 25 will be used at the simulation stage to implement the data fusion for merging the information of the sensors.

Example 2: Mini bi-rotor platform

Analogously to the analysis of the mini car robot, the modelling and control of the bi rotor shown in figure 2 The mass of the aircraft is represented by 𝑚 𝑣 ∈ ℝ + and the gravity acceleration 𝑔 = 9.81 𝑚 𝑠 2 ⁄ acts over the vehicle as depicted, defining the equation of motion, according to the Newton Euler formalism, to be:

𝑚 𝑣 𝑧̈+ 𝑚 𝑣 𝑔 = 𝑓 1 + 𝑓 2 (26)
In this case, the sum of the forces acts as the control, i.e.

𝑓 1 + 𝑓 2 = 𝑢 𝑧 ( 27
)
Once more, the objective of the controller is to stabilize the vehicle at certain constant altitude 𝑧 𝑑 ∈ ℝ above the ground. As in the previous example, a PD controller is used to accomplish such goal nevertheless we add the gravitational terms to the equation as follows:

𝑢 𝑧 =

Sensors characterization

In order to study the proposed data fusion algorithm, the characterization and correct simulation of the sensors are a most. In this regard, the accelerometer can be simulated as a signal with an addition of some white noise (Gaussian) and a given offset, this because the accelerometer measures also the gravity acceleration and, depending on its attitude, the sensor gives back some acceleration value even when the vehicle is not in motion [START_REF] Awasthi | MEMS accelerometer based system for motion analysis[END_REF]] thus, the total acceleration measurement coming from the sensor is composed by the sum of three components:

the real acceleration of the vehicle, the Bias acceleration (or Offset) and the Gaussian distributed noise. One should be warned that obtaining the velocity and position from the integration of the accelerometer information can produce significant errors due to the (almost) constant which becomes a linear function when integrated the first time and a quadratic function after the second integration.

The ultrasonic sensor, in simulation, has the advantage of not carrying a derivation accumulative error for both, velocity and acceleration, computations but presents the disadvantage of amplification of the velocity and accelerations as well as not considering the initial positions and velocities of the system as when using the accelerometer. In addition, the sensing process with this kind of sensors is slow compared with that of the accelerometer [START_REF] Carullo | An ultrasonic sensor for distance measurement in automotive applications[END_REF]. For short, in Matlab/Simulink, this sensor signal will be composed by the real position of the vehicle and a Gaussian distributed noise with slower sampling time than that of the acceleration sensor previously described.

One should have in mind that most of the times, control simulations are carried out considering ideal elements (actuators and plants) that can provide the desired control command to the system which, in fact, does not happen in reality. Moreover, we must think of which kind of control signals we need, e.g. for translational mechanical systems the signal control is given (typically) in Newtons yet sometimes the actuators provide actions or signals that are related to displacements (meters or radians) and torques (Newton-meters) which implies that a conversion is needed, leading to more errors and less accuracy in the prediction of the response. In these examples, we consider only a saturation of the control input and assume that we can directly provide such command in the corresponding units.

Results

The proposed fusion data technique can be proved to be valid via numerical simulations, in order to do so, Matlab and Simulink provide the most appropriate tools and environment. 1 which contains all the parameters used in simulation as well as the corresponding value or magnitude.

For both systems, we defined the total simulation to be 30 s with a sampling time of 0.001 s in order to perform a fixed time step simulation. In the case of the Bi rotor platform the gravity acceleration is also established in the program according to Table 1. In addition, we assumed that the accelerometer and the ultrasonic sensor installed on the robots have the characteristics exposed in Table 1. In this regard, The equations 16 -21 were used in the simulation to make the data fusion, nevertheless the covariance matrices and those of the system were defined in the Matlab code as exposed by figure 4. In this matter, the covariance matrix of the process noise is most of the times defined as the zero matrix because it is difficult to be defined in reality yet sometimes it is used to indicate parameters variation and or errors in the mathematical modelling process. In our case we used this matrix to define an error in the computation of the velocity as it comes from an integration of the accelerometer data which has an offset as explained before. In this sense, in this important to keep in mind that the greater the matrix Q, the slower response of the Simulink. We used to different files to complete the simulation nevertheless the structure of both files is exactly the same, the only thing that changes are the properties of the systems according to Table 1 and the equations provided in the subsections of each pedagogical platform. For such reason only the Simulink file of the mini car simulation will be explained and shown next.

Before going any further in the Simulink environment, in the "Model Parameters Configuration" option the simulation time is changed as well as the simulation type which in our case it corresponds to the fixed step time with the given sampling time in Table 1.

For different scenarios were simulated for the same system: The first one corresponded to the ideal performance of the sensors, the second one considered only the accelerometer, the third one was defined in order to use only the ultrasonic sensor and lastly, the last one merged the data coming from both sensors by means of the Kalman filter. In the four scenarios, the saturation of the motors was taken into consideration. Figure 5 depicts what is discussed in this paragraph.

In the middle of the Figure 5, surrounded by a circle, you find the clock connected to a "To Workspace block" which allows us to store the time. All the blocks of the file have a sampling time 𝑑𝑡 given in Table 1. Notice that we have used the "From" and "Goto" blocks to avoid having too many connections which would make more difficult the interpretation of the block diagram below. Similarly, the blocks diagram of the up right corner, which corresponds to the system when using only the ultrasonic sensor, differs from the ideal case by the addition of the ultrasonic sensor noise at the output of the integrator that computes the position. Once the position is obtained by a double integration process and the noise had been added to it, the results is derived w.r.t. time once to get the velocity and twice to compute the acceleration. The noise of this sensor is characterized as depicted in Figure 8. The noisy position and velocity were used as feedback to close the system as can be appreciated in Figure 5.

To implement the Kalman filter for data fusion, we considered the accelerometer and the ultrasonic sensor as described for the previous scenarios. The signals are

given as inputs to the Kalman filter Matlab function block alongside the covariance matrix as shown in Figure 5. "Memory" blocks were used to avoid the algebraic loop problem and also to initialize the filter as they contained the corresponding initial values of the position, velocity and covariance matrix. The code written inside the Kalman function block is given in Figure 9.

Once the Simulink environment was ready, the file was saved with the name "SimCar" and the simulation was run from the Matlab main file previously explained.

It is worth to mention that the files should be located in the same folder.

The results of the mini car robot and the bi-rotor platform simulations are depicted in Figures 10 and11, respectively. In both cases the positon of the vehicle, the velocity and the accelerations as well as the control inputs are depicted.

Similar conclusions can be made for both systems. One can observe, in the position plots, that when using only the accelerometer for closing the control loop, the system does not reach the desired position, i.e. it exists a steady state error which is caused (mainly) by the offset of the sensor and the saturation of the actuators in addition to the accumulative error in the integration process. To use exclusively the ultrasonic sensor seems to work in terms of position, and as expected, the system in which the Kalman filter was implemented also accomplishes the goal of reaching a given reference in a finite time. The velocity plots show the disadvantage of using only the ultrasonic sensor as the computed velocity becomes noise and its magnitude is big in comparison with the velocity in the ideal scenario. The Kalman filter provides a noisy velocity that tracks the desired behavior. The velocity obtained only by the integration of the accelerometer signal behaves far from the desired response. In this sense, the Kalman Filter provides a better velocity estimation. The acceleration plots show a magnification of the acceleration in the case where only the ultrasonic sensor is used. On the other hand, the acceleration given by the accelerometer is clearly affected by the offset and the noise. For the case of the Kalman Filter implementation, we can conclude that even when the acceleration provided by the accelerometer is not the real one, the data fusion technique does a correct estimation of the position and the velocity, making the system reach the desired position even with wrong acceleration measurements.

The influence saturation of the actuators is more evident in the case of the mini car robot as depicted by the corresponding plots, however the system is able to overcome such issue when implementing the data fusion by means of the Kalman Filter.

Conclusions

A pedagogical approach to Kalman Filtering was given and explained considering two examples where such technique was use to illustrate the data fusion algorithm.

In addition, the characterization of an ultrasonic sensor and an accelerometer was established and can be considered in future simulations for students. The mathematical models of the vehicles were developed and additional systems suggestions were proposed. It is expected the usage of the in-here studied technique to improve the results of academic projects in engineering formation.

Figure 1

 1 Figure 1 The mini car robot.

Figure 2

 2 Figure 2 The bi-rotor platform.

Figure 3

 3 Figure 3 shows the Matlab code defining what has been commented in this paragraph.

  filter [Castillo-Zamora et al., 2019].

Figure 3

 3 Figure 3 Matlab code: Systems properties and conditions.

Figure 4

 4 Figure 4 Matlab code: Kalman filter elements.

Figure 4

 4 Figure 4 also gives evidence of the instructions used to launch the simulation in

Figure 5

 5 Figure 5 Simulink environment: up left corner: ideal performance of the sensors; up right corner: usage of the ultrasonic sensor only; bottom left corner: usage of the accelerometer only, and, bottom right corner: data fusion by the Kalman Filter.

Figure 6

 6 Figure 6 Mini car simulation results.

Figure 7

 7 Figure 7 Accelerometer noise and offset blocks.

Figure 8

 8 Figure 8 Ultrasonic sensor noise block.

Figure 9

 9 Figure 9 Mini car simulation results.

Figure 10

 10 Figure 10 Mini car simulation results.

Figure 11

 11 Figure 11 Bi-rotor simulation results.

  𝐾 𝑝 𝑧 𝑒 𝑧 -𝐾 𝑣 𝑧 𝑧̇+ 𝑚 𝑣 𝑔 (28) with 𝐾 𝑝 𝑧 and 𝐾 𝑣 𝑧 (both ∈ ℝ + ) are the proportional and derivative gains of the controller, meanwhile 𝑒 𝑧 = 𝑧 𝑑 -𝑧 ∈ ℝ is the position error. The definitions in equations 27 and 28 allow us to rewrite the dynamics of the closed loop system in equation 26 such that: 𝑚 𝑣 𝑧̈+ 𝑚 𝑣 𝑔 = 𝐾 𝑝 𝑧 𝑒 𝑧 -𝐾 𝑣 𝑧 𝑧̇+ 𝑚

𝑣 𝑔 (26) Further information about the dynamics of the vehicle and its control, as well as other models for pedagogical implementation can be found in [Castillo-Zamora et al., 2015], [Castillo-Zamora, 2016], [Escareño et al., 2017] and [Castillo-Zamora et al., 2018].

Table 1

 1 Simulation parameters and conditions.

		Simulation parameters
	Total simulation time		30 𝑠
	Simulation sampling time (𝑑𝑡)	0.001 𝑠
	Gravity acceleration (𝑔)		9.81 𝑚 𝑠 2 ⁄
		Sensors parameters
	Accelerometer variance		0.0001
	Mean Bias value		-0.97 𝑚 𝑠 2 ⁄
	Bias variance		0.001
	Ultrasonic sensor variance		0.0001
	Ultrasonic sensor sampling time	30 * 𝑑𝑡
	Mini car properties and parameters	Bi-rotor properties and parameters
	Mass	1 𝑘𝑔	Mass	0.5 𝑘𝑔
	Initial position	0 𝑚	Initial position	2 𝑚
	Initial Velocity	0 𝑚 𝑠 ⁄	Initial Velocity	0 𝑚 𝑠 ⁄
	Proportional gain	1	Proportional gain	1
	Derivative gain	1	Derivative gain	1
	Desired position	2 𝑚	Desired position	0.75 𝑚

Before giving the detail information about the development of the simulation environment, let us introduced the Table