扩展卡尔曼滤波（EKF）：处理非线性系统的状态估计 |
您所在的位置：网站首页 › 什么叫非线性系统线性化 › 扩展卡尔曼滤波（EKF）：处理非线性系统的状态估计 |
|
扩展卡尔曼滤波（Extended Kalman Filter, EKF）是一种高效处理非线性系统的状态估计算法。它通过线性化非线性模型来近似系统动态和观测模型，从而允许使用卡尔曼滤波的框架。这里，我们详细地探讨EKF的基本原理、关键方程、以及如何使用EKF进行状态估计。 视频推荐：【卡尔曼滤波器】6_扩展卡尔曼滤波器_Extended Kalman Filter 基本原理EKF处理的核心是两个非线性方程：非线性状态转移方程和非线性观测方程。EKF通过在每一步对这些非线性函数进行泰勒展开（通常保留至一阶项），将非线性问题近似为线性问题，使得基本的卡尔曼滤波方法可以应用。 非线性状态方程（系统模型）：x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1}, u_k) + w_k xk=f(xk−1,uk)+wk x k x_k xk: 系统在时刻 k k k的状态向量。 f ( ⋅ ) f(\cdot) f(⋅): 非线性状态转移函数。 u k u_k uk: 时刻 k k k的控制输入向量。 w k w_k wk: 过程噪声，通常假设为高斯分布。 非线性观测方程（测量模型）：z k = h ( x k ) + v k z_k = h(x_k) + v_k zk=h(xk)+vk z k z_k zk: 在时刻 k k k的测量向量。 h ( ⋅ ) h(\cdot) h(⋅): 非线性测量函数。 v k v_k vk: 测量噪声，通常假设为高斯分布。 线性化过程在非线性系统中，状态转移和观测模型不能直接用线性方程准确描述。基本卡尔曼滤波假设这些模型是线性的，这在处理非线性系统时会导致估计精度下降。为了在卡尔曼滤波框架内处理非线性模型，EKF采用线性化方法来近似这些非线性模型。 在每个时间步骤中，EKF通过对 f ( ⋅ ) f(\cdot) f(⋅)和 h ( ⋅ ) h(\cdot) h(⋅)在当前估计点附近进行一阶泰勒展开来近似这些函数，获得线性化的模型。 线性化是通过泰勒展开非线性函数 f ( ⋅ ) f(\cdot) f(⋅)和 h ( ⋅ ) h(\cdot) h(⋅)来完成的，具体来说，是通过在某一点（通常是上一步的状态估计）处计算这些函数的一阶导数（即雅可比矩阵），来近似非线性函数在该点附近的行为。 状态转移雅可比矩阵 F k F_k Fk：是状态转移函数 f f f相对于状态 x x x的雅可比矩阵，在 x ^ k − 1 \hat{x}_{k-1} x^k−1处求得。它描述了在上一状态估计点附近，状态变化对当前状态的依赖关系。 观测雅可比矩阵 H k H_k Hk：是观测函数 h h h相对于状态 x x x的雅可比矩阵，在预测状态 x ^ k − \hat{x}_k^- x^k−处求得。它描述了在预测状态点附近，测量值对状态的依赖关系。 通过使用这些雅可比矩阵代替 f f f和 h h h的实际非线性形式，EKF将非线性问题转化为了一个在每次迭代中局部线性化的问题。这允许我们使用类似于基本卡尔曼滤波的方法来更新状态估计和协方差矩阵。 EKF的迭代过程EKF的迭代过程包括预测和更新两个步骤，与基本卡尔曼滤波类似，但涉及到上述的线性化处理： 预测步骤： 状态预测： x ^ k − = f ( x ^ k − 1 , u k ) \hat{x}_k^- = f(\hat{x}_{k-1}, u_k) x^k−=f(x^k−1,uk)协方差预测： P k − = F k P k − 1 F k T + Q P_k^- = F_k P_{k-1} F_k^T + Q Pk−=FkPk−1FkT+Q 更新步骤： 计算卡尔曼增益： K k = P k − H k T ( H k P k − H k T + R ) − 1 K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1} Kk=Pk−HkT(HkPk−HkT+R)−1更新状态估计： x ^ k = x ^ k − + K k ( z k − h ( x ^ k − ) ) \hat{x}_k = \hat{x}_k^- + K_k (z_k - h(\hat{x}_k^-)) x^k=x^k−+Kk(zk−h(x^k−))更新协方差估计： P k = ( I − K k H k ) P k − P_k = (I - K_k H_k) P_k^- Pk=(I−KkHk)Pk− 参数与超参数 参数：包括状态转移矩阵 F k F_k Fk、测量矩阵 H k H_k Hk、过程噪声协方差矩阵 Q Q Q和测量噪声协方差矩阵 R R R。这些参数通常基于系统的物理特性或历史数据分析确定。超参数：初始状态估计 x ^ 0 \hat{x}_0 x^0和初始协方差矩阵 P 0 P_0 P0需要基于经验或先验知识设置。 应用示例：跟踪无人驾驶汽车假设我们有一个无人驾驶汽车在一条直线上移动，它的动力系统和传感器都表现出一定程度的非线性特性。我们的目标是使用扩展卡尔曼滤波（EKF）来估计和跟踪这辆车的位置和速度。 系统模型和观测模型非线性系统模型：汽车的动力学可能表现为非线性，例如，汽车的加速度可能与控制输入（如油门踏板的压力）和当前速度的非线性函数。假设状态向量 x k = [ p k , v k ] T x_k = [p_k, v_k]^T xk=[pk,vk]T，其中 p k p_k pk和 v k v_k vk分别表示汽车在时刻 k k k的位置和速度。系统模型可以假设为： x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1}, u_k) + w_k xk=f(xk−1,uk)+wk 其中， f ( ⋅ ) f(\cdot) f(⋅)表示包含位置和速度更新的非线性函数， u k u_k uk是控制输入， w k w_k wk是过程噪声。 非线性观测模型：假设我们使用雷达来测量汽车的距离（位置），雷达的测量可能受到非线性误差的影响（例如，由于信号的传播延迟）。测量模型可以表示为： z k = h ( x k ) + v k z_k = h(x_k) + v_k zk=h(xk)+vk 其中， h ( ⋅ ) h(\cdot) h(⋅)是非线性测量函数， v k v_k vk是测量噪声。 初始化 初始状态估计： x ^ 0 = [ 0 , 0 ] T \hat{x}_0 = [0, 0]^T x^0=[0,0]T，假设汽车从原点出发，初始速度为0。初始协方差矩阵： P 0 P_0 P0，表明对初始状态估计的不确定性。 线性化过程 在每个时间步，我们需要计算 f ( ⋅ ) f(\cdot) f(⋅)和 h ( ⋅ ) h(\cdot) h(⋅)相对于 x x x的雅可比矩阵 F k F_k Fk和 H k H_k Hk，以进行线性化。 EKF迭代过程预测： 使用非线性系统模型进行状态预测： x ^ k − = f ( x ^ k − 1 , u k ) \hat{x}_k^- = f(\hat{x}_{k-1}, u_k) x^k−=f(x^k−1,uk)。更新预测协方差： P k − = F k P k − 1 F k T + Q P_k^- = F_k P_{k-1} F_k^T + Q Pk−=FkPk−1FkT+Q。更新： 计算卡尔曼增益： K k = P k − H k T ( H k P k − H k T + R ) − 1 K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1} Kk=Pk−HkT(HkPk−HkT+R)−1。使用非线性测量模型更新状态估计： x ^ k = x ^ k − + K k ( z k − h ( x ^ k − ) ) \hat{x}_k = \hat{x}_k^- + K_k (z_k - h(\hat{x}_k^-)) x^k=x^k−+Kk(zk−h(x^k−))。更新估计协方差： P k = ( I − K k H k ) P k − P_k = (I - K_k H_k) P_k^- Pk=(I−KkHk)Pk−。通过这个过程，EKF能够有效地对非线性系统进行状态估计和跟踪，即使在系统和测量模型表现出显著非线性特性的情况下。对于无人驾驶汽车，这意味着即使在复杂的驾驶环境和动力学条件下，也能实现准确的位置和速度跟踪，从而为安全驾驶和路径规划提供可靠的信息支持。 为了构建这个扩展卡尔曼滤波（EKF）的示例，我们假设非线性系统模型和观测模型如下： 系统模型： x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1}, u_k) + w_k xk=f(xk−1,uk)+wk，其中 f ( ⋅ ) f(\cdot) f(⋅)表示非线性函数，模拟汽车的动力学。在这个简化的例子中，我们可以假设 f ( x k − 1 , u k ) = x k − 1 + u k Δ t f(x_{k-1}, u_k) = x_{k-1} + u_k \Delta t f(xk−1,uk)=xk−1+ukΔt，即控制输入 u k u_k uk（加速度）直接影响速度和位置的变化。这里 Δ t \Delta t Δt是时间步长。观测模型： z k = h ( x k ) + v k z_k = h(x_k) + v_k zk=h(xk)+vk，其中 h ( ⋅ ) h(\cdot) h(⋅)是非线性函数，代表由雷达测量的位置。在这个例子中，我们简化 h ( x k ) = x k h(x_k) = x_k h(xk)=xk的位置部分。请注意，这些模型的选择是为了说明EKF的实现，真实世界的动力学和观测模型可能更加复杂。 接下来，我们将展示使用Python来实现这个EKF的过程，包括状态预测、更新以及线性化过程的简化实现。注意，为了简化，我们将省略控制输入 u k u_k uk的动力学效应，假设每一步的控制输入为常量。 import numpy as np def f(x, u, dt): """ 非线性状态转移函数。 """ # 简化的动力学模型：位置更新为位置加速度乘时间步长，速度更新为速度加加速度 return np.array([x[0] + x[1]*dt + 0.5*u*dt**2, x[1] + u*dt]) def h(x): """ 非线性观测模型。 """ # 简化的观测模型：直接观测位置 return np.array([x[0]]) def jacobian_f(x, u, dt): """ f的雅可比矩阵。 """ return np.array([[1, dt], [0, 1]]) def jacobian_h(x): """ h的雅可比矩阵。 """ return np.array([[1, 0]]) # 初始参数 x_est = np.array([0, 0]) # 初始状态估计 P = np.array([[1, 0], [0, 1]]) # 初始状态协方差矩阵 Q = np.array([[0.1, 0], [0, 0.1]]) # 过程噪声协方差 R = np.array([[1]]) # 观测噪声协方差 dt = 1 # 时间步长 u = 0.1 # 假设的控制输入（加速度） # 模拟几个时间步 for k in range(1, 4): # 预测 F = jacobian_f(x_est, u, dt) x_pred = f(x_est, u, dt) P_pred = F @ P @ F.T + Q # 更新 H = jacobian_h(x_pred) z = h(x_pred) + np.random.normal(0, np.sqrt(R)) # 模拟观测 y = z - h(x_pred) # 观测残差 S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) x_est = x_pred + K @ y P = (np.eye(2) - K @ H) @ P_pred print(f"Iteration {k}:") print(f"Estimated State: {x_est}") print(f"Estimation Error Covariance: {P}\n")这段代码实现了一个简化版本的EKF，用于跟踪无人驾驶汽车的位置和速度。在每次迭代中，我们首先对系统状态进行预测，然后使用模拟的雷达测量来更新状态估计。注意，为了简化，我们在这里假设了控制输入 u u u为一个小的加速度，并且假设过程噪声和观测噪声都是已知的。 在提供的扩展卡尔曼滤波（EKF）示例代码中，我们模拟了一个非线性系统的状态估计过程，主要用于跟踪一个无人驾驶汽车的位置和速度。以下是代码中使用的各个数值及其含义： 动态系统和观测模型相关数值x_est：表示当前估计的系统状态，初值为[0, 0]。这个向量包含两个元素，分别是汽车的位置（p_k）和速度（v_k）。初始假设汽车位于原点且速度为零。 f(x, u, dt)：非线性状态转移函数，用于描述系统状态（位置和速度）如何随时间变化。在这个简化模型中，我们假设汽车的位置更新为前一状态的位置加上速度乘以时间步长dt，速度更新为前一状态的速度加上控制输入u（加速度）乘以时间步长。 h(x)：非线性观测模型，表示如何从系统状态得到测量值。在这个例子中，我们假设可以直接观测到汽车的位置。 线性化过程 jacobian_f(x, u, dt) 和 jacobian_h(x)：这两个函数分别计算状态转移函数f和观测模型h关于状态x的雅可比矩阵。雅可比矩阵在EKF中用于线性化非线性函数，以便在每一步中近似处理非线性特性。 过程噪声和观测噪声Q：过程噪声协方差矩阵，描述了系统状态转移过程中存在的随机变化的不确定性。在这个示例中，我们假设过程噪声较小，协方差矩阵为对角线上元素为0.1的2×2矩阵。 R：观测噪声协方差矩阵，表示测量过程中的不确定性。这里假设测量噪声的方差为1。 控制输入和时间步长u：控制输入，这里简化为一个恒定的加速度0.1，用于模拟汽车的加速过程。 dt：时间步长，这里假设为1，表示每次迭代代表的时间间隔。 EKF迭代过程中的变量F_k 和 H_k：分别是在当前估计点线性化的状态转移矩阵和观测矩阵的雅可比矩阵。 P：状态估计的协方差矩阵，表示对当前状态估计的不确定性。随着EKF迭代过程的进行，这个矩阵被更新以反映估计的精确度。 K_k：卡尔曼增益，用于在更新步骤中调整测量残差对状态估计的影响。它基于预测协方差和观测噪声，以最小化估计后协方差。 通过这些数值和过程，EKF能够有效地对包含非线性动态的系统进行状态估计和跟踪。 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |