CN108545081B - 基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法及系统 - Google Patents

Abstract

本发明公开了一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法及系统，该方法包括建立与质心侧偏角相关的三自由度车辆动力学模型，并根据此模型确定无迹卡尔曼滤波器的状态方程和观测方程，确定输入量、状态量、观测量；输入量包括前轮转角和纵向加速度；状态量包括质心侧偏角、横摆角速度和纵向车速；观测量包括侧向加速度和横摆角速度；采用M估计器算法计算权重因子；结合权重因子、当前时刻状态量、状态方程、观测方程，采用无迹卡尔曼滤波算法对下一时刻的质心侧偏角估计。本发明通过将M估计器算法与无迹卡尔曼滤波算法相结合，抑制离群点带来的影响，提高无迹卡尔曼滤波算法对观测信号中离群点的鲁棒性，提高车辆质心侧偏角的估计准确度。

Description

基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法及系统

技术领域

本发明涉及车辆质心侧偏角估计技术领域，特别是涉及一种基于鲁棒无迹卡尔曼滤波算法的质心侧偏角估计方法及系统。

背景技术

车辆质心侧偏角是表征车辆稳定性的重要参数，因此在进行稳定性控制时必须获取车辆质心侧偏角。然而车辆质心侧偏角传感器价格昂贵，不适用于量产车，因此需要采用参数估计方法对车辆质心侧偏角进行估计。在构建车辆模型时，2自由度线性模型的精度较差，因此常常采用非线性模型对质心侧偏角进行估计，以期得到更准确的估计结果。

卡尔曼滤波(KalmanFilter，简称KF)方法仅适用于线性系统的参数估计。扩展卡尔曼滤波(ExtendedKalmanFilter，简称EKF)方法采用了泰勒级数展开方法对状态量进行线性化。当系统非线性特性较强时，泰勒级数展开的截断误差较大。无迹卡尔曼滤波(Unscented Kalman Filter，简称UKF)方法更加适用于强非线性系统的参数估计，采用无迹变换的形式产生一系列与状态量的均值和协方差相同的sigma点，通过对这些sigma点进行计算并加权求和，来实现卡尔曼滤波算法。因此，UKF方法适用于车辆质心侧偏角估计。

然而，UKF方法与KF方法的机理一脉相承，均是递归最小二乘法的延伸，因此当观测信号中包含离群点时，车辆质心侧偏角估计结果会受到较大影响，即为：UKF方法对观测信号中的离群点具有弱鲁棒性。在进行车辆质心侧偏角估计时，观测信号往往来源于车辆传感器，而传感器信号经常存在离群点，因此严重影响了UKF方法对质心侧偏角估计结果的准确程度。

发明内容

本发明的目的是提拱了一种基于鲁棒无迹卡尔曼滤波算法的质心侧偏角估计方法及系统，将M估计器算法与无迹卡尔曼滤波算法相结合，能够抑制离群点带来的影响，提高无迹卡尔曼滤波算法对观测信号中离群点的鲁棒性，进而提高车辆质心侧偏角的估计准确度。

为实现上述目的，本发明提供了如下方案：

一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法包括：

建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型；

根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度；

采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差；

结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

可选的，在结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计之前，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法还包括：

对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

可选的，所述建立三自由度车辆动力学模型，具体包括：

根据以下公式建立三自由度车辆动力学模型；所述公式为：

其中，

为质心侧偏角的导数；

为横摆角速度的导数；

为纵向加速度的导数；k1，k2分别为前、后轮胎的侧偏刚度；m为整车质量；v_x为纵向速度；β为质心侧偏角；a，b分别为质心到前、后轴的距离；γ为横摆角速度；δ为前轮转角；I_z为绕z轴转动惯量；a_x为纵向加速度。

可选的，所述无迹卡尔曼滤波器的状态方程为：

x_k+1＝f(x_k,u)+w_k；

其中，下标k表示第k时刻，下标k+1表示第k+1时刻，x_k表示k时刻的状态量，x_k+1表示k+1时刻的状态量，w_k～(0，Q)为零均值、协方差为Q的白噪声，表示预测误差；u表示输入量[δ，a_x]^T；

所述无迹卡尔曼滤波器的观测方程为：

z_k＝h(x_k,u)+v_k；

其中，z_k表示k时刻的观测量，v_k～(0，R)为零均值、协方差为R的白噪声，表示观测误差。

可选的，所述采用M估计器算法，计算权重因子，具体包括：

根据以下公式计算权重因子；所述公式为：

其中，w为权重因子；e＝v_k，e在无迹卡尔曼滤波算法中被称为新息；

表示估计规模；ξ为阈值，用来界定观测信号中是否出现离群点；θ表示影响方程；

当

时，表示观测信号中没有离群点，影响方程为

当

时，表示观测信号中出现离群点，影响方程变为

本发明还提供了一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统包括：

建立模块，用于建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型；

方程确定模块，用于根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度；

权重因子计算模块，用于采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差；

估计模块，用于结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

可选的，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统还包括：

无迹变换模块，用于对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供了一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法及系统，该方法包括：建立三自由度车辆动力学模型；三自由度车辆动力学模型为与质心侧偏角相关的动力学模型；根据三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定无迹卡尔曼滤波器的输入量、状态量以及观测量；输入量包括前轮转角以及纵向加速度；状态量包括质心侧偏角；观测量包括侧向加速度和横摆角速度；采用M估计器算法计算权重因子；权重因子为观测量、状态量与观测量的协方差的权重因子；结合权重因子、当前时刻的状态量、无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。本发明通过将M估计器算法与无迹卡尔曼滤波算法相结合，能够抑制离群点带来的影响，提高无迹卡尔曼滤波算法对观测信号中离群点的鲁棒性，进而提高车辆质心侧偏角的估计准确度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法的流程示意图；

图2为本发明权重因子变化示意图；

图3为本发明影响方程变化示意图；

图4为本发明鲁棒无迹卡尔曼循环滤波过程示意图；

图5为本发明实施例基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统的流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提拱了一种基于鲁棒无迹卡尔曼滤波算法的质心侧偏角估计方法及系统，将M估计器算法与无迹卡尔曼滤波算法相结合，能够抑制离群点带来的影响，提高无迹卡尔曼滤波算法对观测信号中离群点的鲁棒性，进而提高车辆质心侧偏角的估计准确度。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

M估计器算法的思想来源于J.Huber于1964年的论文“Robust Estimation of aLocation Parameter”，原文中叫做：M-estimator，直译为M估计器。

图1为本发明实施例基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法的流程示意图，如图1所示，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法具体包括以下几个步骤：

步骤101：建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型。

步骤102：根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度。

步骤103：采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差。

步骤104：结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法还包括步骤105。在执行步骤104之前，先执行步骤105。

步骤105：对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

步骤101具体包括：

建立三自由度车辆动力学初始模型；该初始模型如公式(1)所示。

其中，F_x，F_y，M_z代表纵向力、侧向力和横摆转矩，v_x，v_y，I_z代表纵向速度、侧向速度和绕z轴转动惯量，m为整车质量，γ为横摆角速度。

进一步，对三自由度车辆动力学初始模型进行变形，得到三自由度车辆动力学模型，如公式(2)所示。

其中，

为质心侧偏角的导数；

为横摆角速度的导数；

为纵向加速度的导数；k1，k2分别为前、后轮胎的侧偏刚度；m为整车质量；v_x为纵向速度；β为质心侧偏角；a，b分别为质心到前、后轴的距离；γ为横摆角速度；δ为前轮转角；I_z为绕z轴转动惯量；a_x为纵向加速度。

步骤102具体包括：基于公式(2)，确定无迹卡尔曼滤波器的状态方程。

该状态方程为：x_k+1＝f(x_k,u)+w_k(3)；

其中，下标k表示第k时刻，下标k+1表示第k+1时刻，因此x_k表示k时刻的状态量，x_k+1表示k+1时刻的状态量，w_k～(0，Q)为零均值、协方差为Q的白噪声，表示预测误差；u表示输入量[δ，a_x]^T，包括两个变量：前轮转角δ和纵向加速度a_x。状态量x＝[β,γ,v_x]^T包括三个变量：质心侧偏角β，横摆角速度γ，纵向车速v_x。上述三个变量共同组成了状态量x，因此对状态量x采用无迹卡尔曼滤波算法进行循环计算的过程中，对上述三个变量都进行了估计。但是本发明仅关注状态量中第一个变量(即质心侧偏角β)的估计结果。

无迹卡尔曼滤波器的的观测方程为：

基于公式(4)，将无迹卡尔曼滤波器的的观测方程表示为：z_k＝h(x_k,u)+v_k(5)；其中，观测量z＝[a_y,γ]^T包括两个变量：侧向加速度a_y和横摆角速度γ，z_k表示k时刻的观测量，v_k～(0，R)为零均值、协方差为R的白噪声，表示观测误差。

步骤103具体包括：

根据M估计器算法，将传统最小二乘法的目标函数

进行改进，变为

其中Ax≈B表示系统方程，下标i表示离散系统中的第i个采样点，ε表示权重因子，该权重因子的计算方法如公式(6)所示。

其中，e＝v_k，e在卡尔曼滤波算法中被称为新息。

表示估计规模，ξ为阈值，用来界定观测信号中是否出现了离群点。当

时，表示观测信号中没有离群点，权重因子取

能够与传统无迹卡尔曼滤波保持一致；当

时，表示观测信号中出现离群点，因此需要减小权重因子，从而抑制离群点的影响，此时权重因子取为

对公式(6)求导可以得到影响方程θ，如公式(7)所示。

当

时，表示观测信号中没有离群点，影响方程为

能够与传统无迹卡尔曼滤波保持一致；当

时，表示观测信号中出现离群点，因此需要减小权重因子，从而抑制离群点的影响，此时影响方程变为

表示趋于饱和值。公式(6)-公式(7)的变化如图2、图3所示。

将公式(7)的分段函数形式整合成权重的形式，得到公式(8)。

步骤105具体包括：

无迹变换方法

假设变量x的均值为

协方差为P。对该变量x进行无迹变换得到2n+1个sigma点χ_(i)，如公式(9)所示。

分别计算2n+1个sigma点的权值，如公式(10)所示。

其中上标为m的权值应用于状态量x的预测和更新过程，上标为c的权值应用于协方差P的更新。λ＝a²(n+κ)-n是一个缩放比例函数，a(0.0001≤a≤1)表示采样点的分布状态，针对高斯分布，取b＝2为最优值。κ为二阶比例参数，取

步骤104具体包括：

Step1、初始化状态量及其误差协方差：

其中，x₀表示状态量初值，

表示状态量初值的估计值，

表示状态量初值估计误差的协方差。

Step2、按照公式(9)-(10)，对状态量进行无迹变换，并计算sigma点的权值。

Step3、根据无迹卡尔曼滤波器的状态方程，对sigma点进行预测计算，得到sigma点的预测值。

Step4、对sigma点的预测值进行加权求和得到状态量的预测值及误差协方差矩阵：

其中，

表示对状态量在k+1时刻的预测值，即：状态量的先验估计。

表示对状态量在k+1时刻的预测值的预测误差的协方差，即：误差协方差的先验估计。

Step5、对上一步计算出的状态量的预测值进行无迹变换；

Step6、根据无迹卡尔曼滤波器的观测方程，对上一步无迹变换后的sigma点进行观测计算，得到sigma点的观测值，并对sigma点的观测值加权求和计算得到观测量：

其中

表示在k+1时刻sigma点的观测值，

表示在k+1时刻的观测量。

Step7、计算观测量以及状态量和观测量之间的协方差。由于协方差的大小与卡尔曼增益的大小正相关，即协方差越大，观测量对参数估计结果的影响越大。因此，将公式(8)计算得到的权重应用于协方差计算，如式(19)-(20)所示。

Step8、计算卡尔曼增益：

Step9、更新状态量的预测值及误差协方差矩阵：

其中，

表示对状态量在k+1时刻的更新值，即：状态量的后验估计。P_k+1表示状态量在k+1时刻的更新值的误差协方差，即：误差协方差的后验估计。

至此，得到了k+1时刻质心侧偏角的估计结果，即：Step9中的状态量更新值中的第一个变量——质心侧偏角估计结果。当进入下一时刻(k+2)，将Step9中更新后的k+1时刻的状态量和误差协方差，代回Step2，开始进行下一轮计算，当再次计算到Step9时，能够得到(k+2)时刻的质心侧偏角估计结果，以此类推，不断循环计算，得到了每一时刻的质心侧偏角估计结果，即为本发明的目的。如式(19)-(23)所示，当观测量中存在离群点时，权重较小，使得协方差减小，进而卡尔曼增益也减小，状态量的更新计算对观测量的依赖较小，即为离群点对状态量的估计影响较小。因此，所述方法对观测量中的离群点具有鲁棒性。

所述鲁棒无迹卡尔曼滤波过程如图4所示：首先进行粗线箭头部分：由车辆模型写出UKF的状态方程和状态量，经过对状态量的无迹变换得到2n+1个sigma点，并分别计算sigma点的权值。对sigma点运行状态方程求得其预测值，加权求和得到状态量的预测量和误差协方差。然后进行细线箭头部分，将状态量的预测量重新进行无迹变换，得到一组新的2n+1个sigma点，对sigma点运行观测方程求得其观测值，加权求和得到观测量。而后引入鲁棒性方法计算得到的权重w，用来计算观测量的协方差以及状态量和观测量之间的协方差。最后通过计算卡尔曼增益来更新协方差和状态量，得到的状态量更新值，即为该时刻对质心侧偏角的估计值。将更新后的协方差和状态量代回到无迹变换模块，进行下一轮循环计算，用来预测下一时刻的质心侧偏角。

为实现上述目的，本发明还提供了一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统。

图5为本发明实施例基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统的结构示意图，如图5所示，本发明实施例提供的基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统包括：

建立模块100，用于建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型。

方程确定模块200，用于根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度。

权重因子计算模块300，用于采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差。

估计模块400，用于结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

无迹变换模块500，用于对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

本发明提出了一种基于鲁棒无迹卡尔曼滤波算法的质心侧偏角估计方法及系统，用来估计车辆质心侧偏角，通过将M估计器与无迹卡尔曼滤波算法相结合，能够抑制离群点带来的影响，提高无迹卡尔曼滤波算法对观测信号中离群点的鲁棒性。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法，其特征在于，所述质心侧偏角估计方法包括：

建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型；

根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度；

采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子；所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差；根据以下公式计算权重因子；所述公式为：

其中，w为权重因子；e＝v_k，e在无迹卡尔曼滤波算法中被称为新息，v_k～(0，R)为零均值、协方差为R的白噪声，表示观测误差；

表示估计规模；ξ为阈值，用来界定观测信号中是否出现离群点；θ表示影响方程；

当

时，表示观测信号中没有离群点，影响方程为

当

时，表示观测信号中出现离群点，影响方程变为

结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

2.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法，其特征在于，在结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计之前，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法还包括：

对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

3.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法，其特征在于，所述建立三自由度车辆动力学模型，具体包括：

根据以下公式建立三自由度车辆动力学模型；所述公式为：

其中，

为质心侧偏角的导数；

为横摆角速度的导数；

为纵向加速度的导数；k1，k2分别为前、后轮胎的侧偏刚度；m为整车质量；v_x为纵向速度；β为质心侧偏角；a，b分别为质心到前、后轴的距离；γ为横摆角速度；δ为前轮转角；I_z为绕z轴转动惯量；a_x为纵向加速度。

4.根据权利要求3所述的基于鲁棒无迹卡尔曼滤波的质心侧偏角估计方法，其特征在于，所述无迹卡尔曼滤波器的状态方程为：

x_k+1＝f(x_k,u)+w_k；

其中，下标k表示第k时刻，下标k+1表示第k+1时刻，x_k表示k时刻的状态量，x_k+1表示k+1时刻的状态量，w_k～(0，Q)为零均值、协方差为Q的白噪声，表示预测误差；u表示输入量[δ，a_x]^T；

所述无迹卡尔曼滤波器的观测方程为：

z_k＝h(x_k,u)+v_k；

其中，z_k表示k时刻的观测量，v_k～(0，R)为零均值、协方差为R的白噪声，表示观测误差。

5.一种基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统，其特征在于，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统包括：

建立模块，用于建立三自由度车辆动力学模型；所述三自由度车辆动力学模型为与质心侧偏角相关的动力学模型；

方程确定模块，用于根据所述三自由度车辆动力学模型，确定无迹卡尔曼滤波器的状态方程和观测方程，并确定所述无迹卡尔曼滤波器的输入量、状态量以及观测量；所述输入量包括前轮转角以及纵向加速度；所述状态量包括质心侧偏角、横摆角速度和纵向车速；所述观测量包括侧向加速度和横摆角速度；

权重因子计算模块，用于采用M估计器算法，计算权重因子；所述权重因子为协方差的权重因子所述协方差包括所述观测量的协方差以及所述状态量和所述观测量之间的协方差；在所述权重因子计算模块中是根据以下公式计算权重因子；所述公式为：

其中，w为权重因子；e＝v_k，e在无迹卡尔曼滤波算法中被称为新息，v_k～(0，R)为零均值、协方差为R的白噪声，表示观测误差；

表示估计规模；ξ为阈值，用来界定观测信号中是否出现离群点；θ表示影响方程；

当

时，表示观测信号中没有离群点，影响方程为

当

时，表示观测信号中出现离群点，影响方程变为

估计模块，用于结合所述权重因子、当前时刻的所述状态量、所述无迹卡尔曼滤波器的状态方程和观测方程，采用无迹卡尔曼滤波算法，对下一时刻的质心侧偏角进行估计。

6.根据权利要求5所述的基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统，其特征在于，所述基于鲁棒无迹卡尔曼滤波的质心侧偏角估计系统还包括：

无迹变换模块，用于对当前时刻的所述状态量进行无迹变换，得到多个sigma点，并计算每个所述sigma点的权值。

Images