[发明专利]横摆角速度滤波方法、控制终端、车辆及存储介质

权利要求书

1.一种横摆角速度滤波方法，其特征在于，包括步骤：

当系统上电后，根据预设采样频率采集横摆角速度测量值；

每采集到一个横摆角速度测量值时，判断当前采样时刻是否为系统上电后的第一个采样时刻；

若当前采样时刻为系统上电后的第一个采样时刻，则将当前采样时刻的横摆角速度测量值和预设的系统误差协方差矩阵分别作为当前采样时刻的横摆角速度滤波值和系统误差协方差矩阵，以及将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；

若当前采样时刻不为系统上电后的第一个采样时刻，则根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵；

判断当前采样时刻前累积采集的横摆角速度测量值的数量是否大于或等于预设数量；

若否，则将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；

若是，则根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵；

根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型。

2.根据权利要求1所述的横摆角速度滤波方法，其特征在于，所述根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵的步骤包括：

根据前一采样时刻的车辆横摆角速度滤波值和当前采样时刻的卡尔曼滤波模型中的预测值计算公式，获得当前采样时刻的横摆角速度预测值，其中预测值计算公式为X_pk＝A*X_k-1，X_pk为当前采样时刻的横摆角速度预测值、X_k-1为前一采样时刻的车辆横摆角速度滤波值和A为预设状态转移矩阵；

根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第一系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+Q_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵；

根据当前采样时刻的系统预测误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的卡尔曼增益计算公式，获得当前采样时刻的卡尔曼增益，其中卡尔曼增益计算公式为K_k＝P_pk*H^T/(H*P_pk*H^T+R_k)，K_k为当前采样时刻的卡尔曼增益、H为预设观测矩阵以及R_k为当前采样时刻的传感器噪声协方差矩阵；

根据当前采样时刻的横摆角速度预测值、横摆角速度测量值和卡尔曼增益和当前采样时刻的卡尔曼滤波模型中的横摆角速度滤波值计算公式，获得当前采样时刻的横摆角速度滤波值，其中横摆角速度滤波值计算公式为X_k＝X_pk+K_k(Z_k-H*X_pk)，其中X_k为当前采样时刻的横摆角速度滤波值，Z_k为当前采样时刻的横摆角速度测量值；

根据当前采样时刻的卡尔曼增益、系统预测误差协方差矩阵和卡尔曼滤波模型中的系统误差协方差矩阵计算公式，获得当前采样时刻的系统误差协方差矩阵，其中系统误差协方差矩阵计算公式为P_k＝(1-K_k*H)*P_pk，P_k为当前采样时刻的系统误差协方差矩阵。