卡尔曼_扩展卡尔曼滤波算法实例解析
本文将以车辆三自由度模型为基础，利用扩展卡尔曼滤波，通过车辆的侧向加速度来估计横摆角速度、质心侧偏角、纵向速度等三个参数，通过一个实际的仿真案例来进行具体介绍扩展卡尔曼滤波的使用。
一般地，卡尔曼滤波会选择比较容易获取的参数，来估计不易测量的参数。
在这里，脚主把卡尔曼参数估计仿真分为四个步骤：
1)车辆模型搭建；
2)扩展卡尔曼滤波算法搭建；
3)模型整合及仿真工况设置；
4)仿真及结果分析。
车辆模型搭建
本例中，利用车辆三自由度模型(如下图)进行参数估计，需要知道车辆的输入信号(车轮转角、纵向加速度)和输出信号(侧向加速度)，所以需要自己搭建一个车辆模型来创造这些数据。即对车辆模型输入一个方向盘转角和纵向加车速，得到侧向加速度。
在实车上，这一步是可以忽略的，因为我们可以通过传感器直接测量卡尔曼滤波所需的信号。
脚主暂时选择比较简便的方法，借助carsim中的车辆模型来完成这项工作，仅需要设置好我们关注的车辆基本参数及信号接口即可。
质心到前后轴距离、沿Z轴转动惯量、质量设置位置如下图。
前后轮侧偏刚度设置位置如下图。
方向盘转角到车轮转角的传动比设置位置如下图。
输出接口选择输出横摆角速度、质心侧偏角、纵向车速、方向盘转角、纵向加速度、侧向加速度。前三个是待估计的参数，用于与仿真结果对比；后三个是车辆的输入输出信号，会作为卡尔曼滤波算法的输入。
一直用别人的车辆模型也不合适，后面脚主会自己动手搭建车辆模型，这样就可以避免联合仿真的麻烦，仿真可以全部在simulink中实现了。更重要的是自己搭建车辆模型更加能加深对车辆的理解，这个是商业软件所无法替代的。
扩展卡尔曼滤波算法搭建
扩展卡尔曼滤波算法就是把上文提到的5个核心公式表达出来。再次强调一下：需要使用非线性函数f、h来表示状态方程和输出方程；系统矩阵A、输出矩阵H需要用f、h函数求偏导后的雅克比矩阵表示。
脚主这里借助以前搭建的卡尔曼滤波算法，稍微改动一下，得到如下图的扩展卡尔曼滤波算法。
用5个function表达卡尔曼滤波算法其实比较繁琐，但是可以更好地表达5个公式之间的时序关系，便于初学者理解。
模型整合及仿真工况设置
把上述两部分内容组合起来就是整个基于扩展卡尔曼滤波的参数估计仿真模型，如下图。基本思路就是，carsim模型输出滤波算法所需的信号，然后进行参数估计，输出估计的结果，最后将估计结果与车辆实际信号对比，来验证算法的有效性。
为了验证算法，还需要在carsim中设置一下仿真工况。
1)初始车速为30km/h进行滑行。
2)方向盘转角按下图的曲线执行。
仿真及结果分析
运行模型，得到估计的横摆角速度、质心侧偏角、纵向车速，与车辆实际的状态对比如下图。
横摆角速度估计结果：
质心侧偏角估计结果：
纵向车速估计结果：
从图中可以看出，横摆角速度、质心侧偏角、纵向车速的估计值与实际值基本一致，算法可靠有效。
以上，介绍了扩展卡尔曼滤波算法进行参数估计的一个实例，仅供大家参考。