目标跟踪中的状态估计算法：从Kalman滤波到粒子滤波

1. 目标跟踪中的状态估计问题

在目标跟踪领域，我们经常需要从带有噪声的观测数据中估计目标的真实状态（如位置、速度等）。这就像在雾天开车时，仪表盘显示的车速和GPS定位都有误差，我们需要综合各种信息来准确判断车辆的实际位置和速度。

状态估计问题的数学本质可以表示为：

code复制x_k = f(x_{k-1}) + w_k
z_k = h(x_k) + v_k

其中x是系统状态，z是观测值，w和v分别是过程噪声和观测噪声。我们的目标就是从一系列观测{z}中估计出真实的{x}。

2. Kalman滤波器：线性高斯场景的最优解

2.1 基本假设与原理

Kalman滤波器（KF）基于两个核心假设：

1. 系统动态模型和观测模型都是线性的
2. 过程噪声和观测噪声都是高斯白噪声

其工作原理分为预测和更新两个阶段：

预测步骤：

code复制x̂_k|k-1 = F_k x̂_k-1|k-1
P_k|k-1 = F_k P_k-1|k-1 F_k^T + Q_k

更新步骤：

code复制K_k = P_k|k-1 H_k^T (H_k P_k|k-1 H_k^T + R_k)^-1
x̂_k|k = x̂_k|k-1 + K_k (z_k - H_k x̂_k|k-1)
P_k|k = (I - K_k H_k) P_k|k-1

2.2 MATLAB实现示例

matlab复制% 初始化
x = [0; 0]; % 初始状态 [位置; 速度]
P = [1 0; 0 1]; % 初始协方差
F = [1 1; 0 1]; % 状态转移矩阵
H = [1 0]; % 观测矩阵
Q = [0.01 0; 0 0.01]; % 过程噪声协方差
R = 1; % 观测噪声方差

% 模拟数据
true_x = cumsum(randn(100,1));
z = true_x + sqrt(R)*randn(100,1);

% Kalman滤波
for k = 1:length(z)
    % 预测
    x = F * x;
    P = F * P * F' + Q;
    
    % 更新
    K = P * H' / (H * P * H' + R);
    x = x + K * (z(k) - H * x);
    P = (eye(2) - K * H) * P;
    
    est_x(k) = x(1);
end

实际应用中常见问题：当模型不匹配或噪声统计特性未知时，KF性能会显著下降。这时需要采用自适应滤波技术或考虑更复杂的滤波器。

3. 扩展Kalman滤波器（EKF）：处理非线性系统

3.1 基本原理

EKF通过局部线性化处理非线性系统：

code复制x_k = f(x_{k-1}) + w_k
z_k = h(x_k) + v_k

其中f和h是非线性函数。

EKF的关键是计算雅可比矩阵：

code复制F_k ≈ ∂f/∂x|_{x=x̂_k-1|k-1}
H_k ≈ ∂h/∂x|_{x=x̂_k|k-1}

3.2 实现要点

matlab复制% 非线性状态转移函数
f = @(x) [x(1)+x(2); x(2)+0.1*sin(x(1))];
h = @(x) x(1)^2; % 非线性观测

% 计算雅可比矩阵
F_jac = @(x) [1, 1; 0.1*cos(x(1)), 1];
H_jac = @(x) [2*x(1), 0];

for k = 1:length(z)
    % 预测
    x_pred = f(x);
    F = F_jac(x);
    P_pred = F * P * F' + Q;
    
    % 更新
    H = H_jac(x_pred);
    K = P_pred * H' / (H * P_pred * H' + R);
    x = x_pred + K * (z(k) - h(x_pred));
    P = (eye(2) - K * H) * P_pred;
end

注意：EKF的线性化近似可能导致滤波器发散，特别是在强非线性或初始估计误差较大时。实际工程中常采用迭代EKF（IEKF）来提高精度。

4. 高斯滤波器：更精确的非线性处理

4.1 基本思想

高斯滤波器通过直接近似状态分布来处理非线性问题，比EKF更精确。常见的有：

• Unscented Kalman Filter (UKF)
• Cubature Kalman Filter (CKF)
• Gauss-Hermite Kalman Filter (GHKF)

以UKF为例，其核心是Unscented变换：

1. 选择一组sigma点
2. 通过非线性函数传播这些点
3. 计算传播后点的均值和协方差

4.2 UKF实现

matlab复制alpha = 1e-3;
kappa = 0;
beta = 2;
n = length(x);
lambda = alpha^2*(n+kappa)-n;

% Sigma点生成
[sigma, Wm, Wc] = ut_weights(n, lambda, alpha, beta);

for k = 1:length(z)
    % 生成sigma点
    X = sigmapoints(x, P, lambda);
    
    % 预测
    X_pred = zeros(size(X));
    for i = 1:2*n+1
        X_pred(:,i) = f(X(:,i));
    end
    x_pred = X_pred * Wm';
    P_pred = zeros(n);
    for i = 1:2*n+1
        P_pred = P_pred + Wc(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)';
    end
    P_pred = P_pred + Q;
    
    % 更新
    Z_pred = zeros(1,2*n+1);
    for i = 1:2*n+1
        Z_pred(i) = h(X_pred(:,i));
    end
    z_pred = Z_pred * Wm';
    
    Pzz = 0;
    Pxz = zeros(n,1);
    for i = 1:2*n+1
        Pzz = Pzz + Wc(i)*(Z_pred(i)-z_pred)^2;
        Pxz = Pxz + Wc(i)*(X_pred(:,i)-x_pred)*(Z_pred(i)-z_pred)';
    end
    Pzz = Pzz + R;
    
    K = Pxz / Pzz;
    x = x_pred + K*(z(k)-z_pred);
    P = P_pred - K*Pzz*K';
end

5. 粒子滤波器：非高斯非线性场景的解决方案

5.1 基本原理

粒子滤波器（PF）通过蒙特卡洛方法近似后验分布，特别适合非高斯噪声和多模态分布。

基本步骤：

1. 初始化粒子群
2. 重要性采样
3. 重采样
4. 状态估计

5.2 SIR粒子滤波器实现

matlab复制N = 1000; % 粒子数
particles = zeros(2,N); % 每个粒子是状态向量
weights = ones(1,N)/N;

for k = 1:length(z)
    % 重要性采样
    for i = 1:N
        particles(:,i) = f(particles(:,i)) + sqrt(Q)*randn(2,1);
        weights(i) = normpdf(z(k), h(particles(:,i)), sqrt(R));
    end
    weights = weights/sum(weights);
    
    % 重采样
    idx = systematic_resample(weights);
    particles = particles(:,idx);
    weights = ones(1,N)/N;
    
    % 状态估计
    x_est = mean(particles,2);
end

粒子退化问题：随着时间推移，少数粒子会占据大部分权重。有效的重采样策略（如系统重采样、残差重采样）对PF性能至关重要。

6. 滤波器选择与实践建议

6.1 比较总结

6.2 工程实践建议

1. 先尝试最简单的KF，验证系统基本模型
2. 对于非线性系统，优先尝试UKF而非EKF
3. 只有在存在明显非高斯特性时才考虑PF
4. 实时性要求高的场景注意计算复杂度
5. 始终进行充分的仿真测试和参数调优

我在实际目标跟踪项目中发现，UKF通常在精度和计算效率之间提供了很好的平衡。对于汽车跟踪这类应用，UKF配合适当的过程噪声建模，能达到厘米级的定位精度。而PF虽然理论上更通用，但其计算成本和实现复杂度往往限制了实际应用。