在重型货车研发过程中，为了节约传感器成本，就不会安装质量传感器和坡度传感器，而是通过数学公式进行质量估算和坡度估算。工程中常用的方法是扩展卡尔曼滤波和最小二乘法。步骤如下：

1、利用汽车纵向动力学公式：Fx = mgf + 0.5pCdAv^2 + mgθ + m*dv/dt    （公式1），能够发现坡度助力mgθ中质量和坡度信号是耦合的，所以需要对两个变量进行解耦。

     在重型货车上，线控底盘、L2智能驾驶很多地方会用到X,Y,Z三个方向的加速度，所以车上一般都会装有IMU传感器，其测量的加速度有如下关系：

a = gθ + dv/dt     （公式2），带入动力学公式后，得到如下公式：

m = （Fx - 0.5pCdAv^2）/（a + gf）    （公式3）

       利用已知的车辆纵向力、车速、加速度信号，就可以先估算出车辆质量。质量几乎是不会变化的，接下来利用扩展卡尔曼滤波估算坡度。

2、估算出质量，且质量变化不大，则默认质量为常量。对纵向动力学公式变型可得：

(公式4)

公式展开为（公式5）

考虑到公路设计规范，道路坡度一般较小，则sina = i ，（公式5）变为

（公式6）

      接下来是扩展卡尔曼算法的实现：选取状态变量为车速、质量、坡度，其中质量和坡度变化小，导数近似为0，则微分方程组为：

（公式7）

利用前向欧拉，如果不懂的话可以理解为y1=y0+Δy*Δt ,得到离散化差分方程：

（公式8） 

假设过程噪声为Wk , 测量噪声为Vk,他们是相互独立，均值为零的高斯白噪声（服从正义分布），过程噪声协方差矩阵为Qk，测量噪声协方差矩阵为Rk。常规的状态空间方程表达式为：

(公式9)

本文中，（公式10）

其中，

 以车速作为观测量，则系统测量方程为：

（公式11）

（公式10）与（公式11)组成状态空间表达式：（公式12）,式中，H是测量矩阵。

          由（公式12) ，按照 EKF 算法对车辆质量 m、道路 坡度 i 进行估计。EKF 包括两个计算过程: 时间更 新和测量更新。时间更新方程向前推算先验状态估 计值和先验误差协方差; 测量更新方程将先验状态 估计和测量变量结合产生状态的后验估计并更新估 计状态的后验误差协方差。算法递归进行，只需获 得上一时刻状态变量的估计值和当前状态变量的测 量值便可获得当前状态变量的估计值。算法流程如 图 所示

 卡尔曼增益根据测量噪声协方差 Ｒk 以及先验 误差协方 差 Pk 动态调节测量变量 zk 和 其 估 计 Hx^k 的权重。

3、结果展示：

最近好多悠悠找我要模型，本来我是不太愿意给的，毕竟这是劳动成功，但是为了能让更多初学者深刻的学习卡尔曼滤波的过程原理，我还是将模型放上来了。

1、计算速度的导数，与坡度、质量的导数做成矩阵。

2、计算先验估计值

3、计算雅可比矩阵

4、计算先验协方差

5、计算K增益

6、计算后验估计值

7、计算后验协方差

到此模型就搭完了，可以carsim与simulink联合仿真，看看估算效果辣。