扩展卡尔曼滤波和粒子滤波原理到代码实践
非线性系统的状态估计总带着点玄学色彩。扩展卡尔曼滤波（EKF）像是个数学魔术师，总能把曲线掰直了看。先看个经典案例——雷达跟踪目标。假设目标在做匀速圆周运动，状态向量[x, y, vx, vy]，观测的是极坐标下的距离和方位角。
import numpy as np def ekf_predict(x, P, Q): dt = 0.1 omega = 0.5 # 角速度 F = np.array([[1, 0, np.sin(omega*dt)/omega, (1-np.cos(omega*dt))/omega], [0, 1, (1-np.cos(omega*dt))/omega, np.sin(omega*dt)/omega], [0, 0, np.cos(omega*dt), -np.sin(omega*dt)], [0, 0, np.sin(omega*dt), np.cos(omega*dt)]]) return F @ x, F @ P @ F.T + Q def measurement_jacobian(x): # 观测雅可比矩阵（笛卡尔转极坐标） r = np.sqrt(x[0]**2 + x[1]**2) H = np.array([ [x[0]/r, x[1]/r, 0, 0], [-x[1]/r**2, x[0]/r**2, 0, 0] ]) return H预测步骤用旋转矩阵处理圆周运动，雅可比矩阵求导时得注意极坐标转换的奇点问题。但遇到强非线性场景，比如蛇形机动目标，EKF就容易翻车——这时候粒子滤波（PF）该出场了。
粒子滤波像是个民主投票系统，每个粒子代表一种可能的状态轨迹。来看个地面机器人定位的简化版实现：
def particle_filter(particles, weights, z, R): # 预测阶段（添加运动噪声） particles = motion_model(particles) + np.random.randn(*particles.shape)*0.1 # 更新权重（观测似然） dx = particles[:,0] - z[0] dy = particles[:,1] - z[1] weights = np.exp(-0.5*(dx**2 + dy**2)/R) weights /= np.sum(weights) # 系统重采样 indices = np.random.choice(range(len(particles)), size=len(particles), p=weights) return particles[indices], np.ones_like(weights)/len(weights)这个实现里有三个关键点：运动模型传播、观测加权、重采样。注意重采样后的权重要重置为均匀分布，否则下次更新会出问题。当粒子聚集在错误区域时，会出现"粒子贫化"现象——好比全员投错票，这时候需要增加扰动或者采用辅助粒子滤波。
扩展卡尔曼滤波和粒子滤波原理到代码实践
两种方法在实际中常配合使用：EKF负责日常状态跟踪，遇到突变切到粒子滤波应急。比如自动驾驶中，正常行驶用EKF，突然检测到障碍物时启动粒子滤波做多假设跟踪。代码层面可以这样切换：
if innovation_norm < threshold: x, P = ekf_update(x, P, z) else: particles = generate_maneuver_hypotheses(x) x, P = particle_filter_update(particles)创新量（innovation）的大小反映了观测与预测的偏离程度，类似异常检测机制。这种混合策略在工程中很常见，既保证计算效率又提升鲁棒性。
滤波器实现时容易踩的坑：协方差矩阵失去正定性，粒子权值下溢，雅可比矩阵计算错误。有个调试技巧——给系统输入已知轨迹，观察估计误差是否在3σ范围内。比如用匀速直线运动验证EKF时，横向误差应该满足：
assert np.all(np.abs(position_error) < 3*np.sqrt(P[0,0] + P[1,1]))最后提个实战经验：别迷信理论性能比较，实际效果取决于噪声特性建模的准确度。曾经有个项目，把过程噪声协方差Q中的0.01改成0.015，定位精度直接提升30%——参数调优才是滤波工程的灵魂。
