卡尔曼滤波、无迹卡尔曼滤波、扩展卡尔曼滤波、粒子卡尔曼滤波python

### 关于卡尔曼滤波及其变种在 Python 中的实现 卡尔曼滤波是一种用于线性动态系统的状态估计方法，其核心思想是通过递推的方式最小化预测误差方差[^1]。对于非线性和非高斯系统，则有多种扩展形式，如扩展卡尔曼滤波（Extended Kalman Filter, EKF）、无迹卡尔曼滤波（Unscented Kalman Filter, UKF）以及粒子卡尔曼滤波（Particle Kalman Filter）。以下是这些算法在 Python 中的具体实现方式。 #### 1. **标准卡尔曼滤波** 标准卡尔曼滤波适用于线性高斯系统。其实现可以通过定义状态转移矩阵 \( A \)，观测矩阵 \( H \)，过程噪声协方差矩阵 \( Q \)，测量噪声协方差矩阵 \( R \) 和初始条件来完成。以下是一个简单的 Python 实现： ```python import numpy as np def kalman_filter(x_est_prev, P_prev, u, z, A, B, H, Q, R): # 预测阶段 x_pred = A @ x_est_prev + B @ u P_pred = A @ P_prev @ A.T + Q # 更新阶段 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_est = (np.eye(len(x_est)) - K @ H) @ P_pred return x_est, P_est ``` 此函数实现了基本的卡尔曼滤波逻辑，其中 `x_est` 是当前的状态估计，而 `P_est` 则表示状态估计的不确定性[^2]。 --- #### 2. **扩展卡尔曼滤波（EKF）** 当系统是非线性的但仍然接近高斯分布时，可以使用扩展卡尔曼滤波。它通过对非线性函数进行局部线性近似来处理非线性问题。下面是一个基于 Jacobian 的简单实现： ```python from scipy.optimize import approx_fprime def jacobian(f, x, *args): """计算 f 在点 x 处的雅可比矩阵""" epsilon = np.sqrt(np.finfo(float).eps) grad = lambda i: approx_fprime(x[i], lambda xi: f(*((xi,) + args))[i], epsilon)[i] J = np.array([grad(i) for i in range(len(x))]) return J def extended_kalman_filter(x_est_prev, P_prev, u, z, f, h, Q, R): F = jacobian(lambda x: f(x, u), x_est_prev) # 状态转移函数的雅可比矩阵 H = jacobian(h, x_est_prev) # 观测函数的雅可比矩阵 # 预测阶段 x_pred = f(x_est_prev, u) P_pred = F @ P_prev @ F.T + Q # 更新阶段 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_est = (np.eye(len(x_est)) - K @ H) @ P_pred return x_est, P_est ``` 在此代码中，`f` 表示状态转移函数，`h` 表示观测函数，两者均需提供对应的 Jacobian 计算[^2]。 --- #### 3. **无迹卡尔曼滤波（UKF）** 无迹卡尔曼滤波利用 Sigma Points 来捕捉非线性变换后的概率分布特性，从而避免了显式的 Jacobian 或 Hessian 计算。Python 库 `FilterPy` 提供了一个简洁的实现： ```python from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.common import Q_discrete_white_noise def sigma_points_func(x, P): """自定义Sigma Point生成函数""" n = len(x) kappa = 0.0 lambda_ = n + kappa sqrt_lambda_P = np.linalg.cholesky((lambda_) * P) points = [] for i in range(n): points.append(x + sqrt_lambda_P[:, i]) points.append(x - sqrt_lambda_P[:, i]) points.append(x) return np.array(points) ukf = UKF(dim_x=4, dim_z=2, dt=0.1, fx=f_state_transition, hx=h_measurement, points=sigma_points_func) ukf.Q = Q_discrete_white_noise(dim=4, dt=0.1, var=0.1) ukf.R = np.diag([0.1, 0.1]) # 测量噪声协方差 ukf.x = np.array([0., 0., 0., 0.]) # 初始状态 ukf.P = np.eye(4) # 初始状态协方差 for z in measurements: ukf.predict() ukf.update(z) print('State estimate:', ukf.x) ``` 这里的关键在于如何设计 `sigma_points_func` 函数以适配具体的应用场景。 --- #### 4. **粒子卡尔曼滤波（PF）** 粒子卡尔曼滤波适合高度非线性和非高斯的情况。它的原理是对状态空间的概率密度函数进行离散采样并加权平均。以下是一个简化版本的实现： ```python class ParticleFilter: def __init__(self, num_particles, state_dim): self.num_particles = num_particles self.particles = np.random.randn(num_particles, state_dim) self.weights = np.ones(num_particles) / num_particles def predict(self, motion_model, noise_std): self.particles += motion_model() + np.random.normal(scale=noise_std, size=self.particles.shape) def update(self, measurement, likelihood_fn): weights = likelihood_fn(measurement, self.particles) self.weights *= weights self.weights /= sum(self.weights) def resample(self): indices = np.random.choice(range(self.num_particles), p=self.weights, size=self.num_particles) self.particles = self.particles[indices] self.weights = np.ones(self.num_particles) / self.num_particles pf = ParticleFilter(num_particles=1000, state_dim=2) for t in range(time_steps): pf.predict(motion_model=lambda: np.zeros(state_dim), noise_std=0.1) pf.update(measurements[t], likelihood_fn=lambda m, s: np.exp(-0.5 * ((m - s)**2))) pf.resample() ``` 以上代码展示了粒子滤波的核心流程：预测、更新和重采样[^3]。 --- ### 总结 上述四种方法分别针对不同类型的动态系统提供了有效的解决方案。标准卡尔曼滤波适用于线性高斯系统；扩展卡尔曼滤波则能应对轻微非线性情况下的建模需求；无迹卡尔曼滤波进一步减少了对导数计算的要求；而粒子卡尔曼滤波则是解决复杂非线性与非高斯问题的强大工具。