（二）多传感器EKF融合实战：从理论推导到MATLAB仿真实现

1. 多传感器EKF融合的核心思想

我第一次接触EKF（扩展卡尔曼滤波）是在做无人机姿态估计项目时。当时手头有IMU和视觉传感器数据，但单独使用任一种传感器都存在明显缺陷：IMU短期精度高但会漂移，视觉数据稳定但易受环境干扰。这时候导师扔给我一篇论文："试试EKF融合吧"——没想到这个数学工具成了解决难题的钥匙。

EKF本质上是对经典卡尔曼滤波的"魔改"，专门对付非线性系统。它的核心绝招是一阶泰勒展开：把非线性函数在当前估计点附近线性化。举个例子，就像用直尺在曲线某一点做切线，虽然不够完美，但在小范围内足够近似。实际工程中，90%的非线性问题都能用这招搞定。

多传感器融合时，EKF的威力更明显。去年给工厂做AGV定位系统，融合了激光雷达、UWB和轮速计数据。单个UWB基站误差可能有20cm，但通过EKF融合后整体定位精度提升到5cm内。这背后的数学魔法，就是通过协方差矩阵自动判断该相信哪个传感器——误差小的传感器话语权更大。

2. 从零推导EKF的完整过程

2.1 非线性系统建模

假设我们在跟踪一个做机动运动的无人机，状态向量取位置和速度：

x = [px, py, vx, vy]^T

运动模型可能是这样的非线性方程：

function x_next = motion_model(x, dt) theta = atan2(x(4),x(3)); % 运动方向 v = norm([x(3),x(4)]); % 速度大小 x_next = x + [v*cos(theta)*dt; v*sin(theta)*dt; -0.1*v^2*cos(theta)*dt; -0.1*v^2*sin(theta)*dt]; % 空气阻力导致的非线性 end

这个模型考虑了空气阻力与速度平方成正比的情况——典型的非线性系统。

2.2 雅可比矩阵计算

雅可比矩阵是EKF的灵魂，它记录了各状态变量间的耦合关系。以上述模型为例，其雅可比矩阵F为：

function F = jacobian_F(x, dt) vx = x(3); vy = x(4); v = sqrt(vx^2 + vy^2); F = [1, 0, dt*(vy^2)/v^3, -dt*vx*vy/v^3; 0, 1, -dt*vx*vy/v^3, dt*(vx^2)/v^3; 0, 0, 1-0.2*vx*dt, -0.2*vy*dt; 0, 0, -0.2*vx*dt, 1-0.2*vy*dt]; end

我第一次推导时在这个环节卡了整整两天——求偏导时漏掉了链式法则的项，导致仿真结果完全发散。后来用MATLAB的符号计算工具验证才发现问题。

3. MATLAB仿真实战

3.1 仿真环境搭建

我们先构造一个带噪声的非线性系统：

% 参数设置 dt = 0.1; % 采样时间 T = 10; % 总时长 Q = diag([0.1, 0.1, 0.5, 0.5])^2; % 过程噪声 R1 = diag([0.5, 0.5]); % 传感器1噪声 R2 = diag([1, 1]); % 传感器2噪声 % 真实轨迹生成 true_states = zeros(4, T/dt); for k = 2:length(true_states) true_states(:,k) = motion_model(true_states(:,k-1), dt) + sqrt(Q)*randn(4,1); end

3.2 单传感器EKF实现

以第一个传感器为例，EKF核心代码：

% 初始化 x_est = [0; 0; 1; 0]; % 初始估计 P = eye(4); % 初始协方差 for k = 1:length(true_states) % 预测步骤 F = jacobian_F(x_est, dt); x_pred = motion_model(x_est, dt); P_pred = F*P*F' + Q; % 更新步骤 z = true_states(1:2,k) + sqrt(R1)*randn(2,1); % 模拟观测 H = [1 0 0 0; 0 1 0 0]; % 观测矩阵 K = P_pred*H'/(H*P_pred*H' + R1); x_est = x_pred + K*(z - H*x_pred); P = (eye(4) - K*H)*P_pred; % 存储结果 estimates1(:,k) = x_est; end

3.3 多传感器融合策略

简单凸组合融合的MATLAB实现：

% 假设已有两个传感器的估计结果x1, x2和协方差P1, P2 P_fused = inv(inv(P1) + inv(P2)); x_fused = P_fused*(inv(P1)*x1 + inv(P2)*x2);

这个看似简单的公式其实暗藏玄机：它相当于给每个传感器的估计值按精度（协方差倒数）分配了权重。我在实际项目中验证过，这种融合方式比直接取平均精度提升30%以上。

4. 结果分析与性能提升

4.1 误差对比测试

用以下代码计算各方法误差：

err_raw = vecnorm(true_states(1:2,:) - measurements, 2); err_ekf1 = vecnorm(true_states(1:2,:) - estimates1(1:2,:), 2); err_fused = vecnorm(true_states(1:2,:) - fused_estimates, 2);

典型结果对比：

4.2 调试经验分享

遇到过最棘手的问题是EKF发散。后来总结出几个关键检查点：

1. 雅可比矩阵验证：用MATLAB的jacobian函数自动计算对比
2. 噪声协方差调整：通过Allan方差分析确定Q矩阵
3. 数值稳定性：使用Joseph形式协方差更新

P = (eye(4)-K*H)*P_pred*(eye(4)-K*H)' + K*R*K';

5. 工程实践中的技巧

在实际机器人项目中，有几点教科书不会告诉你的经验：

• 异步传感器处理：不同传感器采样率不同，我的做法是维护一个预测队列，收到新测量时选择最近的状态预测进行更新
• ** outlier处理**：添加马氏距离检测

innov = z - H*x_pred; S = H*P_pred*H' + R; if innov'*inv(S)*innov > chi2inv(0.99,2) % 丢弃异常值 end

• 计算优化：预先分配数组内存，避免MATLAB循环中的动态扩容

最后分享一个真实案例：在室内无人机项目中，仅用售价百元的低成本IMU和单目相机，通过EKF融合实现了厘米级定位。关键是在IMU噪声建模时考虑了温度漂移，建立了Q矩阵的温度补偿模型。这再次验证了EKF的强大——只要模型足够准确，廉价传感器也能出好效果。