# Kalman Filter: Historical Overview and Review of Its Use in Robotics 60 Years after Its Creation

Authors: Claudio Urrea; Rayko Agramonte
Journal: Journal of Sensors
 (2021)

---

## Abstract

Due to its widespread application in the robotics field, the Kalman filter has received increased attention from researchers. This work reviews some of the modifications conducted on to this algorithm over the last years. Problems such as the consistency, convergence, and accuracy of the filter are also dealt with. Sixty years after its creation, the Kalman filter is still used in autonomous navigation processes, robot control, and trajectory tracking, among other activities. The filter is not only restricted to robotics but is also present in different fields, such as economics and medicine. In addition, the characteristics of each modification on this filter are analyzed and compared.


---

## Body

## 1. Introduction

Over the last 20 years, several articles on the use of Kalman filter have been published, with numerous variations and contributions to solve specific problems, particularly concerning robotic systems. This filter has multiple applications, for example, in the car, military, and biomedicine industries. Therefore, it is not limited to the engineering field but also employed in economic systems.The KF (Kalman filter) developed by Rudolf E. Kalman [1] in 1960 is an algorithm for the estimation of nonobservable state variables based on observable variables that may have some measurement error. In other words, it enables identifying the hidden (nonmeasurable) state of a dynamic linear system in the same way as the Luenberger observer but also works when the system is subject to additive white noise [2]. Since this filter is a linear and optimal estimator, from the least-squares perspective, and due to its widespread use in problem solving, it became necessary to extend its use to nonlinear systems.In 1960, Dr. Kalman introduced its known publication [1] to Dr. Schmidt from the ARC (Ames Research Center) and to other researchers who were working on midcourse navigation and guidance for the circumlunar mission from 1959. The problems these researchers were dealing with were modeled through nonlinear systems, but the filter used was linear. However, Kalman’s proposal was interesting to these researchers as the new filter could be adapted and used not only as a solution for their problems but also to mitigate computation calculation problems in IBM 704 computers. Back then, such problems could not be solved by the Weiner filter as this conducted estimations that restricted severely the observation of the system or destroyed inherent precision, even though it had been used in guidance and navigation for beamrider and homing missiles.Later, studies about KF conducted by the Dynamic Analysis Branch of NASA—headed by Dr. Schmidt—undoubtedly were fundamental to the emergence of what today is known as the EKF (Extended Kalman Filter). Thanks to these studies and their first major application, the following was achieved [3]:
(1)
A demonstration of the adaptability of Kalman original theory to nonlinear problems(2)
The development of EKF, which reduces the effects of the problems arising in nonlinear systems after conducting a linearization over the best real state estimation(3)
The decomposition and reformulation of the original Kalman algorithm in separate time-update and measurement-update portions so measurements could be processed at any arbitrary time interval(4)
Showing the potential of the Kalman filter through its application in a full digital simulation that solved a nonlinear orientation and navigation problem that occurs on a spaceship(5)
The dissemination of results from the simulation conducted at the MIT Instrument Laboratory for their possible incorporation into the control and navigation system of the Apollo spacecraft(6)
The dissemination of information about the Ames Kalman filter to a large number of scientific and aerospatial units through presentations and formal workThe EKF extension is achieved using Taylor’s approximation, through which a nonlinear system can be linearized employing estimation techniques. This algorithm is a very powerful tool for positioning: it can accept different types of data, solve a large number of parameters, and produce reliable and accurate results [4]. Both the KF and the EKF are used in robotics, where they are applied in trajectory tracking, position estimation for manipulator robots, SLAM (Simultaneous Localization and Mapping), and object detection, among others, depending on the linearity or nonlinearity of the model. The flexibility of this algorithm has enabled the integration of information from different types of sensors and techniques such as odometry, GNSS (Global Navigation Satellite System), laser and ultrasonic sensors, and recently, artificial vision, making it possible to answer the fundamental questions of autonomous navigation: Where am I? Where am I going? And how do I reach my destination?The Kalman filter is better than other algorithms used for estimation due to the small room it needs for storage and its wide variety of uses. However, the impact on the environment surrounding it, errors from measuring equipment, and incorrect parameter selection usually cause system errors in real applications [5]. Researchers have developed different variants or modifications in the last years. These modifications are aimed at solving the problems the algorithm presents due to the increase in the complexity of the equipment in which is applied, and also owing to the accuracy and efficiency that manufacturing, medicine elaboration, and navigation, among other processes, need nowadays. An analysis of the systems in which the Kalman filter or its modifications have been employed is proposed in this work, emphasizing that the main advantages of the KF and its variants are its simplicity and capability to provide accurate estimations and prediction results.This work addresses the different modifications made to the KF across time, as well as the diverse problems that these modifications have solved, and the original filter failed to deal with. Additionally, some applications for KF/EKF modifications in robotics are presented. This work is divided into five sections. Section2 introduces the structure of the Kalman filter and its extensions to nonlinear systems. In Section 3, different modifications are described, and Section 4 analyzes different applications of the KF. In Section 5, some comparisons and conclusions are drawn.


## 2. Kalman Filter

### 2.1. Kalman Filter for Lineal Systems

The KF is an algorithm that requires two types of equations: one type links state variables to observable variables (main equations), while the other determines the temporary structure of state variables (state equations).Estimations of state variables are conducted based on the dynamics of these variables (time dimension), as well as on the measurements of observable variables obtained at each time instant (transversal). In other words, the dynamics are summarized in two steps as follows:(i)
Estimation of state variables using their own dynamics (prediction stage)(ii)
Improvement of the first estimation using information from observable variables (correction stage)An attractive characteristic of this methodology is its recursion, which implies that it can be used in real time. Once the algorithm predicts the new state at momentt, it adds a correction term, and the new “corrected” state serves as an initial condition at the following stage, t+1. In this way, the estimation of the state variables uses all the information available up to that moment and not only that of the stage before estimation (this is known as “signal extraction”).Given the linear process represented by(1)xk=Axk−1+Buk+wk−1,and the measurement equation:
(2)zk=Hxk+vk,where the independent, white, Gaussian distributed process noise and measurement noise are pw∼N0,Q, pv∼N0,R.The KF represents a belief or trust in thexk state at time k that is given by the mean, x^k−, and the Pk− covariance. The entry received by KF is the belief in time k−1, represented by x^k−1 and Pk−1. To update such a belief, the KF also requires control signals (uk) and the environment observations provided by the sensors (zk). The output of the KF would be the belief in the time instant k, represented by x^k and Pk.Kalman filter stages:(i)
Prediction stage(3)x^k−=Akx^k−1+Buk,(4)Pk−=AkPk−1AH+Qk(ii)
Update stage(5)Kk=Pk−HkHHkPkHkH+Rk−1,(6)x^k=x^k−+Kkzk−Hkx^k−,(7)Pk=I−KkHkPk−Equations (3), (2), (3), (4), and (7) show the steps of the KF algorithm, which are explained in the following:Prediction stage: the state and covariance of the error are projected in the current instantt. It is the sum of instant t−1, which was generated from the instant before the current, to the system.Update stage: observed characteristics are considered in this stage. Using the estimation obtained in the prediction stage, the location of the characteristic can be estimated so the system can be corrected. The addition of new characteristics to the map allows for later reobservation. This can be carried out using information about the current characteristics of the system and adding information about the relationship between new and old characteristics.Defining the matrices that appear in equations (3)–(7):Ak: matrix n×n that relates to the state in instant t−1 and state in instant t, in the absence of control signals.Bk: matrix n×l that relates control signals, optional, to the current state.Qk: matrix n×n that represents the covariance of the process noise, or a constant.Hk: matrix m×n that relates the current state to the environment observations.Rk: matrix m×m that represents the covariance of the observations noise.Kk: matrix n×m that represents the Kalman gain. The Kalman gain indicates the trust in the observed characteristics. To this end, the uncertainty of these observed characteristics together with a measurement of data quality.The Kalman filter has numerous applications in technology. A common application is the guidance, navigation, and control of vehicles, especially aircrafts and exploration robots. In addition, this filter is also widely used in signal processing and econometrics.In other words, the KF combines measurements and predictions to find the optimal state. Figure1 presents an example of how these two characteristics are related, in order to find the position of a vehicle using GNSS for localization. In this figure, an automobile is initially in the x^k−1 position and the function describing this position is given by a function representing some uncertainty, which ensures that most time, the vehicle is in the mean of such a distribution. The first stage of the KF is known as prediction and consists of calculating the a priori x^k− estimated state and the Pk− covariance error. In Figure 1, an increase in uncertainty is observed, which is represented by higher invariance; this increase may be due to the fact that the automobile may have found a pothole on the road or that its tires simply slipped slightly between the x^k−1 and x^k− instants. To obtain the current position of the automobile, complementary information is gathered through the measurements of sensors yk and variance, which is represented by the noise associated with each of these measurements. After calculating the x^k− prediction, the Pk− covariance, and the measurements for yk, the optimal form of calculating the real position of the automobile is by combining both pieces of information (multiplying both functions), which yields a new Gaussian function with a Pk variance lower than previous one due to the use of the Kk Kalman gain. The calculation of this Kalman gain gives the current position of the automobile, as well as the mean of the represented function, in which the automobile x^k should be at instant k that is calculated by the Kalman filter at the update or correction state.Figure 1
Estimation of vehicle position using the Kalman filter.To better understand the operation of this filter, an application example will be presented in the second-order model of a Direct Current (DC) motor like the one in [6]. The continuous-time state variable model of the DC motor is
(8)A=−RmLm−KbLmKtJm−BmJm,B=1Lm0,C=10x=Iaφ,where Ia is the armature’s current and φ is the speed of the DC motor. The motor parameters are represented by an armature resistance of Rm=2.06 ohm, armature inductance of Lm=0.238mH, a Back EMF constant of Kb=0.02352 Vs/rad, a torque constant of Kt=0.0235Nm/A, a rotor inertia of Jm=1.07e−5, and a mechanical damping factor of Bm=12e−7. Calculating values and then substituting them in the continuous-time state variable model, it is obtained that
(9)İaφ̇=−8655.462−98.82521962.616−1.121Iaφ+4201.680u,z=Ia.The continuous-time state (Ia and φ) response in MatLab/Simulink for a 12 V input is shown in Figure 2.Figure 2
Response of armature current and speed of the DC motor when applying 12 V.Considering a sampling time of 0.0001 seconds and using the zero-order hold discretization method, the resulting discrete-time DC motor model is(10)İakφ̇k=0.4146−0.00661.46430.9916Iak−1φk−1+0.28020.3521uk−1.To use the KF, it will be assumed that the armature current in the DC motor was measured with an error of 0.2 A (standard deviation). In addition, in the 12 V input, a noise of 1.1 V (standard) is considered.Now, to obtain theQ and R matrices, the standard deviation of the noise is considered to be 0.2. Therefore, the R matrix will simply correspond to 0.22=0.04. In turn, to obtain Q, since the armature current is proportional to 0.2802 times the voltage applied to the motor, and the noise of applied voltage is 1.1 V, then the noise variance in the armature current will be 0.28022·1.12=0.095, approximately. Additionally, if the speed of the DC motor is proportional to 0.3521 times the voltage applied, the variance in the noise of this speed will be 0.35212·1.12=0.15. Finally, the covariance of the armature current noise and the noise in the speed of the DC motor will equal the standard deviation of the armature current noise times the noise of the DC motor speed, i.e., 0.2802·1.1·0.3521·1.1=0.1194. Afterwards, it is possible to express this as the following matrix:
(11)R=0.4,Q=0.0950.11940.11940.15.Additionally, the influence of the variation inQ matrix (covariance matrix for process noises) on the KF estimation is analyzed, for which a factor—whose initial value is 1—that divides this matrix is included. Subsequently, this factor is substituted for 5, 10, and 100, which improves considerably the estimated state of both armature current and speed of the DC motor, as shown in Figures 3 and 4, respectively. Consequently, when comparing these results with those in Figure 2, an excellent approximation to the ideal result, which corresponds to the real system’s response, is achieved.Figure 3
Effect of the factor that acts onQ in the estimation of armature current.Figure 4
Effect of the factor that acts onQ in the estimation of DC motor speed.Figure5 shows the difference existent between the prediction and correction stages of the armature current of the DC motor.Figure 5
Estimation of armature current during the prediction and correction stages.


### 2.2. Extended Kalman Filter for Nonlinear Systems

As proposed above, Kalman is an estimation method whose parameters are corrected for each iteration, depending on the prediction error of the previous iteration [3]. The algorithm for the extended Kalman filter can be described in the same recursive steps of the linear Kalman filter, i.e., prediction and correction, with the particularity that Taylor linearization is conducted during prediction as shown in the following:
(12)xk=axk−1,uk,wk−1,zk=hxk,vk.Linearizing the process equation and the measurements, we have(13)Ak=∂a∂xx^k−1,uk,0,Wk=∂a∂wx^k−1,uk,0,Hk=∂h∂xx^k−,0,Vk=∂h∂vx^k−,0,x^t=ax^k−1,uk,0,z^t=hx^k,0,where
(14)x^k−=fx^k−1,uk−1,0,Pk−=AkPk−1AKH+VkQk−1VkH,with A and W being the Jacobian matrices of the partial derivatives of function f, and H and V corresponding to the Jacobian matrices of the partial derivatives of function h:
(15)Kk=Pk−HkHHkPk−HkH+VkRVkH−1,x^k=x^k−+Kkzk−hx^k−,0,Pk=I−KkHkPk−.It is well known that the KF assumes a Gaussian distribution. Therefore, if the transition function is linear, the distribution resulting from the application of the linear transformation will preserve the Gaussian characteristics, as shown in Figure6. However, if function fx is nonlinear, the response—after the linear transformation—may be non-Gaussian, and therefore, the KF may not converge with the solution, as shown in Figure 7.Figure 6
Representation of the preservation of the Gaussian features after linear transformation.Figure 7
Representation of lost Gaussian features when functionfx is nonlinear.According to the above, the EKF should be employed to linearize the nonlinear function over the current state mean, as shown in Figure8. In this way, at any time instant, linearization is conducted locally, and the resulting Jacobian matrices are used in the prediction and correction stages. This extension of the Kalman filter delivers good results in state estimation when the nonlinear system is approximated well by linearizing it.Figure 8
Estimation in a nonlinear system.However, the use of the EKF has some inconveniences, such as the following:(i)
Estimation is not optimal if the system is highly linear(ii)
The EKF can only be used in systems that have a differential system(iii)
Jacobians are hard to calculate analytically(iv)
The numerical calculation of Jacobians has higher computational requirementsAfter the analysis of the inconveniences associated with the use of the EKF, the following section presents modifications made on the KF to improve its performance.


## 2.1. Kalman Filter for Lineal Systems

The KF is an algorithm that requires two types of equations: one type links state variables to observable variables (main equations), while the other determines the temporary structure of state variables (state equations).Estimations of state variables are conducted based on the dynamics of these variables (time dimension), as well as on the measurements of observable variables obtained at each time instant (transversal). In other words, the dynamics are summarized in two steps as follows:(i)
Estimation of state variables using their own dynamics (prediction stage)(ii)
Improvement of the first estimation using information from observable variables (correction stage)An attractive characteristic of this methodology is its recursion, which implies that it can be used in real time. Once the algorithm predicts the new state at momentt, it adds a correction term, and the new “corrected” state serves as an initial condition at the following stage, t+1. In this way, the estimation of the state variables uses all the information available up to that moment and not only that of the stage before estimation (this is known as “signal extraction”).Given the linear process represented by(1)xk=Axk−1+Buk+wk−1,and the measurement equation:
(2)zk=Hxk+vk,where the independent, white, Gaussian distributed process noise and measurement noise are pw∼N0,Q, pv∼N0,R.The KF represents a belief or trust in thexk state at time k that is given by the mean, x^k−, and the Pk− covariance. The entry received by KF is the belief in time k−1, represented by x^k−1 and Pk−1. To update such a belief, the KF also requires control signals (uk) and the environment observations provided by the sensors (zk). The output of the KF would be the belief in the time instant k, represented by x^k and Pk.Kalman filter stages:(i)
Prediction stage(3)x^k−=Akx^k−1+Buk,(4)Pk−=AkPk−1AH+Qk(ii)
Update stage(5)Kk=Pk−HkHHkPkHkH+Rk−1,(6)x^k=x^k−+Kkzk−Hkx^k−,(7)Pk=I−KkHkPk−Equations (3), (2), (3), (4), and (7) show the steps of the KF algorithm, which are explained in the following:Prediction stage: the state and covariance of the error are projected in the current instantt. It is the sum of instant t−1, which was generated from the instant before the current, to the system.Update stage: observed characteristics are considered in this stage. Using the estimation obtained in the prediction stage, the location of the characteristic can be estimated so the system can be corrected. The addition of new characteristics to the map allows for later reobservation. This can be carried out using information about the current characteristics of the system and adding information about the relationship between new and old characteristics.Defining the matrices that appear in equations (3)–(7):Ak: matrix n×n that relates to the state in instant t−1 and state in instant t, in the absence of control signals.Bk: matrix n×l that relates control signals, optional, to the current state.Qk: matrix n×n that represents the covariance of the process noise, or a constant.Hk: matrix m×n that relates the current state to the environment observations.Rk: matrix m×m that represents the covariance of the observations noise.Kk: matrix n×m that represents the Kalman gain. The Kalman gain indicates the trust in the observed characteristics. To this end, the uncertainty of these observed characteristics together with a measurement of data quality.The Kalman filter has numerous applications in technology. A common application is the guidance, navigation, and control of vehicles, especially aircrafts and exploration robots. In addition, this filter is also widely used in signal processing and econometrics.In other words, the KF combines measurements and predictions to find the optimal state. Figure1 presents an example of how these two characteristics are related, in order to find the position of a vehicle using GNSS for localization. In this figure, an automobile is initially in the x^k−1 position and the function describing this position is given by a function representing some uncertainty, which ensures that most time, the vehicle is in the mean of such a distribution. The first stage of the KF is known as prediction and consists of calculating the a priori x^k− estimated state and the Pk− covariance error. In Figure 1, an increase in uncertainty is observed, which is represented by higher invariance; this increase may be due to the fact that the automobile may have found a pothole on the road or that its tires simply slipped slightly between the x^k−1 and x^k− instants. To obtain the current position of the automobile, complementary information is gathered through the measurements of sensors yk and variance, which is represented by the noise associated with each of these measurements. After calculating the x^k− prediction, the Pk− covariance, and the measurements for yk, the optimal form of calculating the real position of the automobile is by combining both pieces of information (multiplying both functions), which yields a new Gaussian function with a Pk variance lower than previous one due to the use of the Kk Kalman gain. The calculation of this Kalman gain gives the current position of the automobile, as well as the mean of the represented function, in which the automobile x^k should be at instant k that is calculated by the Kalman filter at the update or correction state.Figure 1
Estimation of vehicle position using the Kalman filter.To better understand the operation of this filter, an application example will be presented in the second-order model of a Direct Current (DC) motor like the one in [6]. The continuous-time state variable model of the DC motor is
(8)A=−RmLm−KbLmKtJm−BmJm,B=1Lm0,C=10x=Iaφ,where Ia is the armature’s current and φ is the speed of the DC motor. The motor parameters are represented by an armature resistance of Rm=2.06 ohm, armature inductance of Lm=0.238mH, a Back EMF constant of Kb=0.02352 Vs/rad, a torque constant of Kt=0.0235Nm/A, a rotor inertia of Jm=1.07e−5, and a mechanical damping factor of Bm=12e−7. Calculating values and then substituting them in the continuous-time state variable model, it is obtained that
(9)İaφ̇=−8655.462−98.82521962.616−1.121Iaφ+4201.680u,z=Ia.The continuous-time state (Ia and φ) response in MatLab/Simulink for a 12 V input is shown in Figure 2.Figure 2
Response of armature current and speed of the DC motor when applying 12 V.Considering a sampling time of 0.0001 seconds and using the zero-order hold discretization method, the resulting discrete-time DC motor model is(10)İakφ̇k=0.4146−0.00661.46430.9916Iak−1φk−1+0.28020.3521uk−1.To use the KF, it will be assumed that the armature current in the DC motor was measured with an error of 0.2 A (standard deviation). In addition, in the 12 V input, a noise of 1.1 V (standard) is considered.Now, to obtain theQ and R matrices, the standard deviation of the noise is considered to be 0.2. Therefore, the R matrix will simply correspond to 0.22=0.04. In turn, to obtain Q, since the armature current is proportional to 0.2802 times the voltage applied to the motor, and the noise of applied voltage is 1.1 V, then the noise variance in the armature current will be 0.28022·1.12=0.095, approximately. Additionally, if the speed of the DC motor is proportional to 0.3521 times the voltage applied, the variance in the noise of this speed will be 0.35212·1.12=0.15. Finally, the covariance of the armature current noise and the noise in the speed of the DC motor will equal the standard deviation of the armature current noise times the noise of the DC motor speed, i.e., 0.2802·1.1·0.3521·1.1=0.1194. Afterwards, it is possible to express this as the following matrix:
(11)R=0.4,Q=0.0950.11940.11940.15.Additionally, the influence of the variation inQ matrix (covariance matrix for process noises) on the KF estimation is analyzed, for which a factor—whose initial value is 1—that divides this matrix is included. Subsequently, this factor is substituted for 5, 10, and 100, which improves considerably the estimated state of both armature current and speed of the DC motor, as shown in Figures 3 and 4, respectively. Consequently, when comparing these results with those in Figure 2, an excellent approximation to the ideal result, which corresponds to the real system’s response, is achieved.Figure 3
Effect of the factor that acts onQ in the estimation of armature current.Figure 4
Effect of the factor that acts onQ in the estimation of DC motor speed.Figure5 shows the difference existent between the prediction and correction stages of the armature current of the DC motor.Figure 5
Estimation of armature current during the prediction and correction stages.


## 2.2. Extended Kalman Filter for Nonlinear Systems

As proposed above, Kalman is an estimation method whose parameters are corrected for each iteration, depending on the prediction error of the previous iteration [3]. The algorithm for the extended Kalman filter can be described in the same recursive steps of the linear Kalman filter, i.e., prediction and correction, with the particularity that Taylor linearization is conducted during prediction as shown in the following:
(12)xk=axk−1,uk,wk−1,zk=hxk,vk.Linearizing the process equation and the measurements, we have(13)Ak=∂a∂xx^k−1,uk,0,Wk=∂a∂wx^k−1,uk,0,Hk=∂h∂xx^k−,0,Vk=∂h∂vx^k−,0,x^t=ax^k−1,uk,0,z^t=hx^k,0,where
(14)x^k−=fx^k−1,uk−1,0,Pk−=AkPk−1AKH+VkQk−1VkH,with A and W being the Jacobian matrices of the partial derivatives of function f, and H and V corresponding to the Jacobian matrices of the partial derivatives of function h:
(15)Kk=Pk−HkHHkPk−HkH+VkRVkH−1,x^k=x^k−+Kkzk−hx^k−,0,Pk=I−KkHkPk−.It is well known that the KF assumes a Gaussian distribution. Therefore, if the transition function is linear, the distribution resulting from the application of the linear transformation will preserve the Gaussian characteristics, as shown in Figure6. However, if function fx is nonlinear, the response—after the linear transformation—may be non-Gaussian, and therefore, the KF may not converge with the solution, as shown in Figure 7.Figure 6
Representation of the preservation of the Gaussian features after linear transformation.Figure 7
Representation of lost Gaussian features when functionfx is nonlinear.According to the above, the EKF should be employed to linearize the nonlinear function over the current state mean, as shown in Figure8. In this way, at any time instant, linearization is conducted locally, and the resulting Jacobian matrices are used in the prediction and correction stages. This extension of the Kalman filter delivers good results in state estimation when the nonlinear system is approximated well by linearizing it.Figure 8
Estimation in a nonlinear system.However, the use of the EKF has some inconveniences, such as the following:(i)
Estimation is not optimal if the system is highly linear(ii)
The EKF can only be used in systems that have a differential system(iii)
Jacobians are hard to calculate analytically(iv)
The numerical calculation of Jacobians has higher computational requirementsAfter the analysis of the inconveniences associated with the use of the EKF, the following section presents modifications made on the KF to improve its performance.


## 3. Kalman Filter Modifications

Since its creation, the Kalman filter has been modified several times in order to provide a better response, solve some operational limitations, and tackle more complex problems, as shown in Table1. There are two main directions for the modifications of this algorithm: using the extended version or employing nonsense transformation. Some modifications are described in the following.Table 1
Modifications to the KF.
KFEKFUKFOthersIEKFAUKFKKFAEKFDUKFCDKFAREKFSRUKFDKFQ-EKFEKF-SLAMOC-EKFUKF-SLAMSO-EKFCDKF-SLAMRI-EKFCV-SLAMK-mean clustering KFSome publications have denominated the EKF Kalman-Schmidt filter due to the contribution of Dr. Schmidt to the development of the extension of the KF. However, the specific Kalman-Schmidt filter was created in 1966 [7]. This filter was developed to reduce the dimensionality of the state estimation without overlooking the effects of the additional state on the calculation of the covariance matrix and Kalman gains. One of the main advantages offered by this modification is reducing computational requirements. In turn, when residual biases are unobservable, i.e., when the effects of residual biases cannot be separated from the measurement, the Kalman-Schmidt filter offers a robust solution by not attempting to estimate the values of these biases but only following their bias effects across the distribution of the real error. Nevertheless, over the years, this modification has been updated to improve its performance under different circumstances.In the engineering field, most of the problems to be solved have nonlinear characteristics. This is one of the major shortcomings of EKF, which performs a linear treatment of a set of equations that are not linear. The mechanism employed by the EKF therefore leads to a series of approximation problems, which are mainly reflected in an inaccurate estimation of the covariance of the real stochastic process [8, 9]. An improvement in this sense is achieved through the use of the UT (Unscented Transform) [10], creating the UKF (Unscented Kalman Filter), which is based on the premise that it is easier to approximate a probability distribution than an arbitrary linear transformation. This modification consists of selecting a group of deterministic points in the state space, called sigma points to capture some of the inherent properties of the distributions to be estimated. To observe the use of this method in a graphic way, Figure 9 shows a simple illustration of a bidimensional system; on the left are the real media and covariance propagation using Monte Carlo sampling; on the center are the results of the EKF linearization approach; on the right is a performance scheme of the new sampling approach, which only requires 5 sigma points.Figure 9
Example of media and covariance propagation using three different approaches.Most physical systems have natural symmetries (or invariance), i.e., there are transformations that leave the system unchanged. From a mathematical and engineering point of view, it makes sense that a well-designed filter for the system under consideration should retain the same invariance properties [11]. The IEKF (Invariant Extended Kalman Filter) is a modification of the EKF equations that takes advantage of the system symmetries [12]. Therefore, the IEKF has the same advantages as the EKF but also retains the invariance properties of the systems. Instead of using a linear correction term based on a linear output error, the IEKF uses a geometrically adapted correction term based on an invariant output error; likewise, the gain matrix is not updated from a linear state error, but from an invariant state error. The main benefit is that the gain and covariance equations converge with constant values over a much larger set of trajectories than equilibrium points; this is the case for the EKF, which results in better estimation convergence. Since its development, the IEKF has been implemented at the industrial level for drone navigation and has proven a solution to a major issue of the EKF when applied to SLAM [13].Different modifications have been conducted to enhance the adaptability of the EKF, for example, its adaptive filtering. This type of filtering is based on the determination of the dynamic system’s statistical parameters according to the system’s behavior during data processing [14, 15]. One of the problems associated with mobile robot localization is error accumulation; to solve or minimize this issue, [15] proposes a variation of the EKF known as AEKF (Adaptive Extended Kalman Filter). This variant employs Taylor’s series in sampling time as an estimator of variable noise over time, and the Sage-Husa method to estimate the observation noise in real time. In this way, the linearization error is corrected and adaptability improves.The KKF (Kinematic Kalman Filter), a modification of the EKF, stands out because of its simplicity in representing the system using the kinematic model. This modified filter exhibits two great advantages: it is an accurate representation of the system states without involving physical parameters and external disturbances, and its kinematic model is very suitable for the Kalman filter theory [16].The square root form of the UKF (SRUKF (Square Root Unscented Kalman Filter)), developed in [17], is the logical replacement for the EKF in state and parameter estimation applications. This variation includes three powerful linear algebra techniques that have excellent numerical properties, namely, QR decomposition, the Cholesky update factor, and efficient least squares. Thanks to these properties, the positive semidefiniteness of the underlying state covariance can be guaranteed, having a lower computational effort than the UKF in parameter estimation [18].Another modification conducted on this filter is the use of the quaternion, which improves the usual linearization procedure and its associated approximation errors. However, the inherent nonlinearity of the quaternion vector measurement is present in the quaternion-dependent noise but has no effect on the filter performance, which led to the creation of the QKF (Quaternion Kalman Filter) [19]. This variation has derived new filters, such as the DQEKF (Dual Quaternion Extended Kalman Filter) and the QVEKF (Quaternion Vector Extended Kalman Filter) presented in [20]; the main difference between these variations is that the relative pose in the latter is expressed by a quaternion-vector pair and in the DQEKF by a dual quaternion.In [21], the algorithm OC-EKF (Observability Constrained Extended Kalman Filter) is introduced. This algorithm was developed to improve the precision and consistency of the inertial navigation system, based on its observability properties. The improvement is made by removing false information along unobservable directions of the estimator.Nørgaard et al. [22] and Ito [23] exposed and proved the central difference filter theory and created the CDKF (Central Difference Kalman Filter). This variation of the Kalman filter uses Sterling’s formula for polynomial interpolation to estimate covariance, and its main advantage is that it spares the calculation of the Jacobian matrix or the linear approximation for the nonlinear model. Other research has suggested using this technique to improve the EKF, which has continued to evolve, leading to the creation of the SOCDKF (Second Order Central Difference Kalman Filter). The latter employs a second-order truncated Stirling interpolation to estimate posterior covariance, so that the approximation accuracy of the posterior mean and autocovariance can reach the first two elements of the UT transform [24].The DEKF (Dual Extended Kalman Filter) is presented in [25]. In this variant, two extended Kalman filters are run simultaneously, one for state estimation and another for weight estimation. By transforming the weighting filter into an observed error form; all developed cost functions are minimized. Overall, the dual EKF algorithm represents a sequential approach that is applicable to both linear and nonlinear models and that can even be used in the presence of white or colored noise measurement. In addition, this algorithm has been extended to provide an estimate of the noise variance parameters, which is crucial in very many applications.Another variation of the KF is carried out in [26] where the algorithm “K-means clustering” is used together with the EKF to decompose the state space of mobile robots. This decomposition is one of the fundamental problems to build a map and move and locate a robot in an unknown environment. The contribution of the authors is that this method takes the values returned by the EKF and passes them onto an algorithm that is capable of creating a group of coordinate sets (x; y), which are the obstacles or invalid map zones, in order to build the map of the environment where the robot moves.The information used by the KF is key for good performance. In robotics, this information originates from the measurements of different sensors equipped in robotic systems. Although the KF and its variants offer an optimal solution for the problems they are applied to, synchronous measurements are required in most cases. This is because the response of the KF can degrade or diverge when incomplete system information or measurements from asynchronous systems are employed. In turn, in most practical applications, theQk and Rk covariance matrices are initially estimated or unknown in many cases; in addition, poor estimations of noise input to the system can affect the performance of the KF. Considering the above, the authors of [27] propose to use the AFEKF (Adaptive Fading Extended Kalman Filter), which included a Ck innovation covariance that is affected at several times by unaccounted errors, or nonmodeled dynamics. Therefore, [28] uses the AFEKF to design a fault-tolerant navigation system since the filter considers the effect of unaccounted errors of the system model. The modification proposed in [27] considers that the measurement equation is not known accurately and thus the estimation error and the innovation covariance may also increase due to the effect of unknown information, as explained above. By not including the errors not accounted for in the dynamic equation, the innovation covariance experiences an increase caused by the augmented measurement covariance instead of the predicted error covariance. However, the effects of excluding errors unaccounted for are compensated in the measurement equation by the reduction of Kk magnitude, which implies less dependence on the measurement information. This variant of the AFEKF is known as AFEKF with Kk rescaling.In [29–31], several works conducted on the KF and that converge in the creation of the DKF (Distributed Kalman Filter) are presented. The DKF is an algorithm composed of a MKF (Micro Kalman Filter) network in which each filter is integrated through a low-pass and a band-pass consensus filter. This MKF network is able to provide a state estimation of the process observed that is identical to the estimation obtained from a central KF as all the nodes converge in two central sums. Consensus filters can approximate these sums and yield an approximated distributed Kalman filter algorithm for the sensor network. The function of consensus filters is to fusion data from the sensor with the covariance obtained from each node. Specifically, [31] presents a modification of the original DKF, as this could not be used in sensors with different nonlinear detection models. Therefore, thanks to this modification, better results are obtained when the analyzed system presents multiple sensors with different nonlinear models or when a robot team or vehicles are employed. In [32], a performance comparison of the DEKF and MCDPF (Markov Chain Distributed Particle Filter) using a vehicle flocking model is conducted, which demonstrates that the DEKF has a RMSE value lower than that of MCDPF for simple systems, i.e., with fewer vehicles or high measurement frequency; however, for complex systems, i.e., with more vehicles or with lower measurement frequency, the resulting MCDPF is more accurate and robust than DEKF.In some practical situations, system models contain parameters that may deviate from their nominal values due to unknown constants or random biases. In turn, unknown random bias can seriously degrade filter performance and even make its performance to diverge. This problem can be solved by using the ASKF (Augmented State Kalman Filter) proposed in [33, 34], where the bias term is included in the state vector. However, the use of ASKF imposes extra computational requirements which translates into an increase in the size of the matrices involved in the filter, causing numerical problems during implementation, especially in ill-conditioned systems [35]. In turn, and related to the contributions to the ASKF above, the authors of [36] made two great advances. First, the system is capable to deal with several loops, and second, the system could dynamically optimize data as new data enter the system as this is a sequential algorithm, instead of waiting for all data to be processed later as in the case of batch filters.To tackle the problems faced by the ASKF, the TEKF (Two-stage Extended Kalman Filter), which expresses optimal estimation as an unbiased filter output and conducts correction with the filter output containing bias, can be employed [37]. Recent studies demonstrate that TEKF is applicable not only under constant bias but also under variable bias [35, 38]. Its use is proposed in nonlinear systems [39].In addition, [40] proposes to use the adaptability property of TEKF. By using an adaptive fading EKF, unknown random biases can be estimated based on the relationship between the covariance of calculated innovation and the covariance of estimated innovation. This proposal was validated in an INS-GPS (Inertial Navigation System-Global Positioning System) loosely coupled system with an unknown fault bias.Since EKF is based on statistical linearization or closing approximation, which is too severe to be useful for all cases, in these opportunities, one of the filters proposed for dealing with strongly nonlinear dynamics is the EnKF (Ensemble Kalman Filter) [41]. The EnKF is a sequential data assimilation method that uses Monte Carlo methods and offers a better alternative to employing the approximate error covariance equation in the EKF, which is extremely demanding in computational terms. This is because integrating a set of model states forward in time makes it possible to calculate the mean and error covariances necessary during analysis. Therefore, the associated numerical calculation implies a computation time shorter than with the EKF, as in general, a limited number of these model states is enough to achieve a reasonable statistical convergence [42]. However, if the state dimension is large, the EnKF is manageable only when the set size is maintained at the minimum, which compensates for computational effort. There have been some other approximations to this filter variant, such as the one introduced in [43], in which the results show that the EnKF can yield estimates of almost equal quality than optimal KF but at a fraction of the computation time.


### 3.1. Comparisons

Due to its widespread use, the KF and its modifications have been compared with one another and with filters that do not belong to this family. If KFs were divided into three big families, these would be represented by the KF, EKF, and UKF. All the modifications derived from them, which can be utilized in different models. Table2 displays the optimal estimations for each of these models. The results presented in this table are supported by diverse studies that measure and compare the performance of all the filters cited in [44–49], among others.Table 2
Comparison between KF and its modifications.
State estimatorModelAssumed distributionComputational effortKalman filterLinealGaussianLowExtended Kalman filterLocally linearGaussianLow or mediumUnscented Kalman filterNonlinearGaussianMediumParticle filterNonlinearNon-GaussianHighIn [50], to solve the same problem, GPF (Gaussian Particle Filter) presents a lower MSE (Mean Square Error); however, its computation time is longer than that of EKF and UKF, as shown in Figure 10. Consequently, generally speaking, using GPF may reduce considerably estimation errors, improving filter accuracy but with longer computation time. However, to achieve an excellent performance without consuming so many resources and computation time, UKF is the best option.Figure 10
Computation time and mean square error [50].Several indicators measure the complexity of an algorithm; one of the most widely employed is FLOP (FLoating point OPerations). This indicator is used in [51] to compare the EKF and UKF algorithms, demonstrating that UKF demands about twice the computing time of an equivalent EKF. The study also proves that the computational complexity of these filters rapidly increases with the size of the system model. Thus, when selecting a filter for a task, an analysis of the resources available needs to be conducted beforehand, including the required computational effort, which can be decisive in small applications.


## 3.1. Comparisons

Due to its widespread use, the KF and its modifications have been compared with one another and with filters that do not belong to this family. If KFs were divided into three big families, these would be represented by the KF, EKF, and UKF. All the modifications derived from them, which can be utilized in different models. Table2 displays the optimal estimations for each of these models. The results presented in this table are supported by diverse studies that measure and compare the performance of all the filters cited in [44–49], among others.Table 2
Comparison between KF and its modifications.
State estimatorModelAssumed distributionComputational effortKalman filterLinealGaussianLowExtended Kalman filterLocally linearGaussianLow or mediumUnscented Kalman filterNonlinearGaussianMediumParticle filterNonlinearNon-GaussianHighIn [50], to solve the same problem, GPF (Gaussian Particle Filter) presents a lower MSE (Mean Square Error); however, its computation time is longer than that of EKF and UKF, as shown in Figure 10. Consequently, generally speaking, using GPF may reduce considerably estimation errors, improving filter accuracy but with longer computation time. However, to achieve an excellent performance without consuming so many resources and computation time, UKF is the best option.Figure 10
Computation time and mean square error [50].Several indicators measure the complexity of an algorithm; one of the most widely employed is FLOP (FLoating point OPerations). This indicator is used in [51] to compare the EKF and UKF algorithms, demonstrating that UKF demands about twice the computing time of an equivalent EKF. The study also proves that the computational complexity of these filters rapidly increases with the size of the system model. Thus, when selecting a filter for a task, an analysis of the resources available needs to be conducted beforehand, including the required computational effort, which can be decisive in small applications.


## 4. Applications of the Kalman Filter in the Robotics Field

As commented above, the first application of the Kalman filter and its extension was conducted in the guided navigation field, specifically in the Apollo space program. Since this moment, the KF and its variants have been used in a wide range of tasks, including all forms of navigation (aerospace, land, and maritime navigation), nuclear plant instrumentation, demographic models, and manufacture, among others. Robotics has been one of the fields that has benefited the most from this filter and its variants in different areas that range from parameter identification and robot control to the autonomous navigation of mobile robots.This algorithm has been used extensively in the field of robotics thanks to the convenience it provides. In [52], it is employed to estimate the angular velocity of a motor in the low-speed range and in the presence of random external disturbances. At low speeds, accuracy is not high, and speed has to be measured with an encoder. This is because the frequency of the latter is proportional to the motor speed and the pulse rate is too low, making the measurement accuracy low [53]. In this work, the KKF was modified by adding a PI controller that will only act before random disturbances of the system that single KKF is not able to reduce. The advantage offered by this KKF variation is that adding proportional action instantly enables continuing the measurement, while the integral action reduces the stationary status error. Despite the excellent results obtained in this specific work, the system has not been tested on other nonlinear systems.Several types of robots employ the KF or one of its subsequent modifications in tasks such as determining the position and orientation of a vehicle at any moment, denominated localization, which is crucial to achieve an autonomous, reliable, and robust navigation. This type of filter is also used to correct the uncertain position of mobile robots. This position is estimated based on a set of markers displayed in a known geometry and combined with the measurements of the inertial sensors [54]. Trajectory or road tracking is another major area of application for this type of filter, as well as object and detection and tracking, which is widely employed as a prediction module for calculating the movement vectors of objects in motion.Table3 presents a summary of the most common applications of the most widespread variations of the KF in several robot types.Table 3
Main filters used in robotics.
FilterWheels and legsAquaticAerialManipulatorsEKFLocalizationLocalizationControlControlPositioningTrajectoryNavigation and guidanceParameter estimationTrajectoryParameter identificationDetectionUKFLocalizationPosition estimationControlParameter estimationPositioningPositioningParameter identificationControlTrajectoryDetectionAEKFLocalizationNavigationNavigation and guidanceControlEstimationLocalizationTrajectory trackingPosition estimationAUKFTrajectory trackingPosition and direction estimationControlAttitude estimationNavigationIEKFState estimation (position and speed)State estimation (position and speed)The work in [55] proposes using contact-aided RI-EKF to estimate the state of a legged robot, as shown in Figure 11. To this end, the filter uses IMU (Inertial Measurement Unit) and contact movement models with corrections made through direct kinematics; consequently, this filter can be used to estimate the state of a robot that has an arbitrary (finite) number of contact points on the static environment. Although the filter is particularly useful for legged robots, the same theory can be applied to manipulator robots as long as the points in [55] are verified. Dynamic simulation is carried out with a biped robot from the Cassie series (Figure 11), comparing the response of the RI-EKF (Right Invariant-Extended Kalman Filter)—which has initialization errors—to the response of the Q-EKF (Quaternion-Extended Kalman Filter), which is widely employed in this type of robots. This simulation can be divided into two parts: in the presence and in the absence of IMU. To compare the convergence properties of both filters, 100 simulations of each filter were conducted using the same measures, noise statistics, initial covariance, and several random initial orientations and speeds. The roll and pitch estimations, as well as the speed estimations (body frame), with the inertial measure unit turned off for both filters, show that the filters converge with the set of initial conditions. However, this seems to indicate that the RI-EKF converges much faster than the Q-EKF. The same is observed when IMU is incorporated; in this case, the RI-EKF converges faster and more consistently than the Q-EKF during the 100 passes. Therefore, the initialization convergence and reliability for tracking displayed by the RI-EKF demonstrate the superiority of the filter proposed in [55].Figure 11
A Cassie-series biped robot is used for both simulation and experimental results. The robot was developed by Agility Robotics and has 20 degrees of freedom, 10 actuators, joint encoders, and an IMU [55].Manipulator robots are increasingly used in industrial processes. Some of the production systems in the field are large scale or need great accuracy to perform a specific task. Therefore, their control is essential, especially in environments with disturbances that affect their correct operation. In [56], the use of AREKF (Adaptive Robust Extended Kalman Filter), which applies Lyapunov’s discontinuous control theory and the EKF, is proposed. The algorithm is used to control a 2-DoF (Degrees of Freedom) manipulator robot, particularly to predict the position and speed of each joint, obtaining a performance superior to that of the EKF alone in the trajectory-tracking task suggested in the study.Regarding the use of adaptative Kalman filters, the simulation conducted in [15] demonstrated that when following a defined trajectory, the AEKF presents less error than the EKF.An interesting approach on the use of different sensors to obtain the position and location of a mobile robot in a specific space, as well as their destination, is conducted in [57]. In this study, a mobile robot is equipped with several types of sensors such as encoder, GNSS, gyroscopes, compasses, and accelerometers. These sensors undergo three different experiments that use only some of them. Afterwards, data is sent to the EKF, and its response is used to track the indicated trajectory. The experiment is divided into groups: in the first one, the EKF uses data obtained from the encoder GNSS and the compass. The second one employs data from the gyroscope, the accelerometer, and the GNSS. The third group integrates all sensors. The experiment is conducted following two trajectories: a circular one and an 8-shaped one. The results show that the more the information about the robot’s position, the better the trajectory tracking. Therefore, the third group presents the best performance, with a mean square error lower than the other groups. A disadvantage of this type of robot is that the method is less accurate to find robot position in environments where a GNSS cannot be employed.Using robot teams has gained prominence in rescue missions and in the military industry, among other fields. There are two main variations to obtain the location and direction of a robot team. In the first one, each robot estimates manually its own position with data from its sensors, for which variants of the EKF have been developed as proposed in this work. The second one has become increasingly popular in the last time due to the fact that each robot estimates its own operation time based on the sensors of the server robot and the information of its team members; thanks to which the robots minimize their operation time. Precisely [58] deals with the localization of a robot team composed of a leader and two followers, using the EKF in two separate stages. In the first one, the Kalman filter is applied to obtain the followers’ position, while in the second one, the leader’s position is calculated, thereby simplifying implementation and reducing computational requirements.One of the problems in navigation or CL (Cooperative Localization) is that the model of the error state system employed in the standard EKF-based CL always has a higher dimensional observable subspace than that of the actual nonlinear CL system. This results in an unwarranted reduction of the EKF covariance estimates in directions of the state space where no information is available, which leads to inconsistency. Considering the above, in [59], an observability-based methodology is developed to build consistent estimators. Two estimators are proposed, namely, OC-EKF 1.0 and OC-EKF 2.0: in OC-EKF 1.0, Jacobians are calculated using the previous state estimates as linearization points, while in OC-EKF 2.0, the linearization points are selected to minimize their expected errors under observability constraints. Simulation and real-world experimental tests prove that both OC-EKFs perform better in terms of accuracy and consistency compared to the standard EKF.In [60], the use of extended Kalman filters to estimate future skeleton trajectories is proposed. In this way, a representation of B-IPOD (Bio-Inspired Predictive Orientation Decomposition) with critical capabilities to reduce noisy skeleton observation data and predict the ongoing activities is achieved. In turn, fractional-order Kalman filters have also been designed for fractional-order linear systems with colored noises, using Tustin’s generating function [61].As commented throughout this work, the Kalman filter has been combined with other techniques such as optimization and fuzzy logic. The study in [62] tests both combinations. First, the authors apply particle swarm optimization to the covariance values for the KF noise using mean square error as a target function in order to obtain an optimized performance of the filter. Second, a new adaptive KF structure is developed that combines disturbance magnitude through fuzzy logic to modify the noise covariance values according to the dynamic behavior of the system. Both methods are used to estimate the attitude of a mobile robot, shown in Figure 12. As a result, optimization improved filtration over convergence in the initial filter by 78%, whereas fuzzy logic increased performance by 10.9% compared to optimization. Both methods are compared against two popular IMU-based algorithms, demonstrating that the proposed methods have robust and even superior performances.Figure 12
Mobile robot [62].In this vein, all variants of the KF have been combined with classic control techniques like PID (Proportional-Integral-Derivative controller). In a study conducted by [63], the design of a control structure for a robot arm with flexible joints is presented. The authors propose closed-loop control with a Proportional Derivative (PD) structure and the EKF as a state estimator.Based on the motor measurements, the output or position is estimated, which minimizes readings from multiple sensors and reduces the effect of external disturbances, increasing accuracy. In this article, the authors also analyze the stability of the system when using the filter. The simulations conducted for the trajectory tracking of the robot arm with flexible joints confirm the superiority of the method proposed by comparing it to a PD controller under the same simulation with RMSE. The methodology proposed presents also better stability when the system is subject to external disturbances, proving that the observer is able to handle external interference in real time.In a recent work by [64], a new online method based on DKF (Discrete Kalman Filter) is presented for the rapid prediction of the RSS metrics in regions unexplored by the robot. This is conducted by employing a localized linear regression for the path loss component by applying differential path loss between RSS measurements at several positions previously visited by the robot. A rapid prediction is achieved, which allows for modest requirements in terms of data and calculations. Additionally, the fading of the shadow is modeled using an empirical spatial variogram. DKF is employed to combine the previous models, filtrate data, and provide spatial extrapolation of RSS. The method uses a set of measurements from two robots, the Tracked UGV and the YouBot, of real indoor and outdoor environments. The results show significant improvements in performance compared with more recent methods such as Kriging interpolation algorithms, GPR (Gaussian Process of Regression), and the LR (Lineal Regression) algorithm. With the implementation of this filter, an average prediction accuracy of 96% was reached for RSS up to 20 meters ahead of the robot trajectory.The study conducted by [65] deals with a navigation method designed to enable an efficient, accurate indoor localization in real time for a mobile robot system. This technology is applicable to IMUs, which consist of gyroscopes, accelerometers, and magnetic vision, in addition to SV (Stereo Vision). First, highly accurate information about the position of the mobile robot can be collected using the fusion algorithm of the Kalman filter for accelerometers, gyroscopes, and magnetometers. Second, the accuracy of the inertial measurement can be optimized using the KF algorithm combined with the artificial vision localization algorithm. The methodology proposed is implemented in the MT-R robot shown in Figure 13. This methodology demonstrates the superiority of IMU/SV over single IMU due to a much lower RMS error.Figure 13
MT-R mobile robot [65].The EKF has also been used for improving the positioning of a MEGV (Magnetic Encoder-type Guided Vehicle), as shown in Figure14, along with the code compensation method [66].Figure 14
Magnet-encoder-type guided vehicle [66].Legged robots are gaining increasing importance due to the advantages they exhibit over wheeled robots in irregular fields, but there are still many challenges related to their control and design. The authors in [67] employ the spring-loaded inverted pendulum dynamic model to analyze biped locomotion. This article uses the Kalman filter to predict the SLIP (Spring Loaded Inverted Pendulum) model based on simulated sensor data by applying the two Kalman filters to the complete model and determine the state of the robot components.The first filter is aimed at determining the attitude of the model body by employing a gyroscope and a control torque applied during stance. The second one is a fusion sensor Kalman filter that uses an accelerometer and a gyroscope on the leg’s pivoting points. The simulations conducted show that in both cases, Kalman filters generate better results. In the first case, the RMS error is much lower than the estimation obtained with the gyroscope alone and with the gyroscope and estimate torque combined. In the second case, the KF was compared with a complementary filter and also performed better, with an RMSE lower than half the complementary filter. In both cases, the robot worked as well as if reading the true states, operating at the desired speed of 2 m/s with a 0.2 m/s deviation.This algorithm has also been implemented for the calibration of a normal sensor like the one in [68], which is used in a robot drill. These authors apply a parameter estimation method based on the EKF and thereby estimate the errors in the robot’s kinematic parameters. To validate the methodology proposed, a simulation is conducted to calibrate a normal sensor and the results are compared to those in the methodology described by [69]. As a result, the proposed calibration method is more accurate than the method in [69]. Furthermore, it can improve the accuracy of the normal surface measurement and satisfy the robot drilling requirements, showing a significant improvement in the reduction of the mean angular deviation and the normal maximum deviation, thus validating the proposed methodology. In addition to the simulation, an experiment is conducted on a KUKA KR210 robot drill, which can be seen in Figure 15, in which the methodology is implemented. The experiment shows that accuracy in the robot’s perpendicularity increases after calibration, which meets the drilling requirements of fields like aeronautics.Figure 15
The robotic drilling system [68].This filter has been employed by the authors of [70] to estimate the unknown state vector of a ballbot-type robot system. The estimated vector is used by an LQR (Lineal Quadratic Regulator) that leads the robot to a vertical position. When these criteria are put into practice to control the robot under study, the use of the EKF and LQR proves effective, as the system is asymptotically stable even with some fictitious uncertainties. In addition, system convergence is analyzed.As proposed in [71], the EKF can be also employed to determine the coordinates for vertices 3-D from multiple images of the robot workspace shown in Figure 16. Applications in driving assistance for advanced systems also use the Kalman filter, as it provides an accurate estimation of a vehicle’s position in a digital roadmap based on the Belief Theory, GNSS, and ABS sensors located in the rear wheel of a car [72].Figure 16
Photograph of the experimental Pioneer 2 mobile robot [71].In this line, several researchers have attempted to demonstrate the superiority of this KF variation over the EKF. In [73], this type of comparison is conducted; both filters are used in a robot system, shown in Figure 17, with real data in order to show the theoretical superiority of the UKF in a real scenario. Additionally, this work departs from the theory that the more measurements, the better the estimation. Therefore, five ultrasonic sensors and an algorithm are used to determine the best sensors for each filter. Simulation is conducted in five different scenarios depending on sensor use. Twenty experiments are conducted, measuring the average error position index and the average estimation error covariance in each of them. As a result, both filters have a similar behavior. This is fundamentally attributed to the fact that the system model is good and not excessively nonlinear, which usually marks a difference between the UKF and the EKF; however, the average estimation error of covariance is much lower than that of the EKF.Figure 17
The Khepera III mobile robot [73].As above mentioned, another application field for the EKF is medicine. For example, the use of flexible needles to access areas of the human body by avoiding obstacles has flourished over the last years. For a flexible needle to reach its target, its position and orientation are necessary [74]. The authors in [74] compare three filters to solve the estimation problem for needle orientation, namely, EKF, UKF, and PF (Particle Filter). Simulations are conducted considering first that the model of the flexible needle is exact; then, a variation of the system during its operation is proposed, and finally, a simulation on the system with non-Gaussian noise is computed. The results from these three simulations indicate that the UKF and PF provide more accurate estimations than the EKF. In addition, the particle filter adapts better to the situation when system noise is non-Gaussian. The EKF has a much faster response than the other filters and is able to follow the state; however, it does not provide accurate results with non-Gaussian noise. In general, the results obtained by the EKF in the estimation of the position and orientation of the flexible needle are worse than those of the other filters.Another type of system that has attracted increased attention these days is AUV (Autonomous Underwater Vehicles), shown in Figure18, which is involved in exploration tasks and complex scenarios that need accurate and robust navigation systems. A recent work by [75] proposes the UKF to estimate the position of this type of system, which has a highly nonlinear dynamic model. The navigation algorithm showed good accuracy for estimating the position and orientation of the vehicle, even when faced with environment disturbances like sea currents, which can strongly affect the accuracy of the navigation system. It must be noted that this is the first time that this methodology is employed online and with an AUV in this type of system; other works have used it offline.Figure 18
Typhoon AUV [75].Other modification to the KF is the QUKF (Quaternion Unscented Kalman Filter), which is used in [76] with inertial/magnetic sensors to follow the trajectory of a human arm, as shown in Figure 19. To obtain the best estimation of the forearm and upper arm, a restriction equation is presented, which is based on the relative speed of the elbow joint according to the inertia sensors located in the two structures above. The proposed algorithm is experimentally assessed, demonstrating the accuracy and good performance of such an algorithm.Figure 19
Attached inertial sensors and optical tracker markers [76].In [18], the authors propose an estimation method for the parameters of a coaxial quadrotor UAV (Unmanned Aerial Vehicle) based on SRUKF. Their results show that a fast and reliable estimation of the model parameters can be obtained with SRUKF. However, the classic method can lead to instability, and thus, the estimation algorithm is modified, becoming more robust from a numerical stability standpoint. In this study, the algorithm tracking properties are presented, which make this method more feasible.Another contribution to this type of aerial vehicle that uses EKF is the one made by [77]. In the latter case, EKF is used to develop a special Euclidian group for the geometric control of a UAV quadrotor, as shown in Figure 20. The estimator proposed considers all the effect pairs between translation and rotation dynamics. The performance of this estimator is presented through numerical examples and is the product of experiments in different scenarios.Figure 20
Hardware development for a quadrotor UAV [77].Another adaptive Kalman filter is the one presented in [78], in which the UKF is employed with the aim of avoiding divergence and further improving the estimation and filtering accuracy. The proposed filter is able to simultaneously estimate the process online and measure the noise covariance and simplify the adaptation algorithm. The proposed AUKF method consists of two main steps: in the first step, the measurement noise covariance matrix is estimated based on the residual covariance matching method, and in the second step, the process noise covariance is adapted using an adaptive scaling factor based on the noise estimation. Simulation comparisons between the proposed RQAUKF and the standard UKF show that the proposed RQAUKF can effectively improve estimation accuracy and stability while improving controller performance.In [79], the authors propose a novel methodology for simultaneously solving trajectory tracking problems and avoiding obstacles in a WMR (Wheels Mobile Robot), as shown in Figure 21, with unknown slipping parameters. Slipping is estimated by an Adaptive UKF, implementing an adaptive adjustment of noise covariances in the estimation process through a covariance match technique within the framework of the Kalman filter. In this work, unified control is developed as a response to the two problems proposed, using the slide estimation response provided by the filer designed. This innovative technique is applied to a WMR following two trajectories, a straight line and a circle, with obstacles. The results of both simulations indicate that this new approach is better than using UKF alone, and that allows for better robot performance after estimation of wheel slipping parameters. Therefore, the unified controller designed, which uses AUKF estimation, presents an excellent performance in tracking control, obstacle avoidance, and robustness against the slipping of unknown wheels.Figure 21
Mobile robot in real experiment [79].While in [80], VBAUKF (Variational Bayesian Adaptive Unscented Kalman Filtering) is researched for indoor localization based on RSSI (Received Signal Strength Indication) in inaccurate processes and measurement noise covariance matrices. First, an inaccurate and slowly varying measurement noise covariance matrix can be estimated by choosing a suitable conjugate prior to distribution for an indoor localization model with inaccurate measurement and process noise covariance matrices. By choosing Wishart’s inverse a priori distribution, the state, predicted error, and measurement noise covariance matrices are inferred separately each time. Second, a parameter optimization algorithm is designed to minimize the VBAUKF localization error until it is less than the threshold set in advance.Robustness is one of the fundamental characteristics of a control system. Therefore, the authors of [81] propose a RUKF- (Robust Unscented Kalman Filter-) type filter based on a multiplicative quaternion error approach for the position estimation of a nanosatellite in the presence of measurement faults. In this work, the global position parameterization is given by a quaternion, while the local attitude error is defined using a generalized three-dimensional attitude representation. The proposed algorithm uses a statistical function that includes measurement residuals to detect measurement faults and then uses an adaptation scheme based on a multiple measurement scaling factor for filter robustness to faulty measurements. It is shown that the proposed algorithm yields a robust and accurate attitude estimation, outperforming other algorithms in the presence of different measurement faults.One of the main EKF-SLAM problems cited in the literature is its inconsistency. This is because the method returns a covariance matrix that is excessively optimistic, which leads to inaccurate estimation [59, 82], i.e., the algorithm overlooks the uncertainty of the estimator, conducing to an excessively stable result. The lack in observability and the poor capacity of the EKF to tackle this issue are considered the root cause of this issue. Therefore, [83] suggests using inconsistency to develop the I-EKF, which prevents the reduction in covariance toward the state space. This variation of the EKF is compared with other filters through a simulation. As a result, the best filter is OC-EKF [59], which is the only method that ensures the adequate dimensions of the nonobservable space when compared with the UKF and the EKF. In this work, researchers highlight that the most important factor when using any of these filters is the adequate linearization of the system.In the same line, several authors deal with the inconsistency of the EKF, yet this is not the only difficulty that arises from its use. Another problem this filter presents is convergence. An analysis of these two last problems is conducted by [84] and its application to the 3D SLAM observation systems. To validate the alleged superiority of this method, a Monte Carlo simulation is conducted with several algorithms, namely, SO (3)-EKF, Robotcentric-EKF, Pseudo-RI-EKF, and SE (3), under different noise levels. The results after conducting RMS and NEES were as expected, confirming the impact of I-EKF on the consistency and accuracy of the estimator.In [85], CDKF is proposed to correct the issues of EKF-SLAM. The simulations conducted, together with the large-scale experiment, show the superiority of the method proposed over EKF-SLAM and UKF-SLAM in terms of accuracy. The consistency of the algorithm is also analyzed, and NEES is measured over 50 Monte Carlo passes per filter, in which since linearization errors do not accumulate, the CDKF-SLAM consistency extends over time.Following the same line of using Kalman filter in SLAM to estimate robot position and then rebuilt its map, [86] proposes the stable CV-SLAM (Ceiling Vision-based Simultaneous Localization and Mapping) technique which uses circles and corners as reference points in a scene and improves process stability by measuring feature saliency strength. This study provides a method that uses different characteristic detection algorithms to search for several key points and then measures the saliency of each point to select the most stable characteristics and generate a hybrid map based on Delaunay triangles between these points. The authors state that the EKF is a fundamental aspect in robot SLAM.In [87], the VR-SLAM (Vision Range sensor-SLAM) algorithm is proposed to combine ultrasonic sensors and a stereo camera very effectively. This combination improves the practical capacity and economic efficiency of SLAM. In particular, the false data association and divergence problem of an algorithm using only ultrasonic sensor is solved; furthermore, the low update rate of SLAM, caused by the computational load and the weakness to illumination changes of an algorithm employing only vision sensor, is overcome. The results of this work show that, with the combination of both techniques that use the EKF-SLAM, correct data association through object recognition and high-frequency update through ultrasonic sensor functions can be achieved. The performance of this algorithm is verified by means of several experiments in real indoor environments.In [88], a robot tracking algorithm in SLAM with the MMUKF (Masreliez-Martin Unscented Kalman Filter) is proposed. This variation is employed to estimate the process noise covariance with an adaptive factor to improve the tracking performance in the MMUKF. Finally, the MMUKF is employed to estimate the positions of robots and landmarks. The proposed algorithm can complete robot tracking with good accuracy and obtain reliable state estimation in SLAM.


## 5. Conclusions

The KF is not influenced by possible structural breaks during estimation. Being a recursive method, it uses the entire history of a series and has the advantage of estimating the stochastic path of the coefficients rather than a deterministic one. The Gauss-Markov theorem enhances the capability of the KF to solve a wide range of statistical inference problems. In this way, the KF stands out due to its ability to predict the state of a model in the past, present, and future, even when the precise nature of the modeled system is unknown.Despite the advantages above, this filter also exhibits some disadvantages, which has led many authors to develop several modifications to it, as presented in this paper. The comparisons presented show the advantages and disadvantages of some KF and KF-derived filters.One of the most widely used variants of the KF is the UKF, which consistently outperforms the EKF in terms of prediction and estimation error, having equal computational complexity for general state-space problems. This is primarily because the EKF can be viewed as a “first-order” approximation and, in turn, the SO-EKF allows the use of an approximation with the second-order terms of the Taylor series. However, these approximations can introduce large errors in the true posterior mean and in the covariance of the transformed (Gaussian) random variable, which can lead to suboptimal performance and sometimes to filter divergence.In SRUKF, square root provides numerical stability and guarantees positive semidefiniteness of state covariances. Other advantages of this square root form of the UKF are the natural ability it offers in trajectory tracking, which allows for its application in nonstationary models.The convergence of the EKF is one of the most discussed problems in the literature. In this regard, it has been shown that when the state estimate is initialized close to the true value, the RI-EKF and Q-EKF have similar and superior performance to the EKF because the linearization of the error dynamics accurately reflects the underlying nonlinear dynamics. Nevertheless, when the state estimate is far from the true value, simulation and experimental results show that the RI-EKF converges considerably faster than the other filters analyzed. Furthermore, when bias estimation is disabled, the invariant error dynamics of the RI-EKF does not depend on the current state estimate. Although this theoretical advantage is lost when bias estimation is turned on, both the simulations are performed, and the experimental results indicate that the RI-EKF has a lower sensitivity to initialization.Compared to the use of KF in SLAM, UKF-SLAM and CDKF-SLAM provide lower estimation errors and slightly tighter uncertainty bounds than the EKF-SLAM approach. This is because in the CDKF, as in the UKF, they provide a more accurate transformation and produce better estimated results than the linear approximation of the nonlinear model. In turn, the IEKF offers the convergence, stability, and robustness that the conventional EKF lacks. Consequently, the IEKF is used in SLAM to solve inconsistency problems.Basically, the success of the methods employed by the conventional Kalman filter depends on the previous knowledge of the characteristics and statistics of the measurement noises and of the same processes in which this filter is used. However, without prior knowledge, the use of adaptive Kalman filter methods is imperative to estimate states.The AEKF provides possible solutions to the problem of error accumulation in mobile robot localization. This filter uses the Taylor series and the Sage-Husa time-varying noise estimator to overcome linearization errors and improve environmental adaptability. Compared with the EKF, it has better localization performance, in addition to good all-round performance in terms of speed and accuracy.The Kalman filter has become one of the main engineering tools used in both linear and nonlinear systems. Over its 60 years of existence, this filter has been employed to solve state estimation problems in several fields, from complex to apparently simple ones, such as system control, robotics, medicine, and economics, among other implementations.With the scientific and technical advances, the implementation of this technique has received contributions and experienced modifications in order to help in the resolution of new problems and improve the performance of the activity addressed. In robotics, the Kalman filter is one of the fundamental pillars in localization, position, trajectory tracking, parameter identification, and also mobile robot control, as shown in this work. The implementation of a navigation system equipped with EKF-SLAM was briefly introduced, which will be further implemented in a hexapod robot.The EKF is widely used in integrated navigation systems, although the EKF cannot effectively track parameters that vary over time, nor unknown parameters. To achieve this, the ATEKF or the TEKF should be employed, as they can remove unknown fault biases caused by accelerometers and gyroscopes, among others, in an effective way.One of the main advantages of this filter is its low computational requirements and easy implementation, as well as its fast convergence and reliability. The last two characteristics have been enhanced through modifications made by researchers over the years, particularly the EKF. In some systems, the Kalman filter is combined with classic or advanced control techniques to improve the performance of such systems. Nowadays, the major contribution to science that Rudolf Kalman made in 1960 acquires great relevance among researchers, not only because of its wide variety of uses but also due to the problems that still need solutions and that are proposed in this work.



---

*Source: 9674015-2021-09-03.xml*