# Vehicle State Estimation Based on Sage–Husa Adaptive Unscented Kalman Filtering

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

- (1)
- The Sage–Husa algorithm is improved to avoid non-positive definiteness of the covariance matrix and to ensure its positivity.
- (2)
- The improved Sage–Husa algorithms are adopted to dynamically update the mean and covariance matrices of the measurement noise, which effectively improves the filtering accuracy and prevents its divergence.
- (3)
- Sage–Husa algorithm is integrated with the UKF algorithm to form the AUKF algorithm for dynamic vehicle state estimation. The simulation results demonstrate that AUKF increases estimation accuracy by 19.13%, 32.8%, and 39.46% in yaw rate, side-slip angle, and longitudinal velocity, respectively, proving the algorithm’s validity in providing accurate vehicle state information for active vehicle safety control.

## 2. Vehicle State Parameter Estimation Model

#### 2.1. 3-DOF Vehicle Dynamics Model

#### 2.2. Tire Model

## 3. Vehicle State Parameter Estimation Based on AUKF

#### 3.1. UKF Algorithm

- (1)
- Obtain a set of sampling points (sigma points) and calculate the corresponding weights of these sampling pointsIt is assumed that the $\overline{X}$ and variance $P$ of the n-dimensional random variable state vector $X$ are known. Then, by obtaining 2n + 1 sigma points $X$ and the appropriate weights via the subsequent unscented transformation, the statistical properties of $f(x)$ may be computed.$$\{\begin{array}{l}{X}^{(0)}=\overline{X},i=0\\ {X}_{k-1}{}^{(i)}=\overline{X}+{(\sqrt{(n+\lambda )P})}_{i},i=1~n\\ {X}_{k-q}{}^{(i)}=\overline{X}-{(\sqrt{(n+\lambda )P})}_{i},i=n+1~2n\end{array}$$These sampling sites’ related weights are determined as$$\{\begin{array}{l}{\omega}_{m}{}^{(0)}={\omega}_{c}{}^{(0)}=\frac{\lambda}{n+\lambda},i=0\\ {\omega}_{m}{}^{(i)}={\omega}_{c}{}^{(i)}=\frac{1}{2(n+\lambda )},i=1~2n\end{array}$$
- (2)
- According to Equations (15) and (16), a set of sampling points and their corresponding weights are calculated$${X}^{(i)}(k|k)=[\widehat{X}(k|k),\widehat{X}(k|k)+\sqrt{(n+\lambda )P(k|k)},\widehat{X}(k|k)-\sqrt{(n+\lambda )P(k|k)}]$$
- (3)
- One-step prediction of the set of 2n + 1 sigma points using the state equation$${X}^{(i)}(k+1|k)=f[{X}^{(i)}(k|k),k]$$
- (4)
- Calculation of one-step prediction and covariance matrix of the system state variables$$\stackrel{\u2322}{X}(k+1|k)={\displaystyle \sum _{i=0}^{2n}{\omega}^{(i)}}{X}^{(i)}(k+1|k)$$$${P}_{{x}_{k}}={\displaystyle \sum _{i=0}^{2n}{\omega}^{(i)}}[{X}^{(i)}(k+1|k)-\stackrel{\u2322}{X}(k+1|k)]{[{X}^{(i)}(k+1|k)-\stackrel{\u2322}{X}(k+1|k)]}^{\mathrm{T}}+Q$$
- (5)
- The predicted observations are calculated by bringing sigma points into the observation equation$${Z}^{(i)}(k+1|k)=h[{X}^{(i)}(k+1|k),k+1]$$
- (6)
- The mean, covariance and cross-covariance are calculated analogously to Equations (19) and (20)$$\overline{Z}(k+1|k)={\displaystyle \sum _{i=0}^{2n}{\omega}^{(i)}}{Z}^{(i)}(k+1|k)$$$${P}_{{z}_{k}}={\displaystyle \sum _{i=0}^{2n}{\omega}^{(i)}}[{Z}^{(i)}(k+1|k)-\overline{Z}(k+1|k)]{[{Z}^{(i)}(k+1|k)-\overline{Z}(k+1|k)]}^{\mathrm{T}}+R$$$${P}_{{x}_{k}{z}_{k}}={\displaystyle \sum _{i=0}^{2n}{\omega}^{(i)}}[{X}^{(i)}(k+1|k)-\stackrel{\u2322}{X}(k+1|k)]{[{Z}^{(i)}(k+1|k)-\overline{Z}(k+1|k)]}^{\mathrm{T}}$$
- (7)
- Kalman gain matrix is calculated$$K(k+1)={P}_{{x}_{k}{z}_{k}}{P}_{{z}_{k}}^{-1}$$
- (8)
- System status and covariance matrix are updated$$\stackrel{\u2322}{X}(k+1|k+1)=\stackrel{\u2322}{X}(k+1|k)+K(k+1)[Z(k+1)-\overline{Z}(k+1|k)]$$$$P(k+1|k+1)=P(k+1|k)-K(k+1){P}_{{z}_{k}}{K}^{\mathrm{T}}(k+1)$$

#### 3.2. AUKF Algorithm

## 4. Simulation Results and Analyses

#### 4.1. Steering Angle Step-Input Condition

^{T}. The initial value of the system-input vector $u(t)$ is [$\delta $, ${a}_{x}$]

^{T}, and its input waveform is shown in Figure 3. The observed quantity $Z$ is ${a}_{y}$, and the waveform after adding Gaussian white noise is shown in Figure 4.

#### 4.2. Sinusoidal Steering Condition

^{T}. The initial value of the system input vector $u(t)$ is [$\delta $, ${a}_{x}$]

^{T}, and its input waveform is shown in Figure 8. The observed quantity $Z$ is ${a}_{y}$, and the waveform after adding Gaussian white noise is shown in Figure 9.

#### 4.3. Double-Lane Change Condition

^{T}. The initial value of the system input vector $u(t)$ is [$\delta $, ${a}_{x}$]

^{T}, and its input waveform is shown in Figure 16. The observed quantity $Z$ is ${a}_{y}$, and the waveform after adding time-varying Gaussian white noise is shown in Figure 17, in which the statistical characteristics of the measurement noise becomes 10 times that of the first 10 s at 10–20 s, i.e., 10${R}_{k}$.

## 5. Conclusions

## Author Contributions

## Funding

## Data Availability Statement

## Conflicts of Interest

## References

- Indu, K.; Aswatha Kumar, M. Electric Vehicle Control and Driving Safety Systems: A Review. IETE J. Res.
**2023**, 69, 482–498. [Google Scholar] [CrossRef] - Guo, H.; Cao, D.; Chen, H.; Lv, C.; Wang, H.; Yang, S. Vehicle dynamic state estimation: State of the art schemes and perspectives. IEEE/CAA J. Autom. Sin.
**2018**, 5, 418–431. [Google Scholar] [CrossRef] - Mazzilli, V.; Ivone, D.; De Pinto, S.; Pascali, L.; Contrino, M.; Tarquinio, G.; Gruber, P.; Sorniotti, A. On the benefit of smart tyre technology on vehicle state estimation. Veh. Syst. Dyn.
**2022**, 60, 3694–3719. [Google Scholar] [CrossRef] - Jin, X.; Yin, G.; Chen, N. Advanced estimation techniques for vehicle system dynamic state: A survey. Sensors
**2019**, 19, 4289. [Google Scholar] [CrossRef] [Green Version] - Chen, B.C.; Hsieh, F.C. Sideslip angle estimation using extended Kalman filter. Veh. Syst. Dyn.
**2008**, 46, 353–364. [Google Scholar] [CrossRef] - Piyabongkarn, D.; Rajamani, R.; Grogg, J.A.; Lew, J.Y. Development and experimental evaluation of a slip angle estimator for vehicle stability control. IEEE Trans. Control Syst. Technol.
**2009**, 17, 78–88. [Google Scholar] [CrossRef] - Viehweger, M.; Vaseur, C.; Aalst, S.; Acosta, M.; Regolin, E.; Alatorre, A.; Desmet, W.; Naets, F.; Ivanov, V.; Ferrara, A.; et al. Vehicle state and tyre force estimation: Demonstrations and guidelines. Veh. Syst. Dyn.
**2021**, 59, 675–702. [Google Scholar] [CrossRef] - González, L.P.P.; Sánchez, S.S.S.; Garcia-Guzman, J.; Boada, M.J.L.; Boada, B.L. Simultaneous Estimation of Vehicle Roll and Sideslip Angles through a Deep Learning Approach. Sensors
**2020**, 20, 3679. [Google Scholar] [CrossRef] - Novi, T.; Capitani, R.; Annicchiarico, C. An integrated artificial neural network–unscented Kalman filter vehicle sideslip angle estimation based on inertial measurement unit measurements. Proc. Inst. Mech. Eng. Part D J. Automob. Eng.
**2019**, 233, 1864–1878. [Google Scholar] [CrossRef] - Yang, S.; Lu, Y.; Li, S. An overview on vehicle dynamics. Int. J. Dyn. Control
**2013**, 1, 385–395. [Google Scholar] [CrossRef] [Green Version] - Sun, W.; Wang, Z.; Wang, J.; Wang, X.; Liu, L. Research on a Real-Time Estimation Method of Vehicle Sideslip Angle Based on EKF. Sensors
**2022**, 22, 3386. [Google Scholar] [CrossRef] - Jeong, D.; Ko, G.; Choi, S.B. Estimation of sideslip angle and cornering stiffness of an articulated vehicle using a constrained lateral dynamics model. Mechatronics
**2022**, 85, 102810. [Google Scholar] [CrossRef] - Song, R.; Fang, Y. Estimation of Vehicle Sideslip Angle based on Modified Sliding Mode Observer and Recurrent Neural Network. In Proceedings of the 2022 7th Asia-Pacific Conference on Intelligent Robot Systems (ACIRS), Tianjin, China, 1–3 July 2022; pp. 135–139. [Google Scholar]
- Zhang, F.; Wang, Y.; Hu, J.; Yin, G.; Chen, S.; Zhang, H.; Zhou, D. A novel comprehensive scheme for vehicle state estimation using dual extended H-infinity kalman filter. Electronics
**2021**, 10, 1526. [Google Scholar] [CrossRef] - Venhovens, P.J.T.; Naab, K. Vehicle dynamics estimation using Kalman filters. Veh. Syst. Dyn.
**1999**, 32, 171–184. [Google Scholar] [CrossRef] - Zong, C.F.; Hu, D.; Yang, X.; Pan, Z.; Xu, Y. Vehicle driving state estimation based on extended Kalman filter. J. Jilin Univ. (Eng. Technol. Ed.)
**2009**, 39, 7–11. [Google Scholar] - Singh, K.B.; Arat, M.A.; Taheri, S. Literature review and fundamental approaches for vehicle and tire state estimation. Veh. Syst. Dyn.
**2019**, 57, 1643–1665. [Google Scholar] [CrossRef] - Heidfeld, H.; Schünemann, M.; Kasper, R. Experimental Validation of a GPS-Aided Model-Based UKF Vehicle State Estimator. In Proceedings of the 2019 IEEE International Conference on Mechatronics (ICM), Ilmenau, Germany, 18–20 March 2019; pp. 537–543. [Google Scholar]
- Villano, E.; Lenzo, B.; Sakhnevych, A. Cross-combined UKF for vehicle sideslip angle estimation with a modified Dugoff tire model: Design and experimental results. Meccanica
**2021**, 56, 2653–2668. [Google Scholar] [CrossRef] - Huang, Y. Estimation of Vehicle Status and Parameters Based on Nonlinear Kalman Filtering. In Proceedings of the 2022 6th International Conference on Robotics and Automation Sciences (ICRAS), Wuhan, China, 9–11 June 2022; pp. 200–205. [Google Scholar]
- Xiao, Z.; Xiao, D.; Havyarimana, V.; Jiang, H.; Liu, D.; Wang, D.; Zeng, F. Toward accurate vehicle state estimation under non-Gaussian noises. IEEE Internet Things J.
**2019**, 6, 10652–10664. [Google Scholar] [CrossRef] - Wang, Q.; Zhao, Y.; Lin, F.; Zhang, C.; Deng, H. Integrated control for distributed in-wheel motor drive electric vehicle based on states estimation and nonlinear MPC. Proc. Inst. Mech. Eng. Part D J. Automob. Eng.
**2022**, 236, 893–906. [Google Scholar] [CrossRef] - Chu, W.; Luo, Y.; Dai, Y.; Li, K. In–wheel motor electric vehicle state estimation by using unscented particle filter. Int. J. Veh. Des.
**2015**, 67, 115–136. [Google Scholar] [CrossRef] - Wang, Z.P.; Xue, X.; Wang, Y.C. State parameter estimation of distributed drive electric vehicle based on adaptive unscented Kalman filter. J. Beijing Inst. Technol.
**2018**, 38, 698–702. [Google Scholar] - Fan, T.E.; Liu, S.M.; Tang, X.; Qu, B.H. Simultaneously estimating two battery states by combining a long short-term memory network with an adaptive unscented Kalman filter. J. Energy Storage
**2022**, 50, 104553. [Google Scholar] [CrossRef] - Li, G.; Zhao, D.; Xie, R.; Han, H.; Zong, C. Vehicle State Estimation Based on Improved Sage–Husa Adaptive Extended Kalman Filtering. Automot. Eng.
**2015**, 37, 1426–1432. [Google Scholar] - Zhou, B.; Li, T.; Wu, X.; Lei, F. Semi-trailer State Estimation Based on Double Adaptive Unscented Kalman Filter. J. Hunan Univ. (Nat. Sci.)
**2022**, 49, 63–73. [Google Scholar] - Xu, D.; Wang, B.; Zhang, L.; Chen, Z. A New Adaptive High-Degree Unscented Kalman Filter with Unknown Process Noise. Electronics
**2022**, 11, 1863. [Google Scholar] [CrossRef] - Luo, Z.; Fu, Z.; Xu, Q. An Adaptive multi-dimensional vehicle driving state observer based on modified Sage–Husa UKF algorithm. Sensors
**2020**, 20, 6889. [Google Scholar] [CrossRef] - Yang, R.; Zhang, A.; Zhang, L. A novel adaptive H-Infinity cubature Kalman filter algorithm based on Sage-Husa estimator for unmanned underwater vehicle. Math. Probl. Eng.
**2020**, 9, 456–463. [Google Scholar] [CrossRef] - Bian, H.; Jin, Z.; Wang, J. The innovation-based estimation adaptive Kalman filter algorithm for INS/GPS integrated navigation system. J. Shanghai Jiaotong Univ.
**2006**, 40, 1000. [Google Scholar] - Dey, A.; Sadhu, S.; Ghoshal, T.K. Adaptive Gauss–Hermite filter for non-linear systems with unknown measurement noise covariance. IET Sci. Meas. Technol.
**2015**, 9, 1007–1015. [Google Scholar] [CrossRef] - Narasimhappa, M.; Mahindrakar, A.D.; Guizilini, V.C.; Terra, M.H.; Sabat, S.L. MEMS-based IMU drift minimization: Sage Husa adaptive robust Kalman filtering. IEEE Sens. J.
**2019**, 20, 250–260. [Google Scholar] [CrossRef] - Yu, Z. Automobile Theory, 5th ed.; China Machine Press: Beijing, China, 2009; pp. 144–146. [Google Scholar]
- Pacejka, H. Tire and Vehicle Dynamics, 3rd ed.; Butterworth-Heinemann: Oxford, UK, 2012; pp. 593–601. [Google Scholar]
- Wan, E.A.; Van Der Merwe, R. The unscented Kalman filter. In Kalman Filtering and Neural Networks; John Wiley & Sons, Inc.: New York, NY, USA, 2001; pp. 221–280. [Google Scholar]
- Liu, K.; Zhao, W.; Sun, B.; Wu, P.; Zhu, D.; Zhang, P. Application of updated Sage–Husa adaptive Kalman filter in the navigation of a translational sprinkler irrigation machine. Water
**2019**, 11, 1269. [Google Scholar] [CrossRef] [Green Version]

**Figure 7.**Comparison of longitudinal velocity estimates under the steering angle step-input condition.

**Figure 15.**Absolute error of longitudinal velocity estimates under the sinusoidal steering condition.

**Figure 23.**Absolute error of longitudinal velocity estimates under the double-lane change condition.

Parameter | Value |
---|---|

$m$ | 1410 kg |

$a$ | 1.015 m |

$b$ | 1.895 m |

${k}_{1}$ | −122,540 N·rad^{−1} |

${k}_{2}$ | −100,500 N·rad^{−1} |

${I}_{z}$ | 1536.7 kg·m^{2} |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.0012 | 0.000462 | 0.3726 |

AUKF | 0.0009 | 0.000140 | 0.1982 |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.0063 | 0.000907 | 0.4644 |

AUKF | 0.0061 | 0.000821 | 0.3133 |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.000726 | 0.000176 | 0.557 |

AUKF | 0.000551 | 0.000141 | 0.154 |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.000916 | 0.000219 | 0.6434 |

AUKF | 0.000689 | 0.000176 | 0.2356 |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.0020 | 0.000363 | 0.348 |

AUKF | 0.0016 | 0.000116 | 0.234 |

Algorithm | Yaw Rate (rad/s) | Side-Slip Angle (rad) | Longitudinal Velocity (km/h) |
---|---|---|---|

UKF | 0.0034 | 0.000578 | 0.4289 |

AUKF | 0.0024 | 0.000177 | 0.3321 |

Algorithm | Total Running Time (s) | Single-Step Running Time (s) | Computational Complexity |
---|---|---|---|

UKF | 23.433 | 0.00117 | O(n^{2}) |

AUKF | 24.026 | 0.00120 | O(n^{2}) |

Algorithm | Total Running Time (s) | Single-Step Running Time (s) | Computational Complexity |
---|---|---|---|

UKF | 52.716 | 0.00132 | O(n^{2}) |

AUKF | 56.439 | 0.00141 | O(n^{2}) |

Algorithm | Total Running Time (s) | Single-Step Running Time (s) | Computational Complexity |
---|---|---|---|

UKF | 24.892 | 0.00124 | O(n^{2}) |

AUKF | 26.214 | 0.00131 | O(n^{2}) |

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content. |

© 2023 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 (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Chen, Y.; Yan, H.; Li, Y.
Vehicle State Estimation Based on Sage–Husa Adaptive Unscented Kalman Filtering. *World Electr. Veh. J.* **2023**, *14*, 167.
https://doi.org/10.3390/wevj14070167

**AMA Style**

Chen Y, Yan H, Li Y.
Vehicle State Estimation Based on Sage–Husa Adaptive Unscented Kalman Filtering. *World Electric Vehicle Journal*. 2023; 14(7):167.
https://doi.org/10.3390/wevj14070167

**Chicago/Turabian Style**

Chen, Yong, Hao Yan, and Yuecheng Li.
2023. "Vehicle State Estimation Based on Sage–Husa Adaptive Unscented Kalman Filtering" *World Electric Vehicle Journal* 14, no. 7: 167.
https://doi.org/10.3390/wevj14070167