EKF与UKF在9维状态估计中的原理与实现对比

1. 扩展卡尔曼滤波(EKF)与无迹卡尔曼滤波(UKF)原理剖析

在目标跟踪和状态估计领域，卡尔曼滤波算法一直占据着核心地位。对于线性高斯系统，标准卡尔曼滤波能够提供最优估计。然而实际工程中绝大多数系统都是非线性的，这就催生了EKF和UKF两种经典的非线性滤波方法。

1.1 EKF的核心思想与数学推导

EKF的基本思路是通过一阶泰勒展开对非线性系统进行局部线性化。具体到9维状态空间，假设我们的状态向量包含位置(x,y,z)、速度(vx,vy,vz)和姿态角(roll,pitch,yaw)，则状态方程可以表示为：

matlab复制function x_next = stateFunc(x_prev, u)
    % x_prev: 上一时刻状态向量 [9×1]
    % u: 控制输入
    dt = 0.1; % 时间步长
    x_next = x_prev + [x_prev(4:6); 
                       acceleration(x_prev(7:9), u); 
                       angularRate(x_prev(7:9), u)] * dt;
end

EKF的关键在于计算雅可比矩阵。对于上述状态函数，雅可比矩阵F的解析解通常难以获得，可以采用数值微分方法：

matlab复制function F = computeJacobian(x, u)
    eps = 1e-6;
    F = zeros(9,9);
    f0 = stateFunc(x, u);
    for i = 1:9
        x_perturbed = x;
        x_perturbed(i) = x_perturbed(i) + eps;
        F(:,i) = (stateFunc(x_perturbed, u) - f0)/eps;
    end
end

实际工程中建议：对于简单系统可以推导解析雅可比，复杂系统建议使用自动微分工具，数值微分仅作为最后选择。

1.2 UKF的无迹变换原理

UKF采用完全不同的思路——无迹变换(UT)。它通过精心选择的一组Sigma点来捕捉状态的统计特性。对于9维系统，我们需要2×9+1=19个Sigma点：

matlab复制function [sigmaPoints, weights] = generateSigmaPoints(x, P)
    n = length(x);
    alpha = 1e-3;
    kappa = 0;
    beta = 2; % 最优参数选择
    lambda = alpha^2*(n+kappa) - n;
    
    sigmaPoints = zeros(n, 2*n+1);
    weights = zeros(1, 2*n+1);
    
    sigmaPoints(:,1) = x;
    weights(1) = lambda/(n+lambda);
    
    [U,S,~] = svd((n+lambda)*P);
    sqrtP = U*sqrt(S);
    
    for i = 1:n
        sigmaPoints(:,i+1) = x + sqrtP(:,i);
        sigmaPoints(:,n+i+1) = x - sqrtP(:,i);
        weights(i+1) = 1/(2*(n+lambda));
        weights