A Strong Tracking Mixed-Degree Cubature Kalman Filter Method and Its Application in a Quadruped Robot

The motion state of a quadruped robot in operation changes constantly. Due to the drift caused by the accumulative error, the function of the inertial measurement unit (IMU) will be limited. Even though multi-sensor fusion technology is adopted, the quadruped robot will lose its ability to respond to state changes after a while because the gain tends to be constant. To solve this problem, this paper proposes a strong tracking mixed-degree cubature Kalman filter (STMCKF) method. According to system characteristics of the quadruped robot, this method makes fusion estimation of forward kinematics and IMU track. The combination mode of traditional strong tracking cubature Kalman filter (TSTCKF) and strong tracking is improved through demonstration. A new method for calculating fading factor matrix is proposed, which reduces sampling times from three to one, saving significantly calculation time. At the same time, the state estimation accuracy is improved from the third-degree accuracy of Taylor series expansion to fifth-degree accuracy. The proposed algorithm can automatically switch the working mode according to real-time supervision of the motion state and greatly improve the state estimation performance of quadruped robot system, exhibiting strong robustness and excellent real-time performance. Finally, a comparative study of STMCKF and the extended Kalman filter (EKF) that is commonly used in quadruped robot system is carried out. Results show that the method of STMCKF has high estimation accuracy and reliable ability to cope with sudden changes, without significantly increasing the calculation time, indicating the correctness of the algorithm and its great application value in quadruped robot system.


Introduction
A legged robot has broad application prospects owing to its strong ability to pass through complex ground without continuous support. To let a quadruped robot gain strong autonomous motion capability, the state estimation of a quadruped robot is a challenge, that is, the robot needs to accurately estimate its state information (position, velocity, attitude) before performing each action, and only in this way can the right decisions be made for the next motion. The inertial measurement unit (IMU) can calculate the state information through the built-in accelerometer and gyroscope, which has been used in almost all mobile robots due to its advantages of good concealment, high short-term accuracy and less susceptibility to environmental interference. However, a long-term use of IMU will cause accumulated error, leading to a drift of calculation results. Some errors are unavoidable in IMU of various levels, which need to be addressed by additional correction measures. Multi-sensor fusion combined with strong tracking are basically based on SFEKF combination, and EKF does not need sampling point calculation, but UKF and CKF and some improved algorithms require sampling point calculation, which makes sampling times increase from two to three for each filtering process, which significantly increases the calculation amount and thereby hider the application of quadruped robots.
In order to solve the problem in the above analyses, this paper proposes a strong tracking mixed-degree cubature Kalman filter (STMCKF) and conducts research from the perspective of bionics. According to the structural characteristics of the quadruped robot, the forward kinematics dead reckoning of the robot is obtained in real time through the linear displacement sensor, and the results of the forward kinematics dead reckoning with the IMU are filtered through the fusion of SMSRCKF, and then fusion results are used to correct the IMU. By demonstrating correct combination of strong tracking and CKF-type algorithms, the calculation method of fading factor matrix is improved, and sampling times per filter of TSTCKF is reduced from three to one. Since no additional external sensors are added, external environmental interference is avoided. In this way, the quadruped robot system using this algorithm can significantly improve state estimation accuracy, real-time performance and robustness without increasing research and development costs. The improved STMCKF has fifth-degree accuracy, strong robustness and good real-time performance, which only needs one sampling point calculation.

Strong Tracking Mixed-Degree Cubature Kalman Filter (STMCKF)
Considering the high complexity of quadruped robot system, it is difficult to establish a system model with sufficient accuracy. In fact, the advantages of a quadruped robot lie in flexibility and strong trafficability because of which quadruped robots often face sudden changes of state. All filters designed with a Kalman filter (KF) as the basic framework, including MSSRCKF, are likely to deteriorate in estimation performance, and even filter divergence may be resulted, which is catastrophic situation for the motion control of quadruped robots. Therefore, it is necessary to adopt strong tracking theory to cope with sudden state changes. The following non-linear time-varying system with additive noise is considered: x k = f k−1 (x k−1 ) + µ k−1 (1) where x k ∈ R n is state vector of the system at the moment k, z k ∈ R m is observation vector of the system at the moment k; f k−1 (·) and h k (·) are, respectively state equation and observation equation of the time-varying system. µ k−1 ∈ R n is the system process noise, and η k ∈ R m is the system observation noise. They are zero-mean Gaussian white noises that are independent of each other. The variances are Q k−1 and R k respectively. The filtering process of TSTCKF can be described as follows. First, volume point calculation is performed, and then the state predictionx k/k−1 and prediction covariance matrix P k/k−1 are calculated; the second volume point calculation is performed, and then the observation covariance matrix P zz,k/k−1 and cross covariance matrix P xz,k/k−1 are calculated without introducing fading factor. On this basis, the fading factor λ k is calculated, and the new P * k/k−1 after introduction of the fading factor is calculated. Next, * means introduction of the fading factor.
where λ k ≥ 1. F k−1 is the Jacobian matrix of the state function f k−1 (x k−1 ) atx k−1 ; P k−1 is the covariance matrix of the state estimate. Perform the third volume point calculation, then calculate the observation covariance matrix P * zz,k/k−1 and cross covariance matrix P * xz,k/k−1 after introduction of the fading factor, and then calculate the filtering gain K * k , state estimatex * k/k and estimated covariance matrix P * k/k , thus completing all filtering algorithms. By using the above traditional method, each strong tracking filtering process requires three volume point calculations, so the calculation amount is seriously increased, which is not conducive to practical applications in engineering. For non-linear systems with high dimensionality of state variables or observation variables, this problem is more prominent. In addition, this algorithm not only increases calculation amount, but also destroys optimality of the algorithm. The core of strong tracking filtering theory is that observational innovation meets the principle of orthogonality. The principle of orthogonality means that on the premise of completely accurate system model, the innovation sequence output by Kalman filter is a Gaussian white noise sequence with zero mean, so that these innovation sequences are orthogonal everywhere. To ensure that the algorithm's optimality and orthogonality principles are both met, the combination of strong tracking and MSSRCKF should meet the following conditions: tr E[(z k+ j −ẑ k+ j )(z k −ẑ k ) T ] = tr E z k+ j z T k = 0 (5) where k and j are both positive integers; and z k is observational innovation. Formula (4) indicates smallest variance of the filtering; Formula (5) indicates that the residual sequences output by the filters at different times are mutually orthogonal. Based on the principle of orthogonality, we re-derived the sufficient conditions for the combination of strong tracking and MSSRCKF, and found that the previous combination of STF and non-linear filtering may be incorrect. It is very embarrassing to decide whether to perform compensation for fading factor when calculating the state estimationx * k/k . No matter the innovation z k without introduction of fading factor is used, or innovation z * k with introduction of fading factor is used, the sufficient conditions for strong tracking filtering to be established cannot be met (see Appendix A for detailed demonstration).
It can be seen from the above analysis that the introduction of fading factor λ k in the prediction covariance matrix P k/k−1 is obviously inappropriate, which is a suboptimal estimation. According to the standard CKF formula, it can be seen that by introducing the fading factor to the observation covariance matrix P zz,k/k−1 and the cross covariance matrix P xz,k/k−1 , an equivalent strong tracking effect can be obtained. At the same time, it can ensure that the estimation result is unbiased, that is, optimal estimation. Moreover, the correct combination of ST and MSSRCKF can be realized by derivation (see Appendix B for details).
The state equation is a 9-dimensional quadruped robot system. The state error of a single channel requires the same compensation from all other channels, which is obviously not scientific. It is more reasonable to form a fading factor matrix with fading factor to achieve one-to-one correspondence with each state channel and adopt individual compensation for each channel.
The fading factor matrix is designed as follows: where α i ≥ 1, λ i k is the element of the fading factor matrix; the calculation of N k and M k is shown in Appendix B. M ii k is the element on the diagonal of the matrix M k ; α i value is obtained by RBF neural network training, which can also be obtained by prior knowledge. Multiple fading factors can adjust different state variables with different fading speeds, which fully reflects the dynamic change of internal state of the entire filter and improves adaptive ability of the filter. When α i = 1, the fading factor matrix is equivalent to a single fading factor.
It can be seen that fading factor plays a role of adaptive adjustment according to the changes in the residual, which avoids the majority of calculation process involving volume point, thus greatly reducing the calculation burden. It can be seen that when the system state does not change suddenly or the system model is accurate, the above formula is established.
However, when the state changes suddenly or the model is inaccurate, the innovation will inevitably increase, so that orthogonality is no longer met. That is, formula (4) is not established. At the same time, V k increases, the filtering result is no longer optimal.
To ensure filtering optimality, the innovation sequence is forced to be orthogonal, so as to extract as much valid information as possible from the measurement innovation. Therefore, if a single fading factor λ k ≥ 1 is introduced into the variance matrix P k/k−1 of state prediction error, then It can be seen that when the model is accurate or there is no sudden change in the state, that is λ k = 1, strong tracking filter becomes invalid and degenerates to the original filter.
For the sake of reducing computation, a determination of whether strong tracing is needed. The strong tracking filter should play a role when needed, and strong tracking function is unnecessary when there is no sudden system change. In this way, a filtering convergence criterion is proposed herein: if the system state changes suddenly, the actual estimation error is usually larger than the prediction error, and the error covariance matrix can be approximately considered as unbounded. This modeling indicates quadratic sum of the innovation sequence z T k z k includes the actual estimation error. According to this feature, filtering convergence criteria is constructed based on the nature of the innovation sequence.
where formula (10) is a criterion for judging whether the filter is converged or not. α ≥ 1 means adjustable coefficient. If the formula (10) is established, it means that the actual filtering error exceeds the theoretical prediction error by ε times, and the filter diverges. At this time, strong tracking is required to give full play to its observation role to suppress the filter divergence. On the contrary, if the formula (10) is not established, it means that the filter works normally. At this stage, it is possible to estimate the state variable with MSSRCKF alone without strong tracking. STMCKF is described in detail below. It is specifically stated that there are two versions of this method, and the following version is universal. STMCKF in Section 3 is a simplified version to reduce calculation amount, which is suitable for systems that can linearize the observation equation. Since the main research object of this paper is the quadruped robot, the simulation and experiment are carried out for the quadruped robot, and other systems will not be discussed.

Initialization
Assume that state estimationx k−1 and state variance matrix P k−1 at the moment k − 1 are known, take a set of vectors a i = [a i,1 , a i,2 , . . . , a i,n ] T , i = 1, 2, . . . , n + 1, where, n is the state dimension.
Volume point of the filter is indicated as follows: Sensors 2020, 20, 2251 where [ ] i means n -dimension point set obtained by calculation, the number of points is n + 1, and the weight corresponding to the volume point is:

Predication
We need calculate ξ i according to formulas (11) and (12); singular value decomposition (SVD) is a decomposition method applicable to any matrix. It has a computational complexity equivalent to Cholesky decomposition, but avoids error in the filtering process caused by non-positive definiteness of mean square error matrix in one-step prediction after multiple cycles, thus improving numerical calculation robustness while enhancing filtering accuracy. As a result, SVD is adopted for the calculation of volume points.
SVD of the state variance P k−1 at the moment k − 1 is performed: Calculate the first volume point X i,k−1 , i = 0, 1, . . . , 2n + 2: where S k−1 = diag(s 1, s 2 , . . . , s r , 0, . . . , 0) , s 1 > s 2 > . . . > s r > 0 is the singular value of the matrix P k−1 ; The first volume point spread: Calculate weight ω i of each volume point according to formula (13), and then calculate the one-step predicted value of the state:x Calculate one-step prediction covariance matrix of the state: If the observation equation is a non-linear function, then a second volume point sampling is needed and fading factor matrix should be calculated.

Pseudo Observation Update
Since the observation equation is changed into the pseudo observation equation, the linear equation H k satisfies the following equations: Sensors 2020, 20, 2251 8 of 25

State Mutation Test
When a sudden change in the robot's state is detected, STMCKF initiates strong tracking mode. After improvement, the number of sampling point calculation in TSTCKF can be reduced from three to one, and a new multiple fading factor matrix is given, thus achieving unbiased estimation; when there is no sudden state change, STMCKF initiates normal mode without adopting strong tracking. Use formula (10) to check whether strong tracking is needed.

Calculate Multiple Fading Factors
Calculate the fading factor matrix according to formula (6).

Recalculation
Recalculate one-step prediction variance matrix P * k/k−1 and the new root mean square S * k/k−1 after adding the fading factor. * means after adding the fading factor.
In the formula, since the modified observation equation has m * n dimensional matrix, the fading factor matrix takes the square matrix composed of the previous m dimensions to perform channel compensation.

Update
The third volume point sampling is no longer needed, and the state estimate and variance are directly calculated: For systems where equation of state is non-linear, Traditional Strong Tracking CKF (TSTCKF) requires three volume point calculations for each filtering, but STMCKF only needs one volume point calculation for each filtering.

Forward Kinematics of Quadruped Robot
Years of research work on mammals and arthropods has shown that the animal's own movement and position can be effectively estimated based on its own inertial information and joint information. For a quadruped robot, a link coordinate system is established according to its leg structure. By calculating forward kinematics of the robot, motion parameters of the robot can be obtained simply and conveniently.
The IMU used in this work is a NAV440CA-200 developed by MEMSIC (MEMSIC, Wuxi, China), which is a cheap civilian IMU, costing about $1500. To improve its accuracy and suppress the drift, this paper adopts linear displacement sensors of each joint of the quadruped robot to calculate the joint angle, and calculates the displacement of each foot relative to the body through positive kinematics so as to obtain the position and velocity of the legged robot and provide redundant calibration information for the IMU. The IMU itself has very accurate pitch angle and roll angle. It is difficult to obtain yaw angle through the combination navigation of positive kinematics and IMU, and other auxiliary sensors are needed for correction.
The three-dimensional model of the quadruped robot studied in his paper is shown in Figure 1. Each leg of the robot includes five joints: side swing, forward side-sway hip joint, front hip joint,  Figure 2 shows the definition of the navigation coordinate system N and the body coordinate system B. It is worth noting that IMU placement position has been accurately fixed at the center of mass of the quadruped robot in the design phase. Without consideration of external load design, the lever arm error is ignored here.    Figure 2 shows the definition of the navigation coordinate system N and the body coordinate system B. It is worth noting that IMU placement position has been accurately fixed at the center of mass of the quadruped robot in the design phase. Without consideration of external load design, the lever arm error is ignored here.  Figure 2 shows the definition of the navigation coordinate system N and the body coordinate system B. It is worth noting that IMU placement position has been accurately fixed at the center of mass of the quadruped robot in the design phase. Without consideration of external load design, the lever arm error is ignored here.
sin cos cos sin sin l The direction cosine matrix C n b from N to B can be calculated according to the yaw angle ψ around the Z axis, the pitch angle θ around the Y axis and the roll angle ϕ around the X axis, as shown in Equation (26).
The quadruped robot adopts ankle joint opposite vertex design, and its four legs are symmetrically arranged, so forward kinematic analysis is made only with the right front leg support ground as an example. Since the displacement of the passive linear elastic link is small and not easy to measure, it has little influence on the kinematics calculation. Therefore, the robot's leg linkage coordinate system is ignored. According to the establishment principle of DH joint coordinate system, the coordinate system of the link from the robot center of mass to the foot end is obtained, as shown in Figure 3. According to the joint link coordinate system, a D-H parameter table from side hip joint to the foot end is established, as shown in Table 1. The quadruped robot adopts ankle joint opposite vertex design, and its four legs are symmetrically arranged, so forward kinematic analysis is made only with the right front leg support ground as an example. Since the displacement of the passive linear elastic link is small and not easy to measure, it has little influence on the kinematics calculation. Therefore, the robot's leg linkage coordinate system is ignored. According to the establishment principle of DH joint coordinate system, the coordinate system of the link from the robot center of mass to the foot end is obtained, as shown in Figure 3. According to the joint link coordinate system, a D-H parameter table from side hip joint to the foot end is established, as shown in Table 1.
Where the connecting rod angles are transverse joint angle θ 1 , hip joint angle θ 2 , knee joint angle θ 3 and ankle joint angle θ 4 , which can be calculated in accordance with the leg structure and hydraulic cylinder configuration. By calculating the homogeneous transform matrix A 10 ∼ A 43 between adjacent joints, the homogeneous transform matrix A 0B (front hip joint coordinate system relative to body coordinate system), and the homogeneous transform matrix A BN (body coordinate system relative to navigation coordinate system), as shown in the equation.
The homogeneous transform matrix A 4N (foot coordinate system relative to navigation coordinate system) can be obtained, as shown in Equation (31).
In calculation of kinematic velocity of mass center, the arithmetic product of joint angular velocity and the Jacobian matrix is avoided. In the moving process of a quadruped robot, serious shocks will occur, and spaces will exist in leg joints, causing errors of displacement sensor data. Therefore, directly using differential to calculate angular velocity will enlarge calculation errors.
The specific method is to calculate robot body displacement distance for every 10 sampling periods (sampling frequency is over 200 Hz). The average velocity can be gained, as shown in Equation (32). This method can remove the impact of shocking vibration and effectively improve the calculation accuracy. Considering the real-time performance, 0.05 s of lag is within the permissible range.
where T is the sampling periods. v is the average locomotion velocity within 10 sampling periods. ∆s is the robot body displacement within 10 sampling periods.

Equation of State for Quadruped Robot
Forward kinematics of the quadruped robot is used to correct the position and velocity innovation of the IMU, which can make full use of high response rate and high accuracy of the IMU. Moreover, long-term drift of the IMU can be corrected via positive kinematics solution. The IMU has very accurate pitch and roll angles. When the quadruped robot is walking in dynamic gait, that is, when it is landing on one or two feet, it is impossible to determine its attitude angle only using positive kinematics. We need to estimate the position and velocity of the quadruped robot. To dynamically compensate the deviation of the accelerometer, the deviation values of the accelerometers on the three axes in the IMU are extended to the state variables of the filter. In this way, 9 state variables are needed. Here, it is worth noting that, the north direction of the IMU is consistent with the robot's forward direction, the east direction of the IMU points horizontally to the right side of the robot, and the vertically downward direction of the IMU points to the bottom of the robot.
To realize dynamic bias compensation, strapdown inertial navigation system (SINS) bias as state variable is introduced into the state equation in this work. The outputs of accelerator and gyroscope include bias and noise. As shown in Equation (33), the derivative of bias and random error is regarded as the Gaussian noise.
where Q a , Q ba represent the variance of random errors, a is the real value of the accelerator,â is the measured value of the accelerator, b a is the bias of accelerator, w a is the random error of the accelerator and w ba is the random error of accelerator bias. The differential equation of the state variable should be established before deriving the state equation. The velocity updating differential equation of SINS is expressed as Equation (34): where v is the robot velocity in navigation coordinate system, ω n e is the projection of earth rotational angular velocity in navigation coordinate system, ω is the projection of angular velocity of body coordinate relatively to earth coordinate in navigation coordinate system. g n represents the acceleration of gravity in the navigation coordinate system.
In Equation (34), (2ω n e + ω)v represents the Coriolis acceleration term and centripetal acceleration term, which can be regarded as harmful acceleration during velocity calculation. For mobile robot, the moving velocity is normally less than 2 m/s, the order of magnitude for ω n e , ω is only 10 −4 . It can be known that the magnitude of coriolis acceleration is 10 −5 and the magnitude of centripetal acceleration is only 10 −7 , which is negligible in velocity calculation. The velocity upgrading differential equation of SINS can be simplified as Equation (35). Subsequently, the integral operation is performed using the Runge-Kutta method, so that the real time velocity variation can be obtained.
where C n b a + g n is the accelerometer measurement value, which can also be written as C n b (â − b a − w a ) + g n . As C n b is time-varying, the discrete recursive form of each state component can be written as follows: where s k ∈ R 3×1 represents the position of the robot in the navigation coordinate system at the moment k; v k ∈ R 3×1 represents the speed of the robot in the navigation coordinate system at the moment k; b a,k ∈ R 3×1 represents the deviation of the accelerometer in the navigation coordinate system at the moment k; t is the sampling time, C n b,k−1 is the direction cosine matrix at the moment k − 1. It is written in matrix form as: However, the magnitudes of these parameters differ significantly from each other. The error values of displacement, velocity, accelerator bias are regarded as state variables, which can be expressed by Equation (40): where ∆s k is the error value of displacement at the moment k, ∆v k is the error value of velocity at the moment k, ∆b a,k is the error value of accelerator bias at the moment k.
Take the error values of position, speed and deviation as state variables, then: So the equation of state (41) can be rewritten as: If the variance of ∆w a,k−1 is Q a,k−1 , the variance of ∆w ba,k−1 is Q b,k−1 , the process noise variance is:

Pseudo-Observation Equation of Quadruped Robot
Previously, the displacement s f and moving velocity v f can be obtained according to robot forward kinematics analysis, here the two parameters are adopted as measurement variables, based on which the measurement model and the other two parameters (the displacement after correction s c and the velocity after correction v c can be established shown as Equation (46) and simply denoted as Equation (47).
For the quadruped robot system, the observation equation is linearized. Based on this, STMCKF is proposed. Only one volume point calculation is needed for each filtering, which greatly reduces the calculation amount and improves real-time performance.
where η s,k and η v,k are uncorrelated white Gaussian noises of variance R s and R v , respectively. The above equation can be written in the form of Equation (47).
For the quadruped robot system herein, H ∈ R 6×9 , the observation noise covariance is:

Multi-Sensor Fusion Structure Diagram
The multi-sensor fusion structure diagram is as shown in Figure 4: where s i,k , v i,k and a i,k are navigation calculation results at the moment k. ∆ŝ k , ∆v k and ∆b a,k are estimations of displacement and velocity errors at the moment k. s c,k , v c,k and a c,k are position, velocity, and acceleration results of the robot in the navigation coordinate system after correction at the moment k.
For the quadruped robot system, the observation equation is linearized. Based on this, STMCKF is proposed. Only one volume point calculation is needed for each filtering, which greatly reduces the calculation amount and improves real-time performance.   For the quadruped robot system herein, 9 6× ∈ R H , the observation noise covariance is:

Multi-Sensor Fusion Structure Diagram
The multi-sensor fusion structure diagram is as shown in Figure 4:   The velocity v b,k of the robot body coordinate system may be needed for control. It can be obtained according to formula (51).

Numerical Experiments
The state equation (42) and observation equation (47) Figure 5. In the North-East moving trajectory of quadruped robot simulated by Monte Carlo, the solid line represents the real position moving track, the red dot line represents the position moving track estimated by STMCKF, the green cross line represents the position moving track estimated by CKF, and the black dotted line represents the position moving track estimated by EKF. In order to test the ability of the algorithm to deal with the sudden change of the motion state, some extreme conditions are set artificially in the simulation. From the starting point, the robot accelerates uniformly in the first 50 samples, and then decelerates and turns eastward. After that, it accelerates and moves eastward for a certain distance before decelerating to zero and turning southward. In the North-East moving trajectory of quadruped robot simulated by Monte Carlo, the solid line represents the real position moving track, the red dot line represents the position moving track estimated by STMCKF, the green cross line represents the position moving track estimated by CKF, and the black dotted line represents the position moving track estimated by EKF. In order to test the ability of the algorithm to deal with the sudden change of the motion state, some extreme conditions are set artificially in the simulation. From the starting point, the robot accelerates uniformly in the first 50 samples, and then decelerates and turns eastward. After that, it accelerates and moves eastward for a certain distance before decelerating to zero and turning southward. Subsequently, it accelerates again and turns westward for an emergency stop before accelerating again. Finally it decelerates gradually to point A. It can be seen that the estimated position trajectory of STMCKF is very close to the real position trajectory, which verifies the correctness and effectiveness of STMCKF. In the process of the robot suddenly changing motion states at high speed, STMCKF converges rapidly, with only a few sampling periods and a high tracking accuracy maintained, which indicates that STMCKF has a strong robustness. In the stable acceleration stage, the position estimation of CKF and EKF is accurate, but the estimated trajectory starts to deviate from the real trajectory seriously when the robot turns from the lateral acceleration. Finally, the robot is located at point B, which is far from the real position. There are two main reasons for this result: one is the poor robustness of CKF and EKF, and second is the process noise of the quadruped robot system, which leads to the performance degradation of CKF and EKF estimation. It can be seen from the figure that the curves of CKF and EKF are very close, which is mainly due to the nonlinear degree of the state equation of the system is low and EKF has equal estimation ability as CKF. The estimation accuracy of EKF is lower than that of CKF when the degree of weak non-linearity is enhanced. The red full line represents the velocity estimation, the short blue dash line represents the forward kinematics calculation value, the black dot dash line represents the results of navigation velocity calculation, and the long green dashed line represents the real motion parameter. It can be seen from the figure that due to the existence of deviation, a fast drift occurs in velocity calculation. Under the impact of linear displacement sensor noise, the error of forward kinematics calculation is relatively large. However, after data fusion the velocity estimation is very close to its real value. Analytic calculation of error between estimation of velocity value and real velocity value is as follows: Figure 6b shows the RMSE of the robot forward velocity estimated by these three filters. In Figure 6a, the solid line represents the true value, the dotted line represents the estimated value of STMCKF, the cross line represents the estimated value of CKF, the dotted line represents the estimated value of EKF, the horizontal axis represents the number of the sampling, the sampling interval is 0.05 s, and the vertical axis represents the forward velocity. It can be observed that the peak forward velocity reaches 25 m/s. This study simulates the harsh working conditions of high-speed motion, large change of acceleration and long sampling interval. Such large sampling intervals present a great challenge to state estimation for systems whose states change frequently. It can be seen from Figure 6a that the estimated curve of STMCKF tracks the real value well. Although the curve lags behind due to the influence of harsh working conditions, the overall estimated result is relatively accurate. The estimation curves of CKF and EKF are similar, indicating that the system is a weak non-linear system and has little impact on the estimation performance of EKF. However, both of them are affected by bad working conditions, and the estimation curves lag significantly. Nevertheless, we can see that the general trend is close to the real value, which shows the correctness of the two. By comparing the estimation curves of three kinds of filters, the superiority and robustness of STMCKF can be proved preliminarily. From Figure 6b, it can be seen that the RMSE of the solid line representing STMCKF is far lower than that of the circle point representing CKF and that of the dotted line representing EKF, indicating that the estimation accuracy of STMCKF is much higher than the other two. From the RMSE curves of CKF and EKF, it can be seen that each sudden change of the robot's motion state makes RMSE increase rapidly, and the larger the robot's speed is, the larger the RMSE is, and the smaller the robot's speed is, the smaller the RMSE is. The RMSE curve of STMCKF can be divided into six sections of 0~50, 50~100, 100~150, 150~190, 190~230, and 230~300, which is completely consistent with the range of motion state change in the simulation, indicating that STMCKF can accurately identify the change of motion state. From the sampling interval of 0~50 and 150~190, it can be seen that the RMSE of STMCKF increases slightly upon the sudden change of the motion state, and does not increase continuously like CKF and EKF. The acceleration of the robot is increasing, but the RMSE of STMCKF shows an obvious downward trend, which shows that STMCKF can recognize in a timely way the change of the motion state, start strong tracking to resist so as to improve its own robustness. It can be clearly seen that 50 and 190 are turning points for emergency stop and turning, while CKF and EKF have a lag of about 10 sampling cycles. The forward speed of the robot decreases rapidly and then turns to an increase. This process can be seen from the RMSE curve of CKF and EKF, but not seen so obviously from the RMSE curve of STMCKF, indicating that STMCKF has a certain inhibition effect on the adverse effects of the sudden change of the robot's motion state. From the range of 100~150 and 230~300, it can be seen that STMCKF has higher estimation accuracy when the robot's motion state changes slowly, which is in line with our expectation. This proves the correctness of the simulation experiment and shows the superiority of STMCKF compared with CKF and EKF. on the adverse effects of the sudden change of the robot's motion state. From the range of 100~150 and 230~300, it can be seen that STMCKF has higher estimation accuracy when the robot's motion state changes slowly, which is in line with our expectation. This proves the correctness of the simulation experiment and shows the superiority of STMCKF compared with CKF and EKF.   Figure 7(b) shows the RMSE curve of the robot's lateral velocity estimated by three filters. As shown in Figure 7(a), the solid line represents the true value, the dotted line represents the estimated value of STMCKF, the cross line represents the estimated value of CKF, the dashed line represents the estimated value of EKF, the horizontal coordinate represents the sampling times, the sampling interval is 0.05 s, and the vertical coordinate represents the forward velocity. It can be seen that the peak lateral velocity reaches -27.2 m/s, which is due to the simulation of high-speed movement of the body and large change in acceleration. This extreme situation has exceeded the range of motion ability of foot-type robot. In addition, the sampling interval is long, which poses a great challenge to state estimation of the system with a drastic change of motion state. It can be seen from Figure 7(a) that the estimated curve of STMCKF tracks the real value well. The estimation error is obvious at the 150th and 232nd sampling, and there is a slight lag in the whole curve, which is mainly due to the large interval sampling and is difficult to overcome completely. It can also be said that the estimated curve is close to the real value on the whole and the estimated result is accurate. The estimation curves of CKF and EKF are similar, indicating that the system belongs to a weak non-linear system. In this system, the estimation performance of EKF and CKF has little difference, but both of them are affected by bad working conditions, and the estimation curve lags significantly. However, it can be seen that the overall trend is close to the real value, which indicates the correctness of the two filters in the simulation. By comparing the estimation curves of three kinds of filters, the superiority and robustness of STMCKF can be preliminarily proved. From Figure7b, it can be seen that in the first 50 sampling periods, all three filters have high accuracy, this is because the robot only has forward acceleration and the lateral velocity is almost 0 during this period. The solid line representing the STMCKF shows the lateral acceleration applied at 50, 100, and 230, and the RMSE of the three  Figure 7a shows the lateral velocity estimation curves of the three filters. Figure 7b shows the RMSE curve of the robot's lateral velocity estimated by three filters. As shown in Figure 7a, the solid line represents the true value, the dotted line represents the estimated value of STMCKF, the cross line represents the estimated value of CKF, the dashed line represents the estimated value of EKF, the horizontal coordinate represents the sampling times, the sampling interval is 0.05 s, and the vertical coordinate represents the forward velocity. It can be seen that the peak lateral velocity reaches −27.2 m/s, which is due to the simulation of high-speed movement of the body and large change in acceleration. This extreme situation has exceeded the range of motion ability of foot-type robot. In addition, the sampling interval is long, which poses a great challenge to state estimation of the system with a drastic change of motion state. It can be seen from Figure 7a that the estimated curve of STMCKF tracks the real value well. The estimation error is obvious at the 150th and 232nd sampling, and there is a slight lag in the whole curve, which is mainly due to the large interval sampling and is difficult to overcome completely. It can also be said that the estimated curve is close to the real value on the whole and the estimated result is accurate. The estimation curves of CKF and EKF are similar, indicating that the system belongs to a weak non-linear system. In this system, the estimation performance of EKF and CKF has little difference, but both of them are affected by bad working conditions, and the estimation curve lags significantly. However, it can be seen that the overall trend is close to the real value, which indicates the correctness of the two filters in the simulation. By comparing the estimation curves of three kinds of filters, the superiority and robustness of STMCKF can be preliminarily proved. From Figure 7b, it can be seen that in the first 50 sampling periods, all three filters have high accuracy, this is because the robot only has forward acceleration and the lateral velocity is almost 0 during this period. The solid line representing the STMCKF shows the lateral acceleration applied at 50, 100, and 230, and the RMSE of the three STMCKF increases slightly in response to the sudden change of motion state, which does not diverge as far as CKF and EKF. STMCKF can suppress the divergence in a few sampling periods, showing excellent robustness.
The running time of EKF, CKF, STMCKF is comparatively studied using a laptop with an Intel Core i5-9400F processor and 8GB RAM, as shown in Table 2. STMCKF increases slightly in response to the sudden change of motion state, which does not diverge as far as CKF and EKF. STMCKF can suppress the divergence in a few sampling periods, showing excellent robustness. The running time of EKF, CKF, STMCKF is comparatively studied using a laptop with an Intel Core i5-9400F processor and 8GB RAM, as shown in Table 2.

Velocity Estimation Experiment of Quadruped Robot
The experimental prototype of the quadruped robot employed is shown in Figure 8. The robot body is equipped with a power system consisting of engine, variable pump, and hydraulic accessories, providing power for the hydraulic cylinder of each joint.

Velocity Estimation Experiment of Quadruped Robot
The experimental prototype of the quadruped robot employed is shown in Figure 8. The robot body is equipped with a power system consisting of engine, variable pump, and hydraulic accessories, providing power for the hydraulic cylinder of each joint. In the walking process of the robot, STMCKF is used to provide the north and east position of the robot for positioning and navigation, and the forward velocity and lateral velocity of the robot are provided for motion control. Figure 9 is a video screenshot of a quadruped robot walking with STMCKF. It can be seen from the figure that when a pair of diagonal legs are in stable supporting posture, the robot mass center will move forward and the whole process is stable. The correctness and validity of the STMCKF proposed in this paper are proved again by experiments. During the experiment, the quadruped robot was walking on the flat ground in a trot gait at a speed of 1 m/s. The sampling frequency of the IMU is 100 HZ, and the sampling frequency of linear displacement sensor is 200 HZ. Let t = 0.005s, Q a,k = eye(3), Q b,k = 1.25e −3 · eye(3), Q k is calculated according to equation (41) based on the value of C n b,k ; Generally, we use the Suge-Husa method for identification, but in order to make the comparison experiment more convincing, we use the time-invariant observation noise variance in the experiment [0.01; 0.01; 0.01; 0.0001; 0.005; 0.005] · eye (6). In the walking process of the robot, STMCKF is used to provide the north and east position of the robot for positioning and navigation, and the forward velocity and lateral velocity of the robot are provided for motion control. Figure 9 is a video screenshot of a quadruped robot walking with STMCKF. It can be seen from the figure that when a pair of diagonal legs are in stable supporting posture, the robot mass center will move forward and the whole process is stable. The correctness and validity of the STMCKF proposed in this paper are proved again by experiments. STMCKF is compared with traditional EKF to highlight the estimation effect of STMCKF more intuitively. For the sake of ensuring the accuracy and fairness of the comparison, we record the data of multiple groups of IMU, and apply STMCKF and EKF to estimate the states of the same group of IMU data, respectively. Here, a 2-minute IMU data is selected, and the comparison diagram of state estimation between STMCKF and EKF for this period of data is shown in Figure 10. STMCKF is compared with traditional EKF to highlight the estimation effect of STMCKF more intuitively. For the sake of ensuring the accuracy and fairness of the comparison, we record the data of multiple groups of IMU, and apply STMCKF and EKF to estimate the states of the same group of IMU data, respectively. Here, a 2-minute IMU data is selected, and the comparison diagram of state estimation between STMCKF and EKF for this period of data is shown in Figure 10. STMCKF is compared with traditional EKF to highlight the estimation effect of STMCKF more intuitively. For the sake of ensuring the accuracy and fairness of the comparison, we record the data of multiple groups of IMU, and apply STMCKF and EKF to estimate the states of the same group of IMU data, respectively. Here, a 2-minute IMU data is selected, and the comparison diagram of state estimation between STMCKF and EKF for this period of data is shown in Figure 10. In Figure 10, the red full line represents STMCKF estimation, and green dash line represents EKF estimation. According to Figure 10, it can be seen that it takes about 4s for the robot to start up and complete initialization, which is consistent with the actual situation. After resetting each foot, the sudden change of the position and velocity of the robot body in all directions will occur, STMCKF and EKF both respond to the sudden change of the robot's motion state, indicating high sensitivity of the two. However, from the estimation curve, it can be seen that STMCKF converges more rapidly than EKF, and the response amplitude of STMCKF is significantly smaller than EKF, indicating that STMCKF has a strong resistance effect to sudden change of motion state. These results prove that STMCKF is better than EKF in coping with the sudden change of motion state. The robot moves forward at 21 s. In Figure 10a and figure 10b, the time-varying curves of the robot's north and east positions related to the robot's positioning and navigation performance are depicted. It can be seen that the solid line representing STMCKF presents a regular change with time, indicating that the STMCKF estimation results are accurate. The state switching during 21 s~22 s and 56 s~58 s is smooth and fast, which shows that STMCKF has a strong tracking ability to the motion state and the application of STMCKF can effectively improve the accuracy and stability of positioning and navigation. As shown in Figure 10a and Figure 10b, the dotted line representing EKF responds to the sudden change at 4s and shows a trend of convergence although it does not fully converge until 21 s. As the robot moves forward, the original convergence trend of EKF becomes the divergence trend and then the robot moves rhythmically, so that the motion state will constantly change and the divergence trend of EKF will converge no more. According to the local enlarged figure in Figure 10a, the robot does not stop moving until 57 s, and STMCKF accurately estimates the static state of the robot and converges directly to the final north position. Although EKF converges gradually from the vibration due to the robot's static state, it will not converge to the exact final position eventually. The information informs the robot "you are still moving back and forth", which is obviously unfavorable for positioning and navigation. The partial enlarged view in Figure 10b shows a slight change of the robot's east position near 79 s, with a displacement of 2.2 mm. Since the moving range of the robot can be ignored, so no one noticed it during the experiment. By comparing Figure 10c with Figure 10d, it can be seen that the robot stops walking after 57 s. The forward velocity of the robot returns to zero, but the lateral speed changes within a very small range. That is to say, the robot swings slightly independently to maintain balance. Instead of swinging back and forth, the robot swings laterally to maintain the balance, which is in line with In Figure 10, the red full line represents STMCKF estimation, and green dash line represents EKF estimation. According to Figure 10, it can be seen that it takes about 4 s for the robot to start up and complete initialization, which is consistent with the actual situation. After resetting each foot, the sudden change of the position and velocity of the robot body in all directions will occur, STMCKF and EKF both respond to the sudden change of the robot's motion state, indicating high sensitivity of the two. However, from the estimation curve, it can be seen that STMCKF converges more rapidly than EKF, and the response amplitude of STMCKF is significantly smaller than EKF, indicating that STMCKF has a strong resistance effect to sudden change of motion state. These results prove that STMCKF is better than EKF in coping with the sudden change of motion state. The robot moves forward at 21 s. In Figure 10a,b, the time-varying curves of the robot's north and east positions related to the robot's positioning and navigation performance are depicted. It can be seen that the solid line representing STMCKF presents a regular change with time, indicating that the STMCKF estimation results are accurate. The state switching during 21 s~22 s and 56 s~58 s is smooth and fast, which shows that STMCKF has a strong tracking ability to the motion state and the application of STMCKF can effectively improve the accuracy and stability of positioning and navigation. As shown in Figure 10a,b, the dotted line representing EKF responds to the sudden change at 4s and shows a trend of convergence although it does not fully converge until 21 s. As the robot moves forward, the original convergence trend of EKF becomes the divergence trend and then the robot moves rhythmically, so that the motion state will constantly change and the divergence trend of EKF will converge no more. According to the local enlarged figure in Figure 10a, the robot does not stop moving until 57 s, and STMCKF accurately estimates the static state of the robot and converges directly to the final north position. Although EKF converges gradually from the vibration due to the robot's static state, it will not converge to the exact final position eventually. The information informs the robot "you are still moving back and forth", which is obviously unfavorable for positioning and navigation. The partial enlarged view in Figure 10b shows a slight change of the robot's east position near 79 s, with a displacement of 2.2 mm. Since the moving range of the robot can be ignored, so no one noticed it during the experiment. By comparing Figure 10c with Figure 10d, it can be seen that the robot stops walking after 57 s. The forward velocity of the robot returns to zero, but the lateral speed changes within a very small range. That is to say, the robot swings slightly independently to maintain balance. Instead of swinging back and forth, the robot swings laterally to maintain the balance, which is in line with our bionic design. Since the robot cannot Sensors 2020, 20, 2251 20 of 25 completely calm down after walking, the change of its velocity is notified to the robot control center via STMCKF. Finally, the control center decides to shift 2.2 mm eastward to keep balance. It can be seen from Figure 10d that after the robot moves 2.2 mm eastward, its lateral velocity basically returns to zero. This shows the role of STMCKF in adjusting the motion state of the robot. If EKF is used to transfer the results to the control center, it is difficult for the control center to solve the balance problem via a slight shift because the dotted line in the local enlarged view in Figure 10b,d is constantly oscillating. This shows the advantage of STMCKF in control efficiency. Figure 10c shows that STMCKF converges rapidly in 0.05 s after responding to the sudden change of forward speed caused by robot start-up, while EKF converges successfully in 2 s. EKF is not good at changing the acceleration of the robot. When the walking velocity of the robot is accelerating from zero to the expected velocity of 1 m/s, the estimated results of EKF show a obvious divergent trend, and the amplitude is very large. The estimation velocity of EKF is much higher than the expected velocity, with negative velocity even occurring, which is obviously incorrect. Furthermore, it can be seen from the local enlarged figure of 40 s~45 s that the curve of EKF's forward velocity estimation is sharp, indicating the velocity often changes abruptly, which is very unfavorable for the control of the quadruped robot. Using such a state estimator will make the track of the foot end of the robot become less smooth, increasing the wear of various components and reducing the service life. The velocity estimation curve plotted by STMCKF shows a regular periodic sawtooth wave, and the peak value of velocity is very close to 1 m/s. This may be because the ground on which the robot walks is made of smooth tile, so the slipping phenomenon occurred, which reduced the walking efficiency.

Conclusions
To further increase the accuracy of state estimation for a quadruped robot system, a strong tracking mixed-degree cubature Kalman filter algorithm is proposed in this paper. The proposed algorithm is used to fuse IMU and forward kinematics for estimation, which can greatly improve the accuracy, robustness and real-timeness of state estimation without increasing the cost. This algorithm improves the robustness of STMCKF by strong tracking. Moreover, the more appropriate combination of STMCKF and strong tracking is demonstrated in this study. Unlike the traditional combination method of a strong tracking cubature Kalman filter, this paper puts forward a new calculation method of fading factor matrix, which realizes the unbiased estimation and reduces the original calculation of sampling points from three times to once. Through the judgment of the sudden change of motion state, it can be known that the strong tracking can only be started when necessary, which further improves the real-time performance of the algorithm. The results of simulation and experiment show the correctness and excellent estimation performance of the STMCKF algorithm proposed in this paper. The comparison of Table 2 and RMSE shows that the accuracy of STMCKF is much higher than that of EKF and CKF while the time consumption of STMCKF is only 31.91% longer than that of EKF and 40.62% shorter than that of CKF. For the sudden change of robot motion state, STMCKF can also make accurate judgment, effective suppression and strong tracking. To sum up, STMCKF is very suitable for the quadruped robot system whose motion state changes constantly. Using the proposed STMCKF algorithm, the state estimation accuracy of quadruped robots can be effectively improved without increasing the research and development (R&D) cost. Moreover, the proposed algorithm has good robustness and real-time performance, making the sensor on the robot achieve the maximum use benefit. Meanwhile, since there is no limitation to the number of feet, SMSRCKF is suitable for all legged robots, which has great application value for improving the motion performance of legged robot and for enhancing the accuracy of navigation and positioning.

Conflicts of Interest:
The authors declare no conflict of interest.

Appendix A
The optimal state estimation based on Kalman filtering derivation has the following forms: where, E z k/k−1 z T k/k−1 = P zz,k/k−1 = H k P k/k−1 H T k + R k (A3) E x k/k−1 z T k/k−1 = P xz,k/k−1 = P k/k−1 H T k (A4) Introduce the fading factor according to the traditional method and compensate the innovation, then, P * k/k−1 = λ k F k−1 P k−1 F T k−1 + Q k−1 (A5) δ * j is the volume point generated by P * k+1/k after the introduction of the fading factor.
When the fading factor is introduced according to the traditional method, but the innovation is not compensated, there is,x * k =x k/k−1 + P * xz,k/k−1 P * −1 zz,k/k−1 z k/k−1 (A11) Then, the performance index formula (3) is met, but if strong tracking is effective at this time, which means λ k > 1, there is still z k/k−1 z * k/k−1 . Then, whether performance index formula (4) is met cannot be guaranteed. That is, the innovation sequence cannot be guaranteed to be orthogonal.