之前有多位朋友留言，希望介绍一下车辆的质量与路面坡度估计，今天就来完成这个话题。去年介绍过卡尔曼滤波相关的车辆状态估计，这次依然会使用卡尔曼滤波来进行质量与路面坡度的估计。工程上最常用的估计算法就是最小二乘法和卡尔曼滤波。
本来打算用卡尔曼滤波同时估计出质量与坡度信息，不幸的是仿真测试效果并不理想，质量收敛的太慢，完全不像某些文章中写的那样好使，当然也可能是脚主参数设置的不太合理。所以，最后打算换一个思路，先用最小二乘法估计质量，再用卡尔曼滤波估计坡度。
汽车理论给出了汽车纵向动力学的基本公式：
Fx = Ff Fw Fi Fj
再写具体一点就是：
Fx = mgf 0.5pCdAv^2 mgθ m*dv/dt
可以看出，对于坡度阻力这一项，同时耦合了质量与坡度信号，没有办法直接使用最小二乘。
考虑到车辆上一般装有纵向加速度传感器，其测量的加速度a有以下关系：
a = gθ dv/dt
所以纵向动力学公式可以简化为以下形式：
Fx - 0.5pCdAv^2 = m(a gf)
根据这个公式，如果已知车辆纵向力、车速、加速度信号，就可以很方便利用最小二乘法估算出车辆质量。
估计出质量以后，就可以利用纵向动力学公式继续进行坡度的估计。
以车速、坡度为状态量，状态方程如下：
dv/dt = -0.5 pCdAv^2/m – gθ – gf Fx/m
dθ/dt = 0
很明显这个状态方程为非线性，需要进行线性化，可以对状态量求偏导计算雅克比矩阵，具体计算方法可参考前面的文章。
之后，利用车速作为测量输出，就可以利用扩展卡尔曼滤波估计出坡度值。
下面进行仿真测试。
首先需要一个车辆模型，以纵向力、坡度为输入，输出车速、加速度传感器信号，方便后面的估计算法使用。具体的车辆模型如下：
其实，也可以使用前两篇介绍的纵向动力学模型，这里暂时更相信Simulink自带的车辆模型吧。
给定一个的输入纵向力、坡度如下：
经车辆模型之后，输出的车速、传感器信号如下：
输出的车速、加速度信号增加了白噪声来模拟真实情况。
车辆模型输出的车速、加速度、纵向力信号给到最小二乘以及卡尔曼滤波算法，分别进行质量以及坡度估计，整个估计模型如下：
最终估计的质量如下图，估计的车辆质量很快就收敛，且稳定在真实质量附近，符合预期。
估计的坡度如下图，基本趋势符合实际坡度情况，估计噪声还可以进一步优化。
以上，以车辆纵向动力学公式为基础，利用最小二乘法、卡尔曼滤波分别进行了质量、坡度的估计，仅供参考。
