CN107985318B - 轮式装载机重心纵向位置和重心高度动态估计方法 - Google Patents

Abstract

本发明公开了一种轮式装载机重心纵向位置和重心高度动态估计方法，首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩以及测量得到的轮胎滚动半径，通过非线性滤波器估计得到纵向速度；然后依据通过传感器测量得到的纵向加速度和路面坡度、估计得到的纵向速度、通过计算得到的前后轮胎滑移率以及通过查表或计算得到的整车质量和铲掘阻力，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到动态的重心纵向位置和重心高度。本发明所述方法为前后桥独立电驱动轮式装载机智能动力学控制提供准确的重心动态位置参数，提升整车控制精度和性能。

Description

轮式装载机重心纵向位置和重心高度动态估计方法

技术领域

本发明属于轮式装载机状态参数估计技术领域，具体涉及一种轮式装载机重心纵向位置和重心高度动态估计方法，适用于前后桥独立电驱动轮式装载机(Front/Rear AxleElectric Wheel Loader,简称FREWL)。

背景技术

近年来，新能源轮式装载机日益得到国内外政府和工程机械厂商的重视。其中，动力学智能控制领域的研究是新能源轮式装载机的重点研究方向之一。为实现整车动力学智能控制，需要向控制系统在线提供整车各种关键的状态参数。然而，这些关键的状态参数有一部分无法通过传感器在线测量得到。其中，整车静态重心位置虽然可以通过静态力学方法测量得到，但是动态重心位置在作业工况中随作业时工作姿态数值变化和铲装载荷数值变化而变化，且变化幅度和频率较大。因此，通过先验估计方法在线估计重心纵向位置和重心高度是实现整车动力学智能精确控制的前提。

现有的估计方法主要通过递推最小二乘法或扩展卡尔曼滤波方法估计车辆重心位置，但该方法存在以下两个主要缺点：

1、该重心估计的方法主要应用于公路车辆领域，由于工作装置和铰接结构的存在，轮式装载机的动力学模型与公路车辆的动力学模型有较大的区别；

2、递推最小二乘法和扩展卡尔曼滤波方法通常适用于非线性不强的数学问题。由于轮式装载机在作业时重心纵向位置和重心高度波动剧烈，变化频率和波动幅度较大，其动力学模型非线性较强。

以上原因导致现有的估计方法在轮式装载机重心纵向位置和高度的动态估计上难以得到良好的估计精度。由此可见，在本技术领域，重心纵向位置和重心高度的动态估计方法需进行改进。

因此，为实现作业过程中装载机重心位置的准确动态估计，提高整车动力学智能控制精度和效果，急需一种适用于轮式装载机的重心纵向位置和高度动态估计方法。

发明内容

针对上述现有技术中存在的问题，本发明提出一种适用于前后桥独立电驱动的轮式装载机重心纵向位置和重心高度动态估计方法，为前后桥独立电驱动轮式装载机智能动力学控制提供准确的重心动态位置参数，提升整车控制精度和性能。本发明所述方法适用于各种作业工况中纵向运动的前后桥独立电驱动轮式装载机，且使用的传感器数量少，价格低廉。结合说明书附图，本发明的技术方案如下：

轮式装载机重心纵向位置和重心高度动态估计方法，所述动态估计方法首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩T_f,r以及测量得到的轮胎滚动半径r_eff，通过非线性滤波器估计得到纵向速度v_x；然后依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度v_x、通过计算得到的前后轮胎滑移率σ_f,r以及通过查表或计算得到的整车质量m和铲掘阻力纵向分力F_x和铲掘阻力垂向分力F_z，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h。

所述动态估计方法具体步骤如下：

一、传感器布置：

在装载机的静态重心位置安装加速度传感器和坡度传感器，用于测量装载机的纵向加速度和装载机行驶时路面坡度的变化；

在装载机桥壳上安装轮速传感器，用于测量轮胎转速；

二、纵向速度v_x的估计：

基于前后桥独立电驱动轮式装载机的前后电机输出转矩实时可知的特点，假设车辆只是纵向行驶，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：

y(t＝0)＝y₀………………………………(2)

公式(1)中，v_x(t)是车辆的纵向速度，x(t)是前后车轮角速度ω_f,r与轮胎滚动半径r_eff的乘积，前后车轮角速度ω_f,r通过轮速传感器测量得到，轮胎滚动半径r_eff通过测量得到；T_f,r是前后桥电机输出转矩，L系统参数，d是自定义值；

三、推导动力学模型：

基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力F_x、前轮纵向力F_xf、后轮纵向力F_xr、前轮滚动阻力R_xf、后轮滚动阻力R_xr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σ_f和后轮纵向滑移率σ_r的计算关系如下：

上述关系式(3)中，铲掘阻力纵向分力F_x通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到；

基于力矩平衡原理，前轮垂向力F_zf、后轮垂向力F_zr与重心到前轮中心的纵向距离l_f、重心到后轮中心的纵向距离l_r、前轮中心到铲斗铲尖的纵向距离l_c、重心高度h、铲掘阻力垂向分力F_z的计算关系如下：

前轮角加速度后轮角加速度和轮胎转动惯量I_e、前轮驱动转矩T_f、后轮驱动转矩T_r、轮胎滚动半径r_eff、前轮纵向力F_xf和后轮纵向力F_xr的计算关系如下：

基于公式(3)、(4)、(5)、(6)和(7)，令l＝l_f+l_r，l是前后轮轴距，l_l＝l_f+l_r+l_c，l_l是铲斗铲尖到后轮中心纵向距离，λ＝l_r/l，λ是重心纵向位置。重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系如下：

上述公式(8)、(9)和(10)即为所需动力学模型；

四、改进型平方根无迹卡尔曼滤波：

(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：

离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：

x_k+1＝F_kx_k+w_k……………………………………(11)

z_k+1＝H_k+1x_k+1+v_k+1………………………………(12)

上述方程(11)和(12)中，F_k是状态转移矩阵，H_k+1是量测矩阵；w_k和v_k+1是相互独立的过程噪声和量测噪声，Q_k是w_k的协方差矩阵，R_k+1是v_k+1的协方差矩阵，x_k是状态变量，z_k是量测量；

扩充状态变量x_k，得到x_k＝[v_x ω_f ω_r K h λ]^T；

量测矩阵H_k+1定义为：

k+1时刻的状态量x_k+1与k时刻的状态量x_k、采样时间Δt的关系如下所示：

关系式(14)中，是x_k的导数；

综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，得到状态方程(15)和量测方程(16)如下所示：

(2)、改进型平方根无迹卡尔曼滤波递推：

在递推运算过程中，首先定义x_k的初值以及协方差矩阵P_k的初值P₀，具体计算公式如下：

式中，表示估计值；

再计算2n+1个Sigma点x_k|k：

上述式(18)中，n是状态的维数，表示协方差矩阵的第i列，λ＝δ²(n+κ)-n，λ＝δ²(n+κ)-n是一个缩放比例参数，用来降低预测误差；δ是一个常数，其取值控制了采样点的分布；κ是待选参数的，其取值保证(n+λ)P_k是一个半正定矩阵，计算2n+1个Sigma点x_k|k所对应的权值：

上述式(19)中，ω_c是协方差对应的权重；ω_m是均值对应的权重；β是一个非负的权重系数；

根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：

根据上述公式(19)和(20)，计算状态变量的下一步预测和协方差矩阵P_k+1|k：

上述公式(22)中，Q_k是过程噪声；qr表示对矩阵进行QR分解；

基于公式(13)和(20)，计算得到Sigma点集预测的量测值

由上述公式(19)和(23)，通过加权求和得到系统量测的均值

基于公式(19)、(20)、(21)、(23)和(24)，进一步计算系统预测的协方差：

根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵K_k：

根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：

根据前述公式(13)和(22)，系统的协方差更新为：

经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，即动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。

与现有技术相比，本发明的有益效果在于：

1、本发明提出了一种低成本、高精度、实时性好的适用于各种作业工况下前后桥独立电驱动轮式装载机纵向运动时的重心纵向位置和重心高度动态估计方法；

2、本发明利用改进型平方根无迹卡尔曼滤波估计重心纵向位置和重心高度，适用于前后桥独立电驱动轮式装载机的动力学模型和作业工况，尤其适用于由于铲掘和工作装置举升时重心纵向位置和重心高度波动剧烈、变化频率和波动幅度较大时的作业工况，克服递推最小二乘法或扩展卡尔曼滤波方法在非线性较强的问题上估计精度较差的问题，同时克服了传统无迹卡尔曼滤波算法在线估计时出现的协方差非正定问题和Cholesky因子非正定问题；

3、本发明仅需一个加速度传感器、一个轮速传感器和一个坡度传感器即可实现重心纵向位置和重心高度动态估计，具有成本低的优点，便于大规模推广。

附图说明

图1为本发明所述轮式装载机重心纵向位置和重心高度动态估计方法的流程框图；

图2为适用本发明所述方法的前后桥独立电驱动轮式装载机结构原理简图；

图3为适用本发明所述方法的前后桥独立电驱动轮式装载机受力分析图。

具体实施方式

为进一步阐述本发明所述的技术方案，结合说明书附图，本发明的具体实施方式如下：

本发明所述的轮式装载机重心纵向位置和重心高度动态估计方法简述如下：

基于前后电机实时可知的电机输出转矩T_f,r以及测量得到的轮胎滚动半径r_eff，通过非线性滤波器估计得到纵向速度v_x；依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度v_x、通过计算得到的前后轮胎滑移率σ_f,r以及通过查表或计算得到的整车质量m和铲掘阻力F，搭建状态方程和量测方程，并通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h。

所述动态估计方法具体步骤如下：

一、传感器布置：

根据本发明所述的动态估计方法所搭建的硬件系统中，包括一个加速度传感器、至少一个轮速传感器以及一个坡度传感器。所述加速度传感器安装在前后桥独立电驱动轮式装载机的静态重心位置附近固定位置，其用于测量前后桥独立电驱动轮式装载机的纵向加速度所述轮速传感器安装在桥壳上，其用于测量轮胎转速；所述坡度传感器安装在静态重心位置附近固定位置，其用于测量车辆行驶时路面坡度α的变化。

二、纵向速度v_x的估计：

如图1所示，前电机输出转矩T_f、后电机输出转矩T_r、纵向速度v_x、前轮纵向滑移率σ_f、后轮纵向滑移率σ_r、纵向加速度整车质量m、路面坡度α、铲掘阻力纵向分力F_x和铲掘阻力垂向分力F_z是进行重心纵向位置和重心高度动态估计所需的重要状态参数。

上述状态参数除了纵向速度v_x外，其他参数都可以通过计算或传感器直接测量得到。在量产车辆上想要通过传感器直接测量纵向速度v_x却很难实现。这是因为普通车载GPS由于存在采样频率低和延时现象，很难提供精确的纵向车速数值。车载惯性导航系统(INS)虽然可以测量精确的纵向速度，但价格昂贵，很难在量产车上安装。因此，需要一种可以低成本且可在线估计纵向速度的方法。

本发明基于前后电机输出转矩实时可知的特点，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：

y(t＝0)＝y₀………………………(2)

公式(1)中，假设车辆只是纵向行驶，则v_x(t)是车辆的纵向速度，x(t)是前后车轮角速度ω_f,r与轮胎滚动半径r_eff的乘积，前后车轮角速度ω_f,r通过轮速传感器测量得到，轮胎滚动半径r_eff通过测量得到。T_f,r是前后桥电机输出转矩，L是一个系统参数，本实施例中取值20。d是一个很小的自定义值，本实施例中取值0.01。

三、推导车辆重心纵向位置和重心高度动态估计所需动力学模型：

因为轮式装载机主要工作在低速区域，其空气阻力数值较小，因此本发明忽略空气阻力。如图2和图3所示，依据FREWL前后电机分别驱动前后桥的驱动特点，基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力F_x、前轮纵向力F_xf、后轮纵向力F_xr、前轮滚动阻力R_xf、后轮滚动阻力R_xr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σ_f和后轮纵向滑移率σ_r的计算关系如下：

关系式(3)中，铲掘阻力纵向分力F_x通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到。

基于力矩平衡原理，前轮垂向力F_zf、后轮垂向力F_zr与重心到前轮中心的纵向距离l_f、重心到后轮中心的纵向距离l_r、前轮中心到铲斗铲尖的纵向距离l_c、重心高度h、铲掘阻力垂向分力F_z的计算关系如下：

因为前后桥电机都为独立驱动，前轮角加速度后轮角加速度和轮胎转动惯量I_e、前轮驱动转矩T_f、后轮驱动转矩T_r、轮胎滚动半径r_eff、前轮纵向力F_xf和后轮纵向力F_xr的计算关系如下：

基于公式(3)、(4)、(5)、(6)和(7)，令l＝l_f+l_r，l是前后轮轴距，l_l＝l_f+l_r+l_c，l_l是铲斗铲尖到后轮中心纵向距离距离，λ＝l_r/l，λ是重心纵向位置。重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系，即上述公式(3)、(6)和(7)可重新定义为以下形式：

上述公式(8)、(9)和(10)即为本发明推导车辆重心纵向位置和重心高度动态估计所需动力学模型，也就是在后续改进型平方根无迹卡尔曼滤波递推运算中所需要的动力学模型。

四、改进型平方根无迹卡尔曼滤波：

在列出改进型平方根无迹卡尔曼滤波递推运算中所需的动力学模型后，还需要进行两步去在线估计轮式装载机重心纵向位置和重心高度；第一步：基于前述公式(8)、(9)和(10)，建立改进型平方根无迹卡尔曼滤波所需的状态方程和量测方程；第二步：将状态方程和量测方程带入改进型平方根无迹卡尔曼滤波的递推算法中，在线估计重心纵向位置和重心高度。具体过程如下：

(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：

离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：

x_k+1＝F_kx_k+w_k……………………………………(11)

z_k+1＝H_k+1x_k+1+v_k+1………………………………(12)

上述方程(11)和(12)中，F_k是状态转移矩阵，H_k+1是量测矩阵；w_k和v_k+1是相互独立的过程噪声和量测噪声，Q_k是w_k的协方差矩阵，R_k+1是v_k+1的协方差矩阵，本实施例中所选用的Q_k矩阵数值为diag(0.0001,0.0001,0.000001,0.0001,0.001,0.0001)，所选用的R_k+1矩阵数值为diag(0.01,0.0001,0.0001,0,0,0)，x_k是状态变量，z_k是量测量。

由公式(8)、(9)和(10)可知公式(11)中的一部分状态变量可以是纵向加速度前轮角加速度和后轮角加速度然而，为了估计重心纵向位置λ、重心高度h和轮胎滑移系数K，需要扩充状态变量x_k，得到x_k＝[v_x ω_f ω_r K h λ]^T。因为重心纵向位置λ、重心高度h和轮胎滑移系数K是待估计量，只有纵向加速度前轮角加速度和后轮角加速度可以测量得到或根据前述步骤估计得到，因此本发明量测矩阵H_k+1为：

k+1时刻的状态量x_k+1与k时刻的状态量x_k、采样时间Δt的关系如下所示：

关系式(14)中，是x_k的导数。

综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，可以得到用于本发明改进型平方根无迹卡尔曼滤波的状态方程(15)和量测方程(16)如下所示：

(2)、改进型平方根无迹卡尔曼滤波递推：

在得到改进型平方根无迹卡尔曼滤波所需的状态方程(15)和量测方程(16)后，通过改进型平方根无迹卡尔曼滤波递推运算在线估计整车重心纵向位置和重心高度。

在递推运算过程中，首先定义x_k的初值以及协方差矩阵P_k的初值P₀，具体计算公式如下：

式中，表示估计值；

再计算2n+1个Sigma点x_k|k：

式中，n是状态的维数，表示协方差矩阵的第i列，λ＝δ²(n+κ)-n，λ＝δ²(n+κ)-n是一个缩放比例参数，被用来降低预测误差。δ是一个常数，其取值控制了采样点的分布，本发明取0.01。κ是待选参数的，其取值通常应该保证(n+λ)P_k是一个半正定矩阵，本实施例中κ取值为0。计算2n+1个Sigma点x_k|k所对应的权值：

式中，ω_c是协方差对应的权重；ω_m是均值对应的权重；β是一个非负的权重系数，本实施例中β取值为2。

根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：

根据上述公式(19)和(20)，计算状态变量的下一步预测和协方差矩阵

上述公式(22)中，Q_k是过程噪声；qr表示对矩阵进行QR分解。

基于公式(13)和(20)，计算得到Sigma点集预测的量测值

由上述公式(19)和(23)，通过加权求和得到系统量测的均值

基于公式(19)、(20)、(21)、(23)和(24)，为了保持对系统状态变量估计的更新，需进一步计算系统预测的协方差：

根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵K_k：

根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：

根据前述公式(13)和(22)，系统的协方差更新为：

经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，可实时、精确地动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。

Claims (1)

1.轮式装载机重心纵向位置和重心高度动态估计方法，其特征在于：

所述动态估计方法首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩T_f,r以及测量得到的轮胎滚动半径r_eff，通过非线性滤波器估计得到纵向速度v_x；然后依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度v_x、通过计算得到的前后轮胎滑移率σ_f,r以及通过查表或计算得到的整车质量m和铲掘阻力纵向分力F_x和铲掘阻力垂向分力F_z，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h；

所述动态估计方法具体步骤如下：

一、传感器布置：

在装载机的静态重心位置安装加速度传感器和坡度传感器，用于测量装载机的纵向加速度和装载机行驶时路面坡度的变化；

在装载机桥壳上安装轮速传感器，用于测量轮胎转速；

二、纵向速度v_x的估计：

基于前后桥独立电驱动轮式装载机的前后电机输出转矩实时可知的特点，假设车辆只是纵向行驶，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：

y(t＝0)＝y₀………………………………(2)

公式(1)中，v_x(t)是车辆的纵向速度，x(t)是前后车轮角速度ω_f,r与轮胎滚动半径r_eff的乘积，前后车轮角速度ω_f,r通过轮速传感器测量得到，轮胎滚动半径r_eff通过测量得到；T_f,r是前后桥电机输出转矩，L系统参数，d是自定义值；

三、推导动力学模型：

基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力F_x、前轮纵向力F_xf、后轮纵向力F_xr、前轮滚动阻力R_xf、后轮滚动阻力R_xr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σ_f和后轮纵向滑移率σ_r的计算关系如下：

上述关系式(3)中，铲掘阻力纵向分力F_x通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到；

基于力矩平衡原理，前轮垂向力F_zf、后轮垂向力F_zr与重心到前轮中心的纵向距离l_f、重心到后轮中心的纵向距离l_r、前轮中心到铲斗铲尖的纵向距离l_c、重心高度h、铲掘阻力垂向分力F_z的计算关系如下：

前轮角加速度后轮角加速度和轮胎转动惯量I_e、前轮驱动转矩T_f、后轮驱动转矩T_r、轮胎滚动半径r_eff、前轮纵向力F_xf和后轮纵向力F_xr的计算关系如下：

基于公式(3)、(4)、(5)、(6)和(7)，令l＝l_f+l_r，l是前后轮轴距，l_l＝l_f+l_r+l_c，l_l是铲斗铲尖到后轮中心纵向距离，λ＝l_r/l，λ是重心纵向位置，重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系如下：

上述公式(8)、(9)和(10)即为所需动力学模型；

四、改进型平方根无迹卡尔曼滤波：

(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：

离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：

x_k+1＝F_kx_k+w_k……………………………………(11)

z_k+1＝H_k+1x_k+1+v_k+1………………………………(12)

上述方程(11)和(12)中，F_k是状态转移矩阵，H_k+1是量测矩阵；w_k和v_k+1是相互独立的过程噪声和量测噪声，Q_k是w_k的协方差矩阵，R_k+1是v_k+1的协方差矩阵，x_k是状态变量，z_k是量测量；

扩充状态变量x_k，得到x_k＝[v_x ω_f ω_r K h λ]^T；

量测矩阵H_k+1定义为：

k+1时刻的状态量x_k+1与k时刻的状态量x_k、采样时间Δt的关系如下所示：

关系式(14)中，是x_k的导数；

综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，得到状态方程(15)和量测方程(16)如下所示：

(2)、改进型平方根无迹卡尔曼滤波递推：

在递推运算过程中，首先定义x_k的初值以及协方差矩阵P_k的初值P₀，具体计算公式如下：

式中，表示估计值；

再计算2n+1个Sigma点x_k|k：

上述式(18)中，n是状态的维数，表示协方差矩阵的第i列；λ＝δ²(n+κ)-n，λ＝δ²(n+κ)-n是一个缩放比例参数，被用来降低预测误差；δ是一个常数，其取值控制了采样点的分布；κ是待选参数的，其取值保证(n+λ)P_k是一个半正定矩阵，计算2n+1个Sigma点x_k|k所对应的权值：

上述式(19)中，ω_c是协方差对应的权重；ω_m是均值对应的权重；β是一个非负的权重系数；

根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：

根据上述公式(19)(20)，计算状态变量的下一步预测和协方差矩阵P_k+1|k：

上述公式(22)中，Q_k是过程噪声；qr表示对矩阵进行QR分解；

基于公式(13)和(20)，计算得到Sigma点集预测的量测值

由上述公式(19)和(23)，通过加权求和得到系统量测的均值

基于公式(19)、(20)、(21)、(23)和(24)，进一步计算系统预测的协方差：

根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵K_k：

根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：

根据前述公式(13)和(22)，系统的协方差更新为：

经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，即动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。