一、卡尔曼滤波的局限性
卡尔曼滤波（KF）是一个强大的状态估计算法，但它有一个关键限制：只能处理线性系统。然而现实世界中的物理系统几乎都是非线性的：
-
机器人运动（转弯时速度和方向的关系）
-
飞行器姿态（涉及三角函数）
-
传感器模型（如摄像头透视投影）
当系统存在非线性时，标准KF的线性假设会失效，导致估计结果不准确甚至发散。扩展卡尔曼滤波（EKF） 就是为了解决这个问题而诞生的。
二、EKF核心思想：泰勒展开（局部线性化的数学工具）
EKF的基本思路很直观：在系统当前状态附近，用线性函数近似非线性函数。就像在曲线上的某一点画一条切线来近似曲线。
数学工具：泰勒展开
EKF使用一阶泰勒展开进行线性化：
其中：
f(x)是原始非线性函数
a是线性化点（当前状态估计）
f'(a)是非线性函数在点a的导数（如果输入是多维的，就是雅可比矩阵(Jacobian Matrix) ）
三、EKF系统模型
EKF 处理的是一个带有噪声的非线性动态系统：
1. 状态方程（预测模型）
xᵏ：想要估计的系统在k时刻的状态向量 (例如:[位置X, 位置Y, 速度, 方向θ]ᵀ)。
f：非线性状态转移函数。它描述了系统状态如何从上一时刻k-1的状态xᵏ⁻¹，在控制输入uᵏ⁻¹和过程噪声wᵏ⁻¹的影响下，演化到当前时刻k的状态xᵏ。
uᵏ⁻¹：k-1时刻的控制输入向量 (例如: 油门、方向盘角度)。
wᵏ⁻¹：k-1时刻的过程噪声向量。它代表了模型的不确定性、未知的外部扰动等。我们假设它是均值为 0、协方差矩阵为Q的高斯白噪声 (wᵏ⁻¹ ~ N(0, Qᵏ⁻¹))。
2. 观测方程（测量模型）
zᵏ：k时刻的观测向量 (测量值) (例如: GPS 坐标、摄像头看到的特征点像素坐标、激光测距值)。
h：非线性的观测函数。它描述了系统状态xᵏ如何被转换成我们能测量到的观测值zᵏ。
vᵏ：k时刻的观测噪声向量。它代表了传感器的误差、测量干扰等。我们假设它是均值为 0、协方差矩阵为R的高斯白噪声 (vᵏ ~ N(0, Rᵏ))。
四、EKF公式推导（分步详解）
EKF 的核心操作就是在当前的最佳估计点，对非线性函数 f 和 h 进行一阶泰勒展开（线性化），然后用线性化后的近似模型来替代原始的非线性模型，套用标准 KF 的公式框架。KF 有预测步(Predict)和更新步(Update)，EKF 也有。
步骤 0：回顾 KF（重要！）
EKF 公式和 KF 公式形式上非常像！KF 的核心公式是：
预测 (Predict):
x̂ᵏ⁻ = Fᵏ⁻¹ * x̂ᵏ⁻¹ + Bᵏ⁻¹ * uᵏ⁻¹(预测状态)
Pᵏ⁻ = Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ + Qᵏ⁻¹(预测协方差)更新 (Update):
ỹᵏ = zᵏ - Hᵏ * x̂ᵏ⁻(新息/残差)
Sᵏ = Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Rᵏ(新息协方差)
Kᵏ = Pᵏ⁻ * (Hᵏ)ᵀ * (Sᵏ)⁻¹(卡尔曼增益)
x̂ᵏ = x̂ᵏ⁻ + Kᵏ * ỹᵏ(更新状态)
Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻(更新协方差)
步骤 1：线性化状态方程 f (用于预测步)
-
线性化点： 我们选择在上一时刻的最优估计值
x̂ᵏ⁻¹处进行线性化。为什么是这里？因为这是我们目前对k-1时刻状态最好的猜测，我们希望基于这个“最佳猜测”来预测k时刻的状态。 -
泰勒展开：
xᵏ = f(xᵏ⁻¹, uᵏ⁻¹, wᵏ⁻¹) ≈ f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹ -
深度解读：
-
f(x̂ᵏ⁻¹, uᵏ⁻¹, 0): 这是非线性函数f在点(x̂ᵏ⁻¹, uᵏ⁻¹, 0)的值。注意我们把过程噪声wᵏ⁻¹暂时设为了它的均值 0。这是一个常数项，代表了基于线性化点的“基础预测”。 -
Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹):-
(xᵏ⁻¹ - x̂ᵏ⁻¹)表示真实状态xᵏ⁻¹偏离我们估计值x̂ᵏ⁻¹的误差。 -
Fᵏ⁻¹是函数f关于状态x的雅可比矩阵 (Jacobian Matrix)，在点(x̂ᵏ⁻¹, uᵏ⁻¹, 0)处计算。Fᵏ⁻¹就是f在当前估计点附近的状态变化率（斜率矩阵）！ 它告诉我们，状态x的一个微小变化，会导致预测状态xᵏ发生多大的变化。 -
这个项线性地修正了由于状态估计误差带来的预测偏差。
-
-
Wᵏ⁻¹ * wᵏ⁻¹:-
wᵏ⁻¹是过程噪声。 -
Wᵏ⁻¹是函数f关于过程噪声w的雅可比矩阵，也在点(x̂ᵏ⁻¹, uᵏ⁻¹, 0)处计算。它描述了过程噪声w是如何线性地影响预测状态xᵏ的。很多时候，如果噪声是加性的 (f(x, u, w) = f_nl(x, u) + w)，那么Wᵏ⁻¹就是单位矩阵I。
-
-
-
得到线性化状态方程：
经过线性化，我们把非线性的状态方程近似成了一个线性方程：
xᵏ ≈ f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹
步骤 2：线性化观测方程 h (用于更新步)
-
线性化点： 我们选择在当前时刻的预测状态值
x̂ᵏ⁻处进行线性化。为什么是这里？因为这是我们利用状态方程预测出来的k时刻状态的最佳估计，我们需要基于这个预测值来“预测”我们会观测到什么，然后与实际观测值比较。 -
泰勒展开：
zᵏ = h(xᵏ, vᵏ) ≈ h(x̂ᵏ⁻, 0) + Hᵏ * (xᵏ - x̂ᵏ⁻) + Vᵏ * vᵏ -
深度解读：
-
h(x̂ᵏ⁻, 0): 这是非线性函数h在点(x̂ᵏ⁻, 0)的值。注意我们把观测噪声vᵏ暂时设为了它的均值 0。这是一个常数项，代表了基于预测状态x̂ᵏ⁻的“预测观测值”。 -
Hᵏ * (xᵏ - x̂ᵏ⁻):-
(xᵏ - x̂ᵏ⁻)表示真实状态xᵏ偏离我们预测值x̂ᵏ⁻的误差。 -
Hᵏ是函数h关于状态x的雅可比矩阵 (Jacobian Matrix)，在点(x̂ᵏ⁻, 0)处计算。Hᵏ就是h在当前预测状态点附近的“观测灵敏度”矩阵！ 它告诉我们，状态x的一个微小变化，会导致预测观测值zᵏ发生多大的变化。 -
这个项线性地修正了由于状态预测误差带来的观测预测偏差。
-
-
Vᵏ * vᵏ:-
vᵏ是观测噪声。 -
Vᵏ是函数h关于观测噪声v的雅可比矩阵，也在点(x̂ᵏ⁻, 0)处计算。它描述了观测噪声vᵏ是如何线性地影响实际观测值zᵏ的。同样，如果噪声是加性的 (h(x, v) = h_nl(x) + v)，那么Vᵏ就是单位矩阵I。
-
-
-
得到线性化观测方程：
经过线性化，我们把非线性的观测方程近似成了一个线性方程：
zᵏ ≈ h(x̂ᵏ⁻, 0) + Hᵏ * (xᵏ - x̂ᵏ⁻) + Vᵏ * vᵏ
步骤 3：预测步 (Predict) - 基于线性化模型进行预测
现在我们有了线性化的状态方程，可以像 KF 的预测步一样操作了。关键点：在预测状态时，我们不知道 xᵏ⁻¹ 的真实值，也不知道噪声 wᵏ⁻¹ 的具体值，我们只有它们的统计特性（均值和协方差）。
-
预测状态
x̂ᵏ⁻：-
回顾线性化方程：
xᵏ ≈ f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹ -
我们取这个方程的期望值 (Expected Value) 作为预测状态：
x̂ᵏ⁻ = E[xᵏ] ≈ E[f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹] -
拆解：
-
f(x̂ᵏ⁻¹, uᵏ⁻¹, 0)是常数，期望是自己：f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) -
E[xᵏ⁻¹ - x̂ᵏ⁻¹]：x̂ᵏ⁻¹是k-1时刻的最优估计（后验估计），我们假设它的期望误差为 0 (E[xᵏ⁻¹ - x̂ᵏ⁻¹] = 0)。 -
E[wᵏ⁻¹] = 0(噪声均值为 0)。
-
-
因此：
x̂ᵏ⁻ = f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) -
解读： 预测状态就是直接把非线性状态转移函数
f作用在上一时刻的最优估计x̂ᵏ⁻¹和当前控制输入uᵏ⁻¹上，并且把过程噪声设为其均值 0。这是最直接的预测方式。
-
-
预测协方差
Pᵏ⁻：-
协方差衡量状态估计的不确定性。我们需要计算预测状态
x̂ᵏ⁻的协方差Pᵏ⁻ = Cov(x̂ᵏ⁻) ≈ Cov(xᵏ)。 -
回顾线性化方程：
xᵏ ≈ f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹ -
定义预测误差：
eᵏ⁻ = xᵏ - x̂ᵏ⁻ -
利用线性化方程和
x̂ᵏ⁻的表达式：
eᵏ⁻ ≈ [f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) + Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹] - f(x̂ᵏ⁻¹, uᵏ⁻¹, 0)
eᵏ⁻ ≈ Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹ -
现在计算协方差
Pᵏ⁻ = Cov(eᵏ⁻) ≈ Cov(Fᵏ⁻¹ * (xᵏ⁻¹ - x̂ᵏ⁻¹) + Wᵏ⁻¹ * wᵏ⁻¹) -
假设
xᵏ⁻¹ - x̂ᵏ⁻¹(上一时刻的后验估计误差) 和wᵏ⁻¹(过程噪声) 是相互独立的。 -
协方差传播公式：
Cov(Ax + By) = A Cov(x) Aᵀ + B Cov(y) Bᵀ(当x和y独立时)。（基于线性化模型和协方差传播公式：Cov(Ax) = A Cov(x) Aᵀ） -
因此：
Pᵏ⁻ ≈ Fᵏ⁻¹ * Cov(xᵏ⁻¹ - x̂ᵏ⁻¹) * (Fᵏ⁻¹)ᵀ + Wᵏ⁻¹ * Cov(wᵏ⁻¹) * (Wᵏ⁻¹)ᵀ
Pᵏ⁻ ≈ Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ + Wᵏ⁻¹ * Qᵏ⁻¹ * (Wᵏ⁻¹)ᵀ -
解读：
-
Pᵏ⁻¹：上一时刻状态估计 (x̂ᵏ⁻¹) 的协方差（不确定性）。 -
Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ：这项表示状态估计的不确定性Pᵏ⁻¹通过状态转移模型Fᵏ⁻¹传播到了预测状态。如果模型放大误差 (F的模很大)，预测不确定性就变大；如果模型稳定 (F的模小)，不确定性传播就小。 -
Wᵏ⁻¹ * Qᵏ⁻¹ * (Wᵏ⁻¹)ᵀ：这项表示过程噪声的不确定性Qᵏ⁻¹通过噪声模型Wᵏ⁻¹传播到了预测状态。它代表了模型本身的不精确性带来的额外不确定性。 -
预测协方差
Pᵏ⁻是这两项的和，意味着预测状态的不确定性总是比上一时刻更大（或至少不变），因为运动过程增加了未知因素（噪声）。
-
-
步骤 4：更新步 (Update) - 利用观测修正预测
现在我们有了预测状态 x̂ᵏ⁻ 和预测协方差 Pᵏ⁻，并且得到了线性化的观测方程。当实际观测值 zᵏ 到来时，我们就可以像 KF 的更新步一样操作了。关键点：我们不知道真实状态 xᵏ，也不知道观测噪声 vᵏ 的具体值，只有观测值 zᵏ 和噪声的统计特性。
-
计算新息 (Innovation) / 残差
ỹᵏ：-
新息表示实际观测值
zᵏ与基于预测状态的预测观测值h(x̂ᵏ⁻, 0)之间的差异。 -
ỹᵏ = zᵏ - h(x̂ᵏ⁻, 0) -
解读： 如果预测状态
x̂ᵏ⁻是完美的，并且没有观测噪声，那么ỹᵏ应该为 0。ỹᵏ不为 0 就包含了两个信息：(1) 预测状态可能不准；(2) 观测有噪声。EKF 的工作就是尽可能合理地利用ỹᵏ来修正x̂ᵏ⁻。
-
-
计算新息协方差
Sᵏ：-
新息
ỹᵏ本身也有不确定性（因为它来源于状态预测误差和观测噪声）。新息协方差Sᵏ衡量了这个差异的不确定性。 -
回顾线性化观测方程：
zᵏ ≈ h(x̂ᵏ⁻, 0) + Hᵏ * (xᵏ - x̂ᵏ⁻) + Vᵏ * vᵏ -
代入新息定义：
ỹᵏ = zᵏ - h(x̂ᵏ⁻, 0) ≈ [h(x̂ᵏ⁻, 0) + Hᵏ * (xᵏ - x̂ᵏ⁻) + Vᵏ * vᵏ] - h(x̂ᵏ⁻, 0)
ỹᵏ ≈ Hᵏ * (xᵏ - x̂ᵏ⁻) + Vᵏ * vᵏ -
定义状态预测误差：
eᵏ⁻ = xᵏ - x̂ᵏ⁻(其协方差正是Pᵏ⁻)。 -
计算新息协方差
Sᵏ = Cov(ỹᵏ) ≈ Cov(Hᵏ * eᵏ⁻ + Vᵏ * vᵏ) -
假设
eᵏ⁻(状态预测误差) 和vᵏ(观测噪声) 是相互独立的。 -
协方差传播公式：
Cov(Ax + By) = A Cov(x) Aᵀ + B Cov(y) Bᵀ(当x和y独立时)。 -
因此：
Sᵏ ≈ Hᵏ * Cov(eᵏ⁻) * (Hᵏ)ᵀ + Vᵏ * Cov(vᵏ) * (Vᵏ)ᵀ
Sᵏ ≈ Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Vᵏ * Rᵏ * (Vᵏ)ᵀ -
解读：
-
Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ：这项表示状态预测的不确定性Pᵏ⁻通过观测模型Hᵏ映射到了观测空间。它代表了预测状态不准导致我们“预测会看到什么”也不准的程度。 -
Vᵏ * Rᵏ * (Vᵏ)ᵀ：这项表示观测噪声的不确定性Rᵏ通过噪声模型Vᵏ传播到了新息中。它代表了传感器本身的不精确性带来的差异。 -
Sᵏ是这两项的和，是ỹᵏ的总协方差。Sᵏ越大，说明观测值zᵏ的可信度越低。
-
-
-
计算卡尔曼增益
Kᵏ：-
卡尔曼增益
Kᵏ是一个权重矩阵，它决定了我们应该多大程度地相信观测值zᵏ来修正预测值x̂ᵏ⁻。 -
公式：
Kᵏ = Pᵏ⁻ * (Hᵏ)ᵀ * (Sᵏ)⁻¹ -
深度解读：
-
(Hᵏ)ᵀ：将观测空间的信息转置回状态空间。 -
(Sᵏ)⁻¹：新息协方差的逆。Sᵏ越小（观测越可靠），(Sᵏ)⁻¹越大，意味着增益Kᵏ会越大，我们会更信任这次观测。 -
Pᵏ⁻ * ...：状态预测协方差Pᵏ⁻越大（预测越不可靠），增益Kᵏ也会倾向于更大，我们会更依赖观测来修正不靠谱的预测。 -
本质上，
Kᵏ是状态预测误差协方差Pᵏ⁻在观测空间投影(Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ)与总新息协方差Sᵏ的比值。它是最小化后验估计协方差Pᵏ的最优权重。
-
-
-
更新状态估计
x̂ᵏ：-
这是 EKF 的核心修正步骤。我们利用卡尔曼增益
Kᵏ将新息ỹᵏ的信息“融合”到预测状态x̂ᵏ⁻中：
x̂ᵏ = x̂ᵏ⁻ + Kᵏ * ỹᵏ -
解读：
-
如果新息
ỹᵏ是 0（观测值完全符合预测），那么更新后的状态x̂ᵏ就等于预测状态x̂ᵏ⁻。 -
如果新息
ỹᵏ不为 0，Kᵏ决定了应该沿着状态空间的哪个方向（由Hᵏ决定，因为ỹᵏ在观测空间）调整x̂ᵏ⁻，以及调整多少。 -
这个公式直接使用了线性化观测方程得到的近似关系
ỹᵏ ≈ Hᵏ * (xᵏ - x̂ᵏ⁻)。Kᵏ * ỹᵏ ≈ Kᵏ * Hᵏ * (xᵏ - x̂ᵏ⁻)就是对状态预测误差(xᵏ - x̂ᵏ⁻)的一个线性估计修正项。
-
-
-
更新协方差估计
Pᵏ：-
在融合了观测信息后，我们对状态的估计应该更准确了，不确定性应该减小了。更新后的协方差
Pᵏ反映了这个降低的不确定性。 -
公式：
Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻ -
深度解读 (非常重要！)：
-
矩阵
(I - Kᵏ * Hᵏ)被称为更新因子。 -
这个公式来源于更新后状态误差的协方差推导（基于线性化模型和最优估计理论）。它表示：融合观测信息后，状态估计的不确定性等于预测不确定性
Pᵏ⁻减去一个修正项Kᵏ * Hᵏ * Pᵏ⁻。 -
Kᵏ * Hᵏ本质上代表了观测带来的信息量。 观测越可靠 (Sᵏ小 ->Kᵏ大)，或者观测模型Hᵏ越敏感，这个修正项就越大，意味着融合后不确定性Pᵏ减少得越多。 -
一个等价的、数值计算有时更稳定的形式是：
Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻ * (I - Kᵏ * Hᵏ)ᵀ + Kᵏ * Rᵏ * (Kᵏ)ᵀ。这个形式明确包含了观测噪声Rᵏ的影响，并保证了Pᵏ的对称正定性。
-
-
五、EKF算法流程
-
初始化：
-
设定初始状态估计
x̂⁰(你的最佳猜测)。 -
设定初始状态估计协方差矩阵
P⁰(表示你对初始猜测的不确定性有多大，通常设为一个较大的对角矩阵)。
-
-
循环（对于每个时间步k）：
a. 预测：-
计算状态转移雅可比矩阵
Fᵏ⁻¹和Wᵏ⁻¹ -
预测状态：
x̂ᵏ⁻ = f(x̂ᵏ⁻¹, uᵏ⁻¹, 0) -
预测协方差：
Pᵏ⁻ = Fᵏ⁻¹Pᵏ⁻¹(Fᵏ⁻¹)ᵀ + Wᵏ⁻¹Q(Wᵏ⁻¹)ᵀ
b. 更新：
-
计算雅可比矩阵
Hᵏ和Vᵏ -
计算新息：
ỹᵏ = zᵏ - h(x̂ᵏ⁻, 0) -
计算新息协方差：
Sᵏ = HᵏPᵏ⁻(Hᵏ)ᵀ + VᵏR(Vᵏ)ᵀ -
计算卡尔曼增益：
Kᵏ = Pᵏ⁻(Hᵏ)ᵀ(Sᵏ)⁻¹ -
更新状态：
x̂ᵏ = x̂ᵏ⁻ + Kᵏỹᵏ -
更新协方差：
Pᵏ = (I - KᵏHᵏ)Pᵏ⁻
-
六、EKF关键概念详解
1. 雅可比矩阵（Jacobian Matrix）
雅可比矩阵是多变量函数的导数矩阵。对于一个函数 f: Rⁿ → Rᵐ：
[ ∂f₁/∂x₁ ∂f₁/∂x₂ ... ∂f₁/∂xₙ ]
J_f(x) = [ ∂f₂/∂x₁ ∂f₂/∂x₂ ... ∂f₂/∂xₙ ]
[ ... ... ... ... ]
[ ∂fₘ/∂x₁ ∂fₘ/∂x₂ ... ∂fₘ/∂xₙ ]
在EKF中：
-
F是状态转移函数f关于状态x的雅可比 -
H是观测函数h关于状态x的雅可比
2. 线性化误差
EKF的主要误差来源是线性化近似。当：
-
系统高度非线性
-
时间步长过大
-
初始估计误差大
时，线性化误差会导致性能下降甚至发散。
3. 与KF的对比
| 特性 | 卡尔曼滤波 (KF) | 扩展卡尔曼滤波 (EKF) |
|---|---|---|
| 系统类型 | 线性 | 非线性 |
| 核心方法 | 精确解析解 | 一阶泰勒近似 |
| 计算复杂度 | 低 | 中（需计算雅可比） |
| 精度 | 最优（线性） | 次优（有近似误差） |
| 所需矩阵 | F, H | 雅可比F, H |
| 适用性 | 线性系统 | 弱非线性系统 |
七、代码示例
扩展卡尔曼滤波 (EKF) 在机器人位置估计中的 Python 实现
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
import matplotlib.transforms as transforms
class ExtendedKalmanFilter:
def __init__(self, initial_state, initial_covariance, process_noise, measurement_noise):
"""
扩展卡尔曼滤波器初始化
参数:
initial_state: 初始状态估计 [x, y, theta, v]ᵀ
initial_covariance: 初始协方差矩阵
process_noise: 过程噪声协方差矩阵 Q
measurement_noise: 测量噪声协方差矩阵 R
"""
self.state = initial_state.astype(float) # 状态向量 [x, y, theta, v]ᵀ
self.covariance = initial_covariance.astype(float) # 状态协方差矩阵
# 噪声协方差矩阵
self.Q = process_noise # 过程噪声
self.R = measurement_noise # 测量噪声
# 时间步长
self.dt = 0.1 # 时间步长 (秒)
def predict(self, u):
"""
预测步 - 根据运动模型预测下一时刻的状态
参数:
u: 控制输入 [加速度, 角速度]ᵀ
"""
# 从状态向量中提取变量
x, y, theta, v = self.state
a, omega = u # 加速度和角速度
# 1. 使用非线性运动模型预测状态
# 状态转移函数 f(x, u)
new_x = x + v * np.cos(theta) * self.dt + 0.5 * a * np.cos(theta) * self.dt ** 2
new_y = y + v * np.sin(theta) * self.dt + 0.5 * a * np.sin(theta) * self.dt ** 2
new_theta = theta + omega * self.dt
new_v = v + a * self.dt
# 更新预测状态
self.state = np.array([new_x, new_y, new_theta, new_v])
# 2. 计算状态转移函数的雅可比矩阵 F
F = np.array([
[1, 0, -v * np.sin(theta) * self.dt - 0.5 * a * np.sin(theta) * self.dt ** 2, np.cos(theta) * self.dt],
[0, 1, v * np.cos(theta) * self.dt + 0.5 * a * np.cos(theta) * self.dt ** 2, np.sin(theta) * self.dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 3. 更新协方差矩阵: P = F * P * Fᵀ + Q
self.covariance = F @ self.covariance @ F.T + self.Q
def update(self, z):
"""
更新步 - 根据传感器测量更新状态估计
参数:
z: 测量值 [x_gps, y_gps]ᵀ
"""
# 1. 计算观测函数的雅可比矩阵 H
# 观测模型 h(x) = [x, y]ᵀ，所以雅可比矩阵很简单
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
# 2. 计算预测的测量值
# 观测函数 h(x)
z_pred = np.array([self.state[0], self.state[1]])
# 3. 计算新息 (测量残差)
innovation = z - z_pred
# 4. 计算新息协方差 S = H * P * Hᵀ + R
S = H @ self.covariance @ H.T + self.R
# 5. 计算卡尔曼增益 K = P * Hᵀ * S⁻¹
K = self.covariance @ H.T @ np.linalg.inv(S)
# 6. 更新状态估计
self.state = self.state + K @ innovation
# 7. 更新协方差矩阵
I = np.eye(self.covariance.shape[0])
self.covariance = (I - K @ H) @ self.covariance
def get_position_estimate(self):
"""返回当前位置估计 (x, y)"""
return self.state[0], self.state[1]
def get_position_covariance(self):
"""返回位置估计的协方差 (2x2 矩阵)"""
return self.covariance[:2, :2]
def simulate_robot_motion(true_state, u, process_noise_std):
"""
模拟机器人的真实运动（带过程噪声）
参数:
true_state: 当前真实状态 [x, y, theta, v]ᵀ
u: 控制输入 [加速度, 角速度]ᵀ
process_noise_std: 过程噪声标准差 [x, y, theta, v]
返回:
new_true_state: 新的真实状态
"""
x, y, theta, v = true_state
a, omega = u
# 添加过程噪声
noise = np.random.normal(0, process_noise_std, 4)
# 更新状态（欧拉积分）
new_x = x + v * np.cos(theta) * dt + 0.5 * a * np.cos(theta) * dt ** 2 + noise[0]
new_y = y + v * np.sin(theta) * dt + 0.5 * a * np.sin(theta) * dt ** 2 + noise[1]
new_theta = theta + omega * dt + noise[2]
new_v = v + a * dt + noise[3]
return np.array([new_x, new_y, new_theta, new_v])
def get_gps_measurement(true_position, measurement_noise_std):
"""
模拟GPS测量（带测量噪声）
参数:
true_position: 真实位置 (x, y)
measurement_noise_std: 测量噪声标准差 [x, y]
返回:
带有噪声的测量值 [x_gps, y_gps]ᵀ
"""
noise = np.random.normal(0, measurement_noise_std, 2)
return np.array([true_position[0] + noise[0], true_position[1] + noise[1]])
def plot_robot(ax, x, y, theta, color, label, uncertainty=None):
"""绘制机器人位置和方向"""
# 绘制位置点
ax.plot(x, y, 'o', color=color, label=label)
# 绘制方向箭头
arrow_length = 0.5
dx = arrow_length * np.cos(theta)
dy = arrow_length * np.sin(theta)
ax.arrow(x, y, dx, dy, head_width=0.2, head_length=0.2, fc=color, ec=color)
# 绘制不确定性椭圆（如果提供了协方差）
if uncertainty is not None:
# 计算椭圆的角度和半轴长度
eigvals, eigvecs = np.linalg.eig(uncertainty)
angle = np.degrees(np.arctan2(eigvecs[1, 0], eigvecs[0, 0]))
width, height = 2 * np.sqrt(eigvals)
# 绘制椭圆
ellipse = Ellipse((x, y), width=width, height=height, angle=angle,
edgecolor=color, fc='none', lw=1, linestyle='--')
ax.add_patch(ellipse)
def plot_uncertainty(ax, ekf_states, true_states, measurements, dt):
errors = []
std_devs = []
# 1. 计算误差和标准差
for i in range(len(ekf_states)):
state, cov = ekf_states[i]
true_state = true_states[i]
# 计算位置误差（x、y方向的距离）
error = np.linalg.norm(state[:2] - true_state[:2])
errors.append(error)
# 计算位置不确定性（协方差对角线之和的开方）
std_dev = np.sqrt(cov[0, 0] + cov[1, 1])
std_devs.append(std_dev)
# 2. 绘制误差和标准差曲线
time_steps = np.arange(len(errors)) * dt # 时间轴：每个时间步对应一个时刻
ax.plot(time_steps, errors, 'b-', label='估计误差（米）')
ax.plot(time_steps, std_devs, 'r--', label='估计标准差（米）')
# 3. 修正测量点时间坐标
# 测量是“每3步执行一次”，因此第i次测量的实际时间是：i * 3 * dt
for i, z in enumerate(measurements):
measurement_time = i * 3 * dt # 修正：每次测量间隔3个时间步
# 在x轴对应时间、y=0处标记测量点（用绿色x）
ax.plot(measurement_time, 0, 'gx', markersize=8, label='GPS测量点' if i == 0 else "")
# 4. 设置图形属性（确保坐标轴范围合理，避免数据被截断）
ax.set_xlabel('时间（秒）')
ax.set_ylabel('位置误差/不确定性（米）')
ax.set_title('EKF位置估计误差与不确定性演变')
ax.legend()
ax.grid(True)
# 自动调整y轴范围，确保所有数据可见
ax.set_ylim(bottom=0, top=max(max(errors) * 1.2, max(std_devs) * 1.2))
# 主程序
if __name__ == "__main__":
# 设置随机种子以确保结果可重现
np.random.seed(42)
# 仿真参数
dt = 0.1 # 时间步长 (秒)
simulation_time = 20.0 # 总仿真时间 (秒)
steps = int(simulation_time / dt) # 总步数
# 初始状态 [x, y, theta, v]ᵀ
initial_state = np.array([0, 0, np.pi / 4, 1.0]) # 初始位置(0,0)，方向45度，速度1m/s
# 过程噪声标准差 [x, y, theta, v]
process_noise_std = np.array([0.01, 0.01, 0.005, 0.02])
# 测量噪声标准差 [x, y]
measurement_noise_std = np.array([0.3, 0.3])
# EKF初始化
# 初始状态估计 - 我们故意设置一个错误的初始估计
initial_estimate = np.array([-1.0, 1.0, np.pi / 2, 0.8])
# 初始协方差矩阵 - 表示初始估计的不确定性
initial_covariance = np.diag([1.0, 1.0, 0.5, 0.2]) ** 2
# 过程噪声协方差矩阵
Q = np.diag(process_noise_std) ** 2
# 测量噪声协方差矩阵
R = np.diag(measurement_noise_std) ** 2
# 创建EKF实例
ekf = ExtendedKalmanFilter(initial_estimate, initial_covariance, Q, R)
# 存储状态历史用于绘图
true_states = [initial_state]
ekf_states = [(ekf.state.copy(), ekf.get_position_covariance().copy())]
measurements = []
# 控制输入序列 - 模拟机器人的运动
# 前1/3时间：直线加速
# 中间1/3时间：匀速转弯
# 最后1/3时间：减速直线
control_sequence = []
for i in range(steps):
if i < steps // 3:
control_sequence.append([0.5, 0.0]) # 加速，直线
elif i < 2 * steps // 3:
control_sequence.append([0.0, 0.3]) # 匀速，转弯
else:
control_sequence.append([-0.4, 0.0]) # 减速，直线
# 主仿真循环
true_state = initial_state.copy()
for i in range(steps):
# 获取当前控制输入
u = control_sequence[i]
# 1. 模拟机器人真实运动（带过程噪声）
true_state = simulate_robot_motion(true_state, u, process_noise_std)
true_states.append(true_state)
# 2. EKF预测步
ekf.predict(u)
# 3. 模拟GPS测量（每3步测量一次）
if i % 3 == 0:
z = get_gps_measurement(true_state[:2], measurement_noise_std)
measurements.append(z)
# EKF更新步
ekf.update(z)
# 存储EKF当前状态和位置协方差
ekf_states.append((ekf.state.copy(), ekf.get_position_covariance().copy()))
# 转换为NumPy数组以便索引
true_states = np.array(true_states)
measurements = np.array(measurements)
# 设置Matplotlib支持中文显示
plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"] # 支持中文的字体
plt.rcParams["axes.unicode_minus"] = False # 解决负号显示异常问题
# 绘制轨迹
plt.figure(figsize=(12, 10))
# 提取估计的位置用于绘图
est_x = [state[0] for state, _ in ekf_states] # 从状态元组中提取x
est_y = [state[1] for state, _ in ekf_states] # 从状态元组中提取y
# 绘制真实轨迹
plt.plot(true_states[:, 0], true_states[:, 1], 'g-', linewidth=2, label='真实轨迹')
# 绘制EKF估计轨迹
plt.plot(est_x, est_y, 'b-', linewidth=1.5, label='EKF估计轨迹') # 使用提取的est_x和est_y
# 绘制测量点
plt.plot(measurements[:, 0], measurements[:, 1], 'rx', markersize=6, label='GPS测量')
# 绘制起点和终点
plt.plot(true_states[0, 0], true_states[0, 1], 'go', markersize=10, label='起点 (真实)')
plt.plot(est_x[0], est_y[0], 'bo', markersize=8, label='起点 (估计)')
plt.plot(true_states[-1, 0], true_states[-1, 1], 'g*', markersize=12, label='终点 (真实)')
plt.plot(est_x[-1], est_y[-1], 'b*', markersize=10, label='终点 (估计)')
# 在关键点绘制机器人和方向
for i in [0, len(true_states) // 3, 2 * len(true_states) // 3, len(true_states) - 1]:
# 获取该时刻的状态和协方差
state, cov = ekf_states[i]
# 绘制EKF估计机器人
plot_robot(plt.gca(), state[0], state[1], state[2], 'b', '', cov)
# 设置图形属性
plt.title('扩展卡尔曼滤波 (EKF) - 机器人位置估计')
plt.xlabel('X 位置 (米)')
plt.ylabel('Y 位置 (米)')
plt.legend(loc='upper left')
plt.grid(True)
plt.axis('equal')
# 绘制不确定性演变
fig, ax = plt.subplots(figsize=(12, 6))
plot_uncertainty(ax, ekf_states, true_states, measurements, dt)
# 显示图形
plt.tight_layout()
plt.show()
扫一扫
1872
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
