扩展卡尔曼滤波器实例与推导

1 前言

在 上一篇文章 中，我们介绍了线性卡尔曼滤波器的来源、推导及实例。但在 很多实际工程问题当中，非线性系统占大多数 ，而卡尔曼提出来的卡尔曼滤波器是一种针对线性系统的估计算[1]。 为了解决这一问题，Schmidt学者分析了卡尔曼滤波器中的系统模型部分，并提出 将卡尔曼理论应用到非线性系统 的扩展卡尔曼算法 Extended Kalman Filter（EKF）[2]。相比于线性卡尔曼滤波器，扩展卡尔曼滤波器有更广的适用范围[3,4]，更高的状态估计精度，可以处理任意更新频率的系统。 有了上文的基础，这里，我们将通过一个二维小车的例子并来介绍扩展卡尔曼滤波的原理。

2 扩展卡尔曼滤波算法介绍

在介绍扩展卡尔曼滤波算法之前，我们先回忆一下之前介绍的卡尔曼滤波算法： \begin{align*} x^{-}_k & = A\hat{x}_{k-1}\label{eq_kf_1}\\ P^{-}_k & = AP_{k-1}A^T +BQB^T\label{eq_kf_2}\\ K_k & = \frac{P^{-}_kH^T}{HP^{-}_kH^T+R}\label{eq_kf_3}\\ \hat{x}_k & = x^{-}_k +K_k\left(z_k - Hx^{-}_k\right)\label{eq_kf_4}\\ P_k & = \left(I- K_kH\right)P^{-}_k\label{eq_kf_5} \end{align*}

其中前两个公式为系统状态预测部分，后三个公式为系统的修正部分。其中整个系统最重要的是要通过公式将系统的模型和测量与状态之间的关系表示出来：

\begin{align*} x_{k+1} & = Ax_k +Bu_k + \omega_k \\ z_k & = Hx_k +\nu_k \end{align*}

式中 A 为系统矩阵， B 为控制矩阵， H 为观测矩阵。 x_k 为目标在 k 时刻的状态， u 为控制量， \omega 和 \nu 分别为系统误差和测量误差。在线性卡尔曼滤波算法中， A 和 H 是常值矩阵，其对应的系统就是线性系统。如果想将卡尔曼理论应用到非线性系统中，则 需要对非线性系统进行线性化处理 。

2.1 扩展卡尔曼滤波算法的不同

非线性系统方程及其观测方程通常可以表示为：

\begin{align*} x_k & = f\left(x_{k-1},u_k,\omega_k\right)\\ z_k & = h\left(x_k,\nu_k\right) \end{align*}

扩展卡尔曼滤波算法可以通过将非线性系统在其参考点处做泰勒级数展开，从而使非线性系统可以线性化。展开后，我们可以得到：

\begin{equation*} \begin{aligned} A_k & = \frac{\partial f}{\partial x}\left(\hat{x}_{k-1},u_k,0\right)\\ H_k & = \frac{\partial h}{\partial x}\left(x^{-}_{k},0\right)\\ W_k & = \frac{\partial f}{\partial \omega}\left(\hat{x}_{k-1},u_k,0\right)\\ V_k & = \frac{\partial h}{\partial \nu}\left(x^{-}_k,0\right)\\ \end{aligned} \end{equation*}

其中 A_k 和 W_k 是非线性系统函数 f 在 k 时刻求偏导数后的雅可比矩阵， H_k 和 V_k 为非线性测量函数 h 求偏导数后的雅克比矩阵。那么，扩展卡尔曼滤波算法可以写成：

\begin{align*} x^{-}_k & = f\left(\hat{x}_{k-1},u_k\right)\label{eq_ekf_1}\\ A_k & = \frac{\partial f}{\partial x}\left(\hat{x}_{k-1},u_k,0\right)\label{eq_ekf_2}\\ P^{-}_k & = A_kP_{k-1}A_k^T +W_kQ_{k-1}W_k^T\label{eq_ekf_3}\\ H_k & = \frac{\partial h}{\partial x}\left(x^{-}_{k},0\right)\label{eq_ekf_4}\\ K_k & = \frac{P^{-}_kH_k^T}{H_kP^{-}_kH_k^T+V_kRV_k^T}\label{eq_ekf_5}\\ \hat{x}_k & = x^{-}_k +K_k\left(z_k - h\left(x^{-}_k,0\right)\right)\label{eq_ekf_6}\\ P_k & = \left(I- K_kH_k\right)P^{-}_k\label{eq_ekf_7} \end{align*}

介绍完公式，接下来我们将通过2维平面内的小车来对扩展卡尔曼滤波算法进行讲解。

3 二维小车实例

如果要估计一个在二维平面内运动的小车，首先要根据要求选择一个较为合适的系统模型。在二维平面内，小车的运动可能是直线也可能是曲线，那么在一维平面用的线性匀速模型constant velocity model(CV) 将不适用。这里将采用一个简单的非线性模型匀转速、速度模型 constant turning rate and velocity magnitude model(CTRV)。

3.1非线性模型CTRV

ctrv模型是cv模型的一般形式。这里我们假设观测到的状态变量为： \begin{equation*} x = \left[p_x,p_y,v,\theta,\omega\right]^T \end{equation*}\\

分别对应目标在 x , y 方向上的位置，速度，角度和角速度，变量之间的关系如图1所示。

从图中可以看出，各变量之间的相互关系。 那么，从 k 时刻到 k+1 时刻，状态的变化应该是： \begin{equation*} x_{k+1}=x_{k}+\left[\begin{array}{c} \int_{t_{k_{k+1}}}^{t_{k+1}} v(t) \cdot \cos (\theta(t)) d t \\ \int_{t_{k}}^{t_{k+1}} v(t) \cdot \sin (\theta)) d t \\ 0 \\ \omega \Delta t \\ 0 \end{array}\right] \end{equation*}

根据积分，我们可以得到下式：

\begin{equation} x_{k+1}=x_{k}+\left[\begin{array}{c} \frac{v(t)}{\omega} \left(\sin(\theta_k +\omega_k \Delta t) - \sin(\theta_k)\right)\\ \frac{v(t)}{\omega} \left(-\cos(\theta_k +\omega_k \Delta t) + \cos(\theta_k)\right) \\ 0 \\ \omega_{k} \Delta t \\ 0 \end{array}\right] \end{equation}\\ 这里，我们对系统函数求偏导得到其雅克比矩阵为：

\begin{equation} A_k = \left[ \begin{array}{ccccc} 1 & 0 & \frac{1}{w}[\sin (w \Delta t+\theta)-\sin (\theta)] & \frac{v k}{w}[\cos (w \Delta t+\theta)-\cos (\theta)] & \frac{v_{k} \Delta t}{w} \cos (w \Delta t+\theta)-\frac{v_{k}}{w^{2}}[\sin (w \Delta t+\theta)-\sin (\theta)] \\ 0 & 1 & \frac{1}{w}[-\cos (w \Delta t+\theta)+\cos (\theta)] & \frac{v k}{w}[\sin (w \Delta t+\theta)-\sin (\theta)] & \frac{v k \Delta t}{w} \sin (w \Delta t+\theta)-\frac{v k}{w^{2}}[-\cos (w \Delta t+\theta)+\cos (\theta)] \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{array} \right] \end{equation}

这里为了将 系统误差为：

\begin{equation} Q=\left\{\begin{array}{cc} \frac{1}{2} \cos \left(\theta_k\right) \Delta t^{2} & 0 \\ \frac{1}{2} \sin \left(\theta_k\right) \Delta t^{2} & 0 \\ \Delta t & 0 \\ 0 & \frac{1}{2} \Delta t^{2} \\ 0 & \Delta t \end{array}\right\}\left\{\begin{array}{cc} \sigma_{a_{k}}{ }^{2} & 0 \\ 0 & \sigma_{\ddot{\theta}_{k}}{ }^{2} \end{array}\right\}\left\{\begin{array}{cc} \frac{1}{2} \cos \left(\theta_k\right) \Delta t^{2} & 0 \\ \frac{1}{2} \sin \left(\theta_k\right) \Delta t^{2} & 0 \\ \Delta t & 0 \\ 0 & 0 \end{array}\right\}^{T} \end{equation}\\ 后续再套入到卡尔曼滤波器中，我们就可以得到一个CTRV的跟踪模型：

\begin{equation*} \begin{aligned} x^{-}_{k+1} & = f\left(\hat{x}_k,0\right)\\ P^{-}_{k+1} & = A_k\hat{P}_kA_k^T +Q_k\\ K_{k+1} & = P^{-}_{k+1}H^T\left(H_kP^{-}_{k+1}H^T + R\right)^{-1}\\ \hat{x}_{k+1} & = x^{-}_{k+1} + K_{k+1}\left(z_{k+1} - h\left(x^{-}_{k+1},0\right)\right)\\ \hat{P}_{k+1} & = \left(I - K_{k+1}H_k\right)P^{-}_{k+1} \end{aligned} \end{equation*}

3.2 仿真

这里我们假设二维小车系统可以观测到目标的位置 \begin{equation*} z_k = \left[p_x,p_y\right]^T \end{equation*}\\

那么观测方程为：

\begin{equation*} H = \left[\begin{array}{ccccc} 1,0,0,0,0\\ 0,1,0,0,0 \end{array}\right] \end{equation*}

仿真总时间10s，时间间隔 \Delta t=0.1s ，目标小车绕原点，半径 r=10m 以速度 v=5m/s 做匀速圆周运动。观测信息为目标的位置，测量误差 \sigma_p=0.1 。实时跟踪如视频所示。

图 2: 跟踪匀速圆周运动仿视频 https://www.zhihu.com/video/1538903357938241536

从视频中我们可以看到，不同的模型即使在当前估计状态相差不大的情况下，其估计 目标未来的状态也是有很大差别的 。在这个例子中，目标做的是匀速绕圈运动，CTRV模型更能描述系统的运动状态，但该模型是非线性系统。因此，需要结合EKF，来实现对非线性系统的状态跟踪和预测。

仿真结果如图2所示，红色轨迹为小车真实轨迹，红点为测量小车位置，蓝色圆圈为CV模型估计的小车当前位置，绿色圆圈为CTRV模型估计的小车当前位置，蓝色实线为CTRV模型估计小车轨迹，蓝色虚线为CTRV模型预测未来一段时间内的小车轨迹，绿色实线为CV模型估计小车轨迹，绿色虚线为CV模型预测未来一段时间内的小车轨迹。 可以看出CTRV模型的预测比CV模型的预测在绕圈运动中更为准确 。

4 总结

在很多实际工程问题中，系统的描述总存在不同程度的非线性，如平方、三角函数等。针对这一问题，EKF的思路是将非线性系统线性化转换为近似的线性系统，然后应用卡尔曼滤波算法完成状态估计。

5 附

本文章对应的github仓库如下：

参考文献

[1] Rudolph Emil Kalman. A new approach to linear filtering and prediction problems. 1960.

[2] Leonard A McGee, Stanley F Schmidt, Leonard A Mcgee, and Stanley F Sc. Discovery of the kalman filter as a practical tool for aerospace and. In Industry,” National Aeronautics and Space Administration, Ames Research. Citeseer, 1985.

[3] Jakub Simanek, Michal Reinstein, and Vladimir Kubelka. Evaluation of the ekf-based estimation architectures for data fusion in mobile robots. IEEE/ASME Transactions on Mechatronics, 20(2):985–990, 2014.

[4] Yibing Wang and Markos Papageorgiou. Real-time freeway traffic state estimation based on extended kalman filter: a general approach. Transportation Research Part B: Methodological, 39(2):141–167, 2005.

本文共1704字，由西湖大学智能无人系统实验室博士生火山原创，
申请文章授权请联系后台相关运营人员。

▌公众号：空中机器人前沿

▌Bilibili：西湖大学空中机器人

▌Youtube：Aerial robotics @ Westlake University

▌实验室网站： https:// shiyuzhao.westlake.edu.cn