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

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Strong Tracking Mixed-Degree Cubature Kalman Filter (STMCKF)

#### 2.1. Initialization

#### 2.2. Predication

#### 2.3. Pseudo Observation Update

_{k}satisfies the following equations:

#### 2.4. State Mutation Test

#### 2.5. Calculate Multiple Fading Factors

#### 2.6. Recalculation

#### 2.7. Update

## 3. Forward Kinematics of Quadruped Robot

#### 3.1. Equation of State for Quadruped Robot

^{−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.

#### 3.2. Pseudo-Observation Equation of Quadruped Robot

#### 3.3. Multi-Sensor Fusion Structure Diagram

## 4. Numerical Experiments

## 5. Velocity Estimation Experiment of Quadruped Robot

## 6. Conclusions

## Author Contributions

## Acknowledgments

## Conflicts of Interest

## Appendix A

## Appendix B

## References

- Yaakov, B.S.; Li, X.; Thiagalingam, K. Estimation with Applications to Tracking and Navigation; John Wiley and Sons: New York, NY, USA, 2001. [Google Scholar]
- Julier, S.J.; Uhlmann, J.K. Unscented Filtering and Nonlinear Estimation. Proc. IEEE
**2004**, 92, 401–422. [Google Scholar] [CrossRef][Green Version] - Doucet, A.; Godsill, S.; Andrieu, C. On sequential Monte Carlo sampling methods for Bayesian filtering. Stat. Comput.
**2000**, 10, 197–208. [Google Scholar] [CrossRef] - Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control
**2009**, 54, 1254–1269. [Google Scholar] [CrossRef][Green Version] - Gupta, D.; Anand, R.S.; Tyagi, B. Ripplet domain non-linear filtering for speckle reduction in ultrasound medical images. Biomed. Signal Process. Control
**2014**, 10, 79–91. [Google Scholar] [CrossRef] - JIA, B.; XIN, M.; CHENG, Y. High-degree cubature Kalman filter. Automatica
**2013**, 49, 510–518. [Google Scholar] [CrossRef] - Meng, D.; Miao, L.; Shao, H. A Seventh-Degree Cubature Kalman Filter. Asian J. Control
**2017**, 20, 250–262. [Google Scholar] [CrossRef] - Yanling, H.; Junwei, Y.; Liang, C. Square Root Cubature Kalman Filter. J. Projectiles Rockets Missiles Guidance
**2012**, 32, 169–172. [Google Scholar] - Cui, X.; Jing, Z.; Luo, M.; Guo, Y.; Qiao, H. A New Method for State of Charge Estimation of Lithium-Ion Batteries Using Square Root Cubature Kalman Filter. Energies
**2018**, 11, 209. [Google Scholar] [CrossRef][Green Version] - Wang, S.; Feng, J.; Tse, C.K. Spherical Simplex-Radial Cubature Kalman Filter. IEEE Signal Process Lett.
**2014**, 21, 43–46. [Google Scholar] [CrossRef] - Liu, H.; Wu, W. Interacting Multiple Model (IMM) Fifth-Degree Spherical Simplex-Radial Cubature Kalman Filter for Maneuvering Target Tracking. Sensors
**2017**, 17, 1374. [Google Scholar] [CrossRef] [PubMed][Green Version] - Wang, S.; Feng, Y.; Duan, S.; Wang, L. Mixed-Degree Spherical Simplex-Radial Cubature Kalman Filter. Math. Probl. Eng.
**2017**, 10, 23–29. [Google Scholar] [CrossRef][Green Version] - Zhang, Y.; Huang, Y.; Wu, Z.; Li, N. Seventh-degree spherical simplex-radial cubature Kalman filter. In Proceedings of the 33rd Chinese Control Conference, Nanjing, China, 28–30 July 2014; Volume 7, pp. 2513–2517. [Google Scholar]
- Zhou, D.H.; Xi, Y.G.; Zhang, Z.J. A suboptimal multiple fading extended Kalman filter. Acta Autom. Sin
**1991**, 17, 689–695. [Google Scholar] - Feng, K.; Li, J.; Zhang, X.; Zhang, X.; Shen, C.; Cao, H.; Yang, Y.; Liu, W. An Improved Strong Tracking Cubature Kalman Filter for GPS/INS Integrated Navigation Systems. Sensors
**2018**, 18, 1919. [Google Scholar] [CrossRef] [PubMed][Green Version] - Huang, W.; Xie, H.; Shen, C.; Li, J. A robust strong tracking cubature Kalman filter for spacecraft attitude estimation with quaternion constraint. Acta Astronautica
**2016**, 121, 153–163. [Google Scholar] [CrossRef] - Hua, L.; Wen, W. Strong Tracking Spherical Simplex-Radial Cubature Kalman Filter for Maneuvering Target Tracking. Sensors
**2017**, 17, 741–753. [Google Scholar] - Zhao, L.; Wang, J.; Yu, T.; Jian, H.; Liu, T. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator. Appl. Math. Comput.
**2015**, 256, 352–367. [Google Scholar] [CrossRef] - Zhang, A.; Bao, S.; Gao, F.; Bi, W. A novel strong tracking cubature Kalman filter and its application in maneuvering target tracking. Chin. J. Aeronaut.
**2019**, 32, 2489–2502. [Google Scholar] [CrossRef] - Wang, P.; Liu, J.; Zha, F.; Guo, W.; Wang, X.; Li, M.; Sun, L. A velocity estimation algorithm for legged robot. Adv. Mech. Eng.
**2017**, 9, 168781401773273. [Google Scholar] [CrossRef]

**Figure 3.**Connecting rod coordinate system of right front leg of quadruped robot [20].

**Figure 6.**Comparison of forward velocity estimation with three filters. (

**a**) Forward velocity estimation; (

**b**) RMSE of forward velocity.

**Figure 7.**Comparison of lateral velocity estimation with three filters. (

**a**) Lateral velocity estimation; (

**b**) RMSE of lateral velocity.

**Figure 9.**Screenshot of walking experiment of quadruped robot prototype. (

**a**) Robot is ready to start a new trot gait cycle; (

**b**) Robot raises the left front leg and the right hind leg on the diagonal; (

**c**) Robot chooses landing points according to state estimation result; (

**d**) Robot raises the right front leg and the left hind leg on the diagonal; (

**e**) Robot chooses landing points according to update state estimation result; (

**f**) Robot starts next cycle of trot gait.

**Figure 10.**Comparison of state estimation between EKF and STMCKF of a quadruped robot. (

**a**) Comparison of north position estimation between EKF and STMCKF; (

**b**) Comparison of east position estimation between EKF and STMCKF; (

**c**) Comparison of forward velocity estimation between EKF and STMCKF; (

**d**) Comparison of forward velocity estimation between EKF and STMCKF.

Joint i | Connecting Rod Length ${\mathit{l}}_{\mathit{i}}$ (mm) | Torsional Angle ${\mathit{\alpha}}_{\mathit{i}}$ (°) | Connecting Rod Distance ${\mathit{d}}_{\mathit{i}}$ (mm) | Connecting Rod Angle ${\mathit{\theta}}_{\mathit{i}}$ (°) |
---|---|---|---|---|

1 | 100 | 90 | 0 | ${\theta}_{1}$ |

2 | 315 | 0 | 0 | ${\theta}_{2}$ |

3 | 335 | 0 | 0 | ${\theta}_{3}$ |

4 | 340 | 0 | 0 | ${\theta}_{4}$ |

Algorithm | EKF | CKF | STMCKF |
---|---|---|---|

Running time (s) | 1.288 | 2.861 | 1.699 |

Increased (%) | 0 | 122.13 | 31.91 |

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Liu, J.; Wang, P.; Zha, F.; Guo, W.; Jiang, Z.; Sun, L. A Strong Tracking Mixed-Degree Cubature Kalman Filter Method and Its Application in a Quadruped Robot. *Sensors* **2020**, *20*, 2251.
https://doi.org/10.3390/s20082251

**AMA Style**

Liu J, Wang P, Zha F, Guo W, Jiang Z, Sun L. A Strong Tracking Mixed-Degree Cubature Kalman Filter Method and Its Application in a Quadruped Robot. *Sensors*. 2020; 20(8):2251.
https://doi.org/10.3390/s20082251

**Chicago/Turabian Style**

Liu, Jikai, Pengfei Wang, Fusheng Zha, Wei Guo, Zhenyu Jiang, and Lining Sun. 2020. "A Strong Tracking Mixed-Degree Cubature Kalman Filter Method and Its Application in a Quadruped Robot" *Sensors* 20, no. 8: 2251.
https://doi.org/10.3390/s20082251