1. 非线性状态评估与UKF算法概述

在工程实践中，我们经常需要处理非线性系统的状态估计问题。传统卡尔曼滤波器(KF)在线性高斯系统中表现优异，但当系统存在显著非线性时，其性能会急剧下降。无迹卡尔曼滤波器(Unscented Kalman Filter, UKF)通过采用无迹变换(Unscented Transform)技术，有效解决了非线性系统的状态估计难题。

UKF的核心思想是：选择一组精心设计的采样点（称为sigma点），这些点能够精确捕获随机变量的均值和协方差。将这些sigma点通过非线性系统传播后，再重新计算传播后的均值和协方差。这种方法避免了线性化带来的误差，对高度非线性系统特别有效。

2. UKF算法原理详解

2.1 Sigma点生成策略

UKF的第一步是生成sigma点。对于n维状态向量x，其均值为x̄，协方差为P，我们通常选择2n+1个sigma点：

X₀ = x̄ Xᵢ = x̄ + (√(n+λ)P)ᵢ, i=1,...,n Xᵢ = x̄ - (√(n+λ)P)ᵢ-n, i=n+1,...,2n

其中λ=α²(n+κ)-n是缩放参数，α决定sigma点的分布范围（通常取1e-3≤α≤1），κ是次要缩放参数（通常取0或3-n）。

2.2 权重计算

每个sigma点都有两个权重：均值权重Wₘ和协方差权重Wₖ：

Wₘ⁰ = λ/(n+λ) Wₖ⁰ = λ/(n+λ) + (1-α²+β) Wₘⁱ = Wₖⁱ = 1/[2(n+λ)], i=1,...,2n

β用于包含x的先验分布信息（对于高斯分布，β=2最优）。

3. Matlab实现步骤

3.1 初始化参数

function [x_est, P_est] = ukf_filter(f,h,x0,P0,Q,R,z) % 参数设置 alpha = 1e-3; % 默认值 beta = 2; % 高斯分布最优值 kappa = 0; % 默认值 n = length(x0); % 状态维度 lambda = alpha^2*(n+kappa) - n; % 权重计算 Wm = [lambda/(n+lambda), 0.5/(n+lambda)*ones(1,2*n)]; Wc = [(lambda/(n+lambda)+(1-alpha^2+beta)), 0.5/(n+lambda)*ones(1,2*n)];

3.2 Sigma点生成函数

function X = sigma_points(x, P, lambda) n = length(x); X = zeros(n, 2*n+1); X(:,1) = x; sqrt_matrix = sqrtm((n+lambda)*P); for i=1:n X(:,i+1) = x + sqrt_matrix(:,i); X(:,i+n+1) = x - sqrt_matrix(:,i); end end

3.3 预测步骤实现

% 生成sigma点 X = sigma_points(x0, P0, lambda); % 通过过程模型传播 X_pred = zeros(size(X)); for i=1:size(X,2) X_pred(:,i) = f(X(:,i)); % f为过程模型函数 end % 计算预测均值和协方差 x_pred = zeros(n,1); for i=1:size(X_pred,2) x_pred = x_pred + Wm(i)*X_pred(:,i); end P_pred = Q; % 添加过程噪声 for i=1:size(X_pred,2) P_pred = P_pred + Wc(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)'; end

4. 更新步骤实现

4.1 观测预测

% 重新生成sigma点 X_sig = sigma_points(x_pred, P_pred, lambda); % 通过观测模型传播 Z_pred = zeros(size(z,1), size(X_sig,2)); for i=1:size(X_sig,2) Z_pred(:,i) = h(X_sig(:,i)); % h为观测模型函数 end % 计算预测观测均值 z_pred = zeros(size(z)); for i=1:size(Z_pred,2) z_pred = z_pred + Wm(i)*Z_pred(:,i); end

4.2 协方差计算与卡尔曼增益

% 计算协方差 Pzz = R; % 添加观测噪声 Pxz = zeros(n, size(z,1)); for i=1:size(Z_pred,2) Pzz = Pzz + Wc(i)*(Z_pred(:,i)-z_pred)*(Z_pred(:,i)-z_pred)'; Pxz = Pxz + Wc(i)*(X_sig(:,i)-x_pred)*(Z_pred(:,i)-z_pred)'; end % 卡尔曼增益 K = Pxz / Pzz; % 状态更新 x_est = x_pred + K*(z - z_pred); P_est = P_pred - K*Pzz*K';

5. 应用实例：车辆轨迹跟踪

5.1 系统建模

考虑一个二维平面内的车辆运动模型：

状态向量：x = [px; py; v; θ] （位置x,y，速度，航向角）

过程模型（CTRV模型）：

function x_next = cv_model(x, dt) theta = x(4); v = x(3); x_next = x; x_next(1) = x(1) + v*cos(theta)*dt; x_next(2) = x(2) + v*sin(theta)*dt; x_next(4) = x(4); % 假设无转向 end

观测模型（直接观测位置）：

function z = obs_model(x) z = x(1:2); % 只观测位置 end

5.2 完整实现流程

% 初始化 x = [0; 0; 5; 0]; % 初始状态 P = diag([0.1, 0.1, 0.5, 0.1]); % 初始协方差 Q = diag([0.1, 0.1, 0.1, 0.1]); % 过程噪声 R = diag([1, 1]); % 观测噪声 % 生成模拟数据 true_states = zeros(4, 100); measurements = zeros(2, 100); for t=1:100 true_states(:,t) = x; measurements(:,t) = x(1:2) + sqrt(R)*randn(2,1); x = cv_model(x, 0.1); end % UKF滤波 est_states = zeros(4, 100); x_est = [0; 0; 0; 0]; % 初始估计 P_est = diag([1, 1, 1, 1]); for t=1:100 [x_est, P_est] = ukf_filter(@(x)cv_model(x,0.1), @obs_model, ... x_est, P_est, Q, R, measurements(:,t)); est_states(:,t) = x_est; end

6. 性能优化技巧

6.1 数值稳定性处理

在实际实现中，需要特别注意协方差矩阵的正定性：

1. 使用Cholesky分解代替直接矩阵开方：

[L,flag] = chol((n+lambda)*P, 'lower'); if flag>0 L = sqrt(n+lambda)*chol(P + 1e-6*eye(n)); % 添加小扰动 end

2. 采用平方根UKF（SR-UKF）算法，直接传播协方差矩阵的平方根。

6.2 参数调优建议

1. α的选择：

• 小α（1e-3）适用于弱非线性系统
• 大α（1）适用于强非线性系统

2. 过程噪声Q和观测噪声R的调整：

• 可以通过创新序列（z - z_pred）的自相关性来验证
• 理想情况下，标准化创新序列应服从N(0,1)分布

7. 常见问题排查

7.1 滤波器发散

症状：估计误差不断增大 可能原因：

1. 过程噪声Q设置过小
2. 初始协方差P0设置过小
3. 系统模型不准确

解决方案：

1. 适当增大Q的对角元素
2. 检查模型实现是否正确
3. 考虑使用自适应UKF

7.2 数值不稳定

症状：协方差矩阵失去正定性 可能原因：

1. 数值舍入误差累积
2. 系统可观测性差

解决方案：

1. 改用平方根UKF实现
2. 添加小扰动保持正定性
3. 检查系统可观测性

8. 扩展应用

8.1 自适应UKF

通过实时调整过程噪声Q和观测噪声R：

% 计算创新序列 innov = z - z_pred; % 自适应调整 if t > 10 R_adapt = 0.9*R_adapt + 0.1*(innov*innov' + Pzz); Q_adapt = 0.9*Q_adapt + 0.1*(K*(innov*innov')*K'); end

8.2 交互多模型UKF

对于多模态系统，可以结合多个UKF滤波器：

% 初始化多个模型 models = {ukf1, ukf2, ukf3}; model_prob = [0.8, 0.1, 0.1]; % 初始模型概率 for t=1:steps % 每个模型独立预测和更新 for m=1:length(models) [x_est{m}, P_est{m}] = models{m}.update(z); % 计算模型似然 innov = z - models{m}.z_pred; S = models{m}.Pzz; model_prob(m) = mvnpdf(innov', zeros(1,length(innov)), S) * model_prob(m); end % 归一化模型概率 model_prob = model_prob / sum(model_prob); % 模型交互 x_combined = zeros(size(x_est{1})); P_combined = zeros(size(P_est{1})); for m=1:length(models) x_combined = x_combined + model_prob(m)*x_est{m}; end for m=1:length(models) P_combined = P_combined + model_prob(m)*(P_est{m} + ... (x_est{m}-x_combined)*(x_est{m}-x_combined)'); end end

9. 与其他非线性滤波器的比较

9.1 UKF vs EKF

优势：

1. 无需计算雅可比矩阵
2. 对强非线性系统精度更高
3. 实现更简单

劣势：

1. 计算量略大（需要传播2n+1个点）
2. 对参数选择更敏感

9.2 UKF vs 粒子滤波(PF)

优势：

1. 计算效率更高
2. 确定性采样无随机性
3. 对小规模问题更适用

劣势：

1. 对高维问题效果下降
2. 难以处理多模态分布

10. 工程实践建议

1. 模型验证：始终先用仿真数据验证滤波器实现
2. 可视化：绘制误差曲线和3σ置信区间
3. 记录：保存每次迭代的协方差矩阵迹以监控性能
4. 模块化：将UKF实现为可重用类/函数

classdef UKF < handle properties x; P; Q; R; alpha; beta; kappa; Wm; Wc; end methods function obj = UKF(x0, P0, Q, R, alpha, beta, kappa) % 初始化代码 end function predict(obj, f) % 预测步骤 end function update(obj, h, z) % 更新步骤 end end end

实际项目中，UKF的参数需要根据具体应用场景进行调整。建议先用仿真数据确定合适的Q、R和UKF参数，再应用到真实系统中。对于关键应用，应考虑实现故障检测机制，当创新序列超出合理范围时触发警报。