扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）

最新推荐文章于 2026-06-29 09:41:17 发布

原创

最新推荐文章于 2026-06-29 09:41:17 发布 · 10w+ 阅读

275

1.6k

本文深入解析扩展卡尔曼滤波(EKF)理论，对比KF，介绍非线性模型如CTRV的应用，提供Matlab、Python和C++代码示例，适合初学者快速掌握。

该文章已生成可运行项目，

扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）

文章目录

扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）

我们上篇提到的卡尔曼滤波（参见我的另一篇文章：卡尔曼滤波理论讲解与应用（matlab和python））是用于线性系统，预测（运动）模型和观测模型是在假设高斯和线性情况下进行的。简单的卡尔曼滤波必须应用在符合高斯分布的系统中，但是现实中并不是所有的系统都符合这样。另外高斯分布在非线性系统中的传递结果将不再是高斯分布。那如何解决这个问题呢？扩展卡尔曼滤波就是干这个事的。

理论讲解

扩展卡尔曼滤波（Extended Kalman Filter，EKF）通过局部线性来解决非线性的问题。将非线性的预测方程和观测方程进行求导，以切线代替的方式来线性化。其实就是在均值处进行一阶泰勒展开。

数学中，泰勒公式是一个用函数在某点的信息描述其附近取值的公式（一句话描述：就是用多项式函数去逼近光滑函数）。如果函数足够平滑的话，在已知函数在某一点的各阶导数值的情况之下，泰勒公式可以用这些导数值做系数构建一个多项式来近似函数在这一点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。
$f(x)_{Taylor} = \sum_{n=0}^{\infty} \frac{f^{(n)}(a)}{n!} \times (x - a)^n\\ =f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f^{(2)}(a)}{2!}(x-a)^2+ \cdots + \frac{f^{(n)}(a)}{n!}(x-a)^n + R_n(x)$
$f^{(n)}(a)$ 表示 $f (x)$ 在第 $n$ 阶导数的表达式，带入一个值 $a$ 计算后得到的结果（注意，它是个值）
$\frac{1}{n!}$ 是一个系数（一个值），每一项都不同，第一项 $\frac{1}{1}$ ，第二项 $\frac{1}{2}$ …… 依此类推
$x−a)^n$ 是一个以 $x$ 为自变量的表达式 。 $R n (x)$ 是泰勒公式的余项，是 $x−a)^n$ 的高阶无穷小

KF和EKF模型对比

首先，让卡尔曼先和扩展卡尔曼滤波做一个对比。在对比过程中可以看出，扩展卡尔曼是一个简单的非线性近似滤波算法，指运动或观测方程不是线性的情况,在预测模型部分，扩展卡尔曼的预测模型和量测模型已经是非线性了。为了简化计算，EKF通过一阶泰勒分解线性化运动、观测方程。KF与EKF具有相同的算法结构，都是以高斯形式描述后验概率密度的，通过计算贝叶斯递推公式得到的。最大的不同之处在于，计算方差时，EKF的状态转移矩阵（上一时刻的状态信息 $k - 1 ∣ k - 1$ ）和观测矩阵（一步预测 $k ∣ k - 1$ ）都是状态信息的雅克比矩阵（偏导数组成的矩阵）。在预测公式部分，扩展卡尔曼滤波的 $F_k$ 为 $f$ 的雅可比矩阵，在更新公式部分，扩展卡尔曼滤波的 $H_k$ 为 $h$ 的雅可比矩阵。

预测模型：
$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ x_k=Fx_{k-1}+Bu_k+w_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad x_k=f(x_{k-1},u_k)+w_k\\ \vec{z_k}=Hx_k+v_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \vec{z_k}=h(x_k)+v_k$
预测公式：

$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ \hat x_k=F_k \hat x_{k-1} + B_k \vec u_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x_k=f(x_{k-1},u_k)\\ P_k=F_kP_{k-1}F^T_k+Q_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P_k=F_kP_{k-1}F^T_k+Q_k$
更新公式：

$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter \\ K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \\ \hat x'_k = \hat x_k+K'(\vec {z_k}-H_k\hat x_k) \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x'_k = \hat x_k+K'(\vec {z_k}-h(\hat x_k)) \\ P'_k=P_k-K'H_kP_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P'_k=P_k-K'H_kP_k$