卡尔曼滤波、粒子滤波、扩展卡尔曼滤波

### 关于卡尔曼滤波及其变种在 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]。 --- ### 总结 上述四种方法分别针对不同类型的动态系统提供了有效的解决方案。标准卡尔曼滤波适用于线性高斯系统；扩展卡尔曼滤波则能应对轻微非线性情况下的建模需求；无迹卡尔曼滤波进一步减少了对导数计算的要求；而粒子卡尔曼滤波则是解决复杂非线性与非高斯问题的强大工具。

卡尔曼滤波是一种用于估计系统状态的递归滤波器，它基于线性系统模型和高斯噪声假设。卡尔曼滤波的原理如下： 1. 预测步骤：根据系统的动态模型和上一时刻的状态估计，通过状态转移方程预测当前时刻的状态和协方差矩阵。 2. 更新步骤：根据测量模型和当前时刻的观测值，通过观测方程计算卡尔曼增益，然后使用该增益来更新状态估计和协方差矩阵。 扩展卡尔曼滤波（Extended Kalman Filter，EKF）是卡尔曼滤波的一种扩展，用于非线性系统的状态估计。它通过在预测和更新步骤中使用线性化的非线性模型来近似非线性系统。具体原理如下： 1. 预测步骤：与卡尔曼滤波相同，使用非线性系统模型进行状态预测。 2. 更新步骤：与卡尔曼滤波不同，使用线性化的非线性观测模型进行状态更新。 粒子滤波（Particle Filter）是一种基于蒙特卡洛方法的滤波器，用于非线性和非高斯系统的状态估计。它通过使用一组粒子来表示状态的后验概率分布，并根据观测值对粒子进行重采样和权重更新来逼近真实的后验概率分布。具体原理如下： 1. 初始化：根据先验概率分布，生成一组初始粒子。 2. 预测步骤：根据系统模型，对每个粒子进行状态预测。 3. 权重更新：根据观测模型，计算每个粒子的权重，并进行归一化。 4. 重采样：根据粒子的权重，进行有放回抽样，生成新的粒子集合。