扩展卡尔曼滤波（EKF）浅谈

一个月前，有脚友提问：怎么基于车辆模型利用卡尔曼滤波算法进行车辆速度估计？

我当时没有回答，主要原因是实际车辆上一般都是基于轮速和纵向加速度等运动学的参数进行车速估计，简单实用。另外关于卡尔曼滤波的内容我也差不多忘了，好多年没用过捡起来也需要一点时间。

后来，有其他脚友也想了解卡尔曼相关的估计问题。。。

既然这么多人感兴趣，说明这个话题是有意义的。脚主决定花一些时间，恢复记忆，去给大家谈一谈个人对扩展卡尔曼滤波算法的理解。

在介绍扩展卡尔曼滤波器之前，有必要回忆一下之前介绍过的卡尔曼滤波器。

卡尔曼滤波主要根据系统状态方程，通过系统输出作为观测数据，来不断迭代修正预估的逻辑，实现对系统状态的最优估计。其核心的5个公式如下图：

其中，前两个为预测部分，后三个为校正部分。我们在进行卡尔曼滤波器设计时，重点是把系统的模型用状态方程表示出来：

测量结果用输出方程表示出来：

上述公式中的系统矩阵A、输出矩阵H就对应卡尔曼5个核心的公式的A、H，A用于状态预测，H用于量测后校正。

需要注意的是，卡尔曼滤波仅适用于线性系统。对于非线性系统，只能使用扩展卡尔曼滤波器，将非线性系统线性化处理。

如果一个系统的输入、输出满足叠加原理，该系统称为线性系统，否则为非线性系统。

扩展卡尔曼滤波可通过将非线性系统在其参考点处作泰勒级数展开，取其一阶线性部分作为该非线性模型的逼近，从而得到非线性系统在当前时刻的线性化描述。

非线性系统的状态方程一般可表示为：

输出方程表示为：

扩展卡尔曼滤波器的核心公式同卡尔曼滤波器基本一致，有两点小差异：

1、需要使用非线性函数f、h来表示状态方程和输出方程；

2、系统矩阵A、输出矩阵H需要用f、h函数求偏导后的雅克比矩阵表示。

回到最开始的问题，我们想基于车辆模型使用卡尔曼滤波器来进行车辆速度估计，怎么做呢？首先列出车辆模型的状态方程，如下图。

状态变量一定要包含你想估计的参数。

可以看出，上述车辆模型包含了纵向、横向、横摆三个自由度，对于系统状态变量[r B Vx]是非线性模型。

这里考虑使用比较容易获得的侧向加速度数据进行估计，所以输出方程选择：

按照上面的非线性模型的线性化处理，该三自由度模型的A、H求偏导后可表示为：

有了以上的系统状态矩阵和输出矩阵后，只要有了车辆的输入信号（车轮转角、纵向加速度），输出信号（侧向加速度），利用扩展卡尔曼滤波器就可以将车辆横摆角速度、质心侧偏角、纵向速度估计出来。

这里的输入和输出数据可以实车测量，可以和其他自带车辆模型的软件联合仿真，也可以自己搭建一个车辆模型创造数据。

以上，感兴趣的可以自己动手尝试一下，下周脚主将做一个实例展示一下。