基于卡尔曼滤波的车速估算

2020-06-17 本文已影响0人知奇者也

车速是车辆控制中非常重要的基础信息，尤其在四轮驱动中对于极限工况下的车辆控制，车速尤其需要估算精准。现用Kalman Filter卡尔曼滤波器对四轮转速计算得到的车速进行融合，并预测车速。

卡尔曼滤波公式

Kalman Filter的公式及其简化如下

Kalman Filter公式及简化

其中，
$x'=VehicleSpeed$
$F=1$
$Bu=a* \Delta t$
$z=\begin{vmatrix}WheelSpeed1\\WheelSpeed2\\WheelSpeed3\\WheelSpeed4\end{vmatrix}$
$H=\begin{vmatrix}SlipOrNot(WheelSpeed1)\\SlipOrNot(WheelSpeed2)\\SlipOrNot(WheelSpeed3)\\SlipOrNot(WheelSpeed4)\end{vmatrix}$
关于 $H$ ，若Wheel1不打滑， $SlipOrNot(WheelSpeed1)=1$ ，若Wheel1打滑， $SlipOrNot(WheelSpeed1)=0$ ，则当车轮打滑时，该车轮轮速将被刨除在车速估算内。