卡尔曼滤波与粒子滤波：状态估计算法Matlab实现

1. 状态估计算法概述：从卡尔曼滤波到粒子滤波

在工程实践中，我们常常需要从带有噪声的观测数据中推断系统的真实状态。这类问题在自动驾驶、机器人导航、工业控制等领域尤为常见。状态估计算法就是为解决这类问题而发展起来的一整套数学工具。

卡尔曼滤波（Kalman Filter）无疑是这一领域最经典的算法。1960年由Rudolf E. Kalman提出后，它迅速在阿波罗登月计划中得到应用，并从此成为工程领域的标准工具。其核心思想是通过预测-更新两个步骤的循环迭代，实现对系统状态的最优估计。

随着应用场景的复杂化，传统卡尔曼滤波的局限性也逐渐显现。非线性系统需要扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF），而非高斯噪声环境则催生了粒子滤波（PF）等蒙特卡洛方法。这些算法各有所长：

• EKF通过一阶泰勒展开处理非线性
• UKF采用sigma点采样保持二阶精度
• PF用粒子集近似任意分布

在Matlab环境下实现这些算法，既能深入理解其数学本质，又能快速验证实际效果。下面我将结合代码实例，详细解析这几种滤波器的实现要点和应用技巧。

2. 卡尔曼滤波基础与Matlab实现

2.1 线性卡尔曼滤波原理

标准卡尔曼滤波针对线性高斯系统，其核心是以下五个方程：

1. 状态预测：

   math复制\hat{x}_k^- = A\hat{x}_{k-1} + Bu_{k-1}
   

2. 协方差预测：

   math复制P_k^- = AP_{k-1}A^T + Q
   

3. 卡尔曼增益：

   math复制K_k = P_k^-H^T(HP_k^-H^T + R)^{-1}
   

4. 状态更新：

   math复制\hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-)
   

5. 协方差更新：

   math复制P_k = (I - K_kH)P_k^-
   

提示：Q和R分别表示过程噪声和观测噪声的协方差矩阵，需要根据具体系统特性仔细调整。

2.2 Matlab实现要点

以下是一个简洁的KF实现框架：

matlab复制function [x_est, P] = kalman_filter(z, u, x_prev, P_prev, A, B, H, Q, R)
    % 预测步骤
    x_pred = A * x_prev + B * u;
    P_pred = A * P_prev * A' + Q;
    
    % 更新步骤
    K = P_pred * H' / (H * P_pred * H' + R);
    x_est = x_pred + K * (z - H * x_pred);
    P = (eye(size(K,1)) - K * H) * P_pred;
end

实际应用中需要注意：

1. 矩阵维度必须严格匹配
2. Q/R需要合理初始化（通常从0.01开始调试）
3. 对于不稳定系统可能需要添加稳定性检查

3. 非线性滤波：EKF与UKF的实现对比

3.1 扩展卡尔曼滤波(EKF)实现

EKF通过一阶泰勒展开处理非线性：

matlab复制function [x_est, P] = ekf(f, h, z, u, x_prev, P_prev, Q, R)
    % 计算雅可比矩阵
    F = jacobian(f, x_prev);
    H_j = jacobian(h, x_prev);
    
    % 预测步骤
    x_pred = f(x_prev, u);
    P_pred = F * P_prev * F' + Q;
    
    % 更新步骤
    K = P_pred * H_j' / (H_j * P_pred * H_j' + R);
    x_est = x_pred + K * (z - h(x_pred));
    P = (eye(size(K,1)) - K * H_j) * P_pred;
end

注意：EKF在强非线性时可能发散，需要谨慎使用。

3.2 无迹卡尔曼滤波(UKF)实现

UKF采用sigma点采样策略，典型实现如下：

matlab复制function [x_est, P] = ukf(f, h, z, x_prev, P_prev, Q, R)
    % Sigma点参数
    alpha = 1e-3;
    beta = 2;
    kappa = 0;
    n = length(x_prev);
    lambda = alpha^2*(n+kappa)-n;
    
    % 生成Sigma点
    [X, W] = sigma_points(x_prev, P_prev, lambda, alpha, beta);
    
    % 预测步骤
    X_pred = zeros(size(X));
    for i=1:size(X,2)
        X_pred(:,i) = f(X(:,i));
    end
    x_pred = X_pred * W(:);
    P_pred = Q;
    for i=1:size(X,2)
        P_pred = P_pred + W(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)';
    end
    
    % 更新步骤
    Z_pred = zeros(size(z,1),size(X,2));
    for i=1:size(X,2)
        Z_pred(:,i) = h(X_pred(:,i));
    end
    z_pred = Z_pred * W(:);
    
    Pzz = R;
    Pxz = zeros(n,size(z,1));
    for i=1:size(X,2)
        Pzz = Pzz + W(i)*(Z_pred(:,i)-z_pred)*(Z_pred(:,i)-z_pred)';
        Pxz = Pxz + W(i)*(X_pred(:,i)-x_pred)*(Z_pred(:,i)-z_pred)';
    end
    
    K = Pxz / Pzz;
    x_est = x_pred + K*(z - z_pred);
    P = P_pred - K*Pzz*K';
end

UKF优势在于：

1. 无需计算雅可比矩阵
2. 能捕捉二阶非线性特性
3. 实现相对稳定

4. 粒子滤波(PF)的Matlab实现

4.1 基本原理

粒子滤波采用蒙特卡洛方法，用一组带权重的粒子近似后验分布：

matlab复制function [x_est, particles] = particle_filter(f, h, z, particles, Q, R)
    N = size(particles,2);
    
    % 重要性采样
    for i=1:N
        particles(:,i) = f(particles(:,i)) + chol(Q)'*randn(size(Q,1),1);
        w(i) = exp(-0.5*(z-h(particles(:,i)))'*inv(R)*(z-h(particles(:,i))));
    end
    w = w/sum(w);
    
    % 重采样
    idx = systematic_resample(w);
    particles = particles(:,idx);
    x_est = mean(particles,2);
end

4.2 实现技巧

1. 粒子数量选择：通常100-1000个，视系统维度而定
2. 重采样策略：系统重采样优于多项式重采样
3. 退化处理：有效粒子数阈值设为N/2

5. 算法对比与工程实践建议

5.1 性能对比表

5.2 选型建议

1. 优先尝试UKF：平衡精度与复杂度
2. 极端非线性考虑PF：但要注意计算成本
3. 实时系统慎用PF：可能无法满足时序要求

6. 典型问题排查指南

1. 滤波器发散：

   • 检查Q/R设置
   • 验证系统可观测性
   • 尝试减小步长

2. 估计滞后：

   • 增大过程噪声Q
   • 检查模型准确性
   • 考虑增加状态维度

3. 粒子退化：

   • 增加粒子数量
   • 改进重采样策略
   • 调整建议分布

实际项目中，我通常会先用仿真验证算法行为。例如构建一个非线性系统模型，注入不同特性的噪声，观察各滤波器的表现。Matlab的Simulink环境特别适合这类验证工作。