当前位置：首页 > news >正文

MATLAB实现UKF（无迹卡尔曼滤波）

news 2025/11/6 7:31:42

UKF（Unscented Kalman Filter，无迹卡尔曼滤波）是一种用于非线性系统状态估计的强大算法。与扩展卡尔曼滤波(EKF)不同，UKF不需要计算雅可比矩阵，而是通过无迹变换来近似非线性函数的概率分布。

UKF原理概述

UKF的核心思想是：选择一组有代表性的样本点（Sigma点），让这些点通过非线性系统，然后基于这些点的变换结果来估计系统的均值和协方差。

UKF算法步骤

初始化
- 初始状态估计 \(\hat{x}_0\)
- 初始误差协方差 \(P_0\)
Sigma点生成
- 根据当前状态和协方差生成一组Sigma点
时间更新（预测）
- 将Sigma点通过非线性状态转移函数
- 计算预测状态和预测协方差
测量更新（校正）
- 将预测的Sigma点通过非线性测量函数
- 计算预测测量值、协方差和交叉协方差
- 计算卡尔曼增益
- 更新状态估计和误差协方差

MATLAB实现

下面是一个完整的UKF实现示例，用于估计一个非线性系统的状态：

%% UKF（无迹卡尔曼滤波）MATLAB实现
clear; close all; clc;%% 参数设置
dt = 0.1;               % 采样时间
T = 10;                 % 总时间
N = T/dt;               % 时间步数
t = 0:dt:T;             % 时间向量% 过程噪声协方差
Q = diag([0.1, 0.1, 0.01, 0.01]);% 测量噪声协方差
R = diag([1, 0.5]);% 初始状态
x0 = [0; 0; 1; 0];      % [px, py, vx, vy]% 初始估计误差协方差
P0 = diag([1, 1, 0.5, 0.5]);% UKF参数
alpha = 1e-3;           % 控制Sigma点分布的参数
beta = 2;               % 包含先验分布信息的参数
kappa = 0;              % 次要缩放参数%% 生成真实轨迹和测量数据
fprintf('生成真实轨迹和测量数据...\n');% 状态转移函数（非线性）
f = @(x) [x(1) + x(3)*dt;x(2) + x(4)*dt;x(3) + 0.1*sin(0.5*t(find(x(1)==x(1),1))*dt);x(4) + 0.1*cos(0.5*t(find(x(1)==x(1),1))*dt)
];% 测量函数（非线性）
h = @(x) [sqrt(x(1)^2 + x(2)^2);      % 距离atan2(x(2), x(1))           % 角度
];% 生成真实状态和测量
true_states = zeros(4, N+1);
measurements = zeros(2, N+1);true_states(:, 1) = x0;
for k = 1:N% 状态转移（添加过程噪声）process_noise = mvnrnd(zeros(4,1), Q)';true_states(:, k+1) = f(true_states(:, k)) + process_noise;% 生成测量（添加测量噪声）measurement_noise = mvnrnd(zeros(2,1), R)';measurements(:, k+1) = h(true_states(:, k+1)) + measurement_noise;
endfprintf('数据生成完成\n');%% UKF滤波实现
fprintf('开始UKF滤波...\n');% 初始化
x_est = x0;             % 状态估计
P_est = P0;             % 误差协方差估计% 存储估计结果
x_est_history = zeros(4, N+1);
P_est_history = zeros(4, 4, N+1);
x_est_history(:, 1) = x_est;
P_est_history(:, :, 1) = P_est;% 状态维数
n = length(x_est);% 计算UKF参数
lambda = alpha^2 * (n + kappa) - n;% 权重计算
Wm = zeros(2*n+1, 1);   % 均值权重
Wc = zeros(2*n+1, 1);   % 协方差权重Wm(1) = lambda / (n + lambda);
Wc(1) = Wm(1) + (1 - alpha^2 + beta);for i = 2:2*n+1Wm(i) = 1 / (2*(n + lambda));Wc(i) = Wm(i);
end% UKF主循环
for k = 1:N%% 1. 生成Sigma点% 计算矩阵平方根S = chol((n + lambda) * P_est, 'lower');% 生成Sigma点X = zeros(n, 2*n+1);X(:, 1) = x_est;for i = 1:nX(:, i+1) = x_est + S(:, i);X(:, i+n+1) = x_est - S(:, i);end%% 2. 时间更新（预测）% 将Sigma点通过状态转移函数X_pred = zeros(n, 2*n+1);for i = 1:2*n+1X_pred(:, i) = f(X(:, i));end% 计算预测状态和协方差x_pred = zeros(n, 1);for i = 1:2*n+1x_pred = x_pred + Wm(i) * X_pred(:, i);endP_pred = zeros(n, n);for i = 1:2*n+1dx = X_pred(:, i) - x_pred;P_pred = P_pred + Wc(i) * (dx * dx');endP_pred = P_pred + Q;  % 添加过程噪声%% 3. 测量更新% 将预测的Sigma点通过测量函数Z_pred = zeros(2, 2*n+1);for i = 1:2*n+1Z_pred(:, i) = h(X_pred(:, i));end% 计算预测测量值z_pred = zeros(2, 1);for i = 1:2*n+1z_pred = z_pred + Wm(i) * Z_pred(:, i);end% 计算测量协方差和状态-测量交叉协方差P_zz = zeros(2, 2);P_xz = zeros(n, 2);for i = 1:2*n+1dz = Z_pred(:, i) - z_pred;dx = X_pred(:, i) - x_pred;P_zz = P_zz + Wc(i) * (dz * dz');P_xz = P_xz + Wc(i) * (dx * dz');endP_zz = P_zz + R;  % 添加测量噪声%% 4. 计算卡尔曼增益并更新状态估计% 卡尔曼增益K = P_xz / P_zz;% 状态更新x_est = x_pred + K * (measurements(:, k+1) - z_pred);% 协方差更新P_est = P_pred - K * P_zz * K';% 存储估计结果x_est_history(:, k+1) = x_est;P_est_history(:, :, k+1) = P_est;% 显示进度if mod(k, 50) == 0fprintf('  已完成 %.1f%%\n', k/N*100);end
endfprintf('UKF滤波完成\n');%% 结果可视化
fprintf('绘制结果...\n');% 位置估计
figure('Name', 'UKF位置估计', 'Position', [100, 100, 1200, 500]);subplot(1, 2, 1);
plot(true_states(1, :), true_states(2, :), 'b-', 'LineWidth', 2);
hold on;
plot(x_est_history(1, :), x_est_history(2, :), 'r--', 'LineWidth', 2);
scatter(measurements(1, :).*cos(measurements(2, :)), ...measurements(1, :).*sin(measurements(2, :)), ...20, 'g', 'filled', 'MarkerFaceAlpha', 0.3);
xlabel('X位置');
ylabel('Y位置');
title('二维轨迹');
legend('真实轨迹', 'UKF估计', '测量值');
axis equal;
grid on;% 位置误差
pos_error = sqrt((true_states(1, :) - x_est_history(1, :)).^2 + ...(true_states(2, :) - x_est_history(2, :)).^2);subplot(1, 2, 2);
plot(t, pos_error, 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('位置误差');
title('位置估计误差');
grid on;% 速度估计
figure('Name', 'UKF速度估计', 'Position', [100, 100, 1200, 500]);subplot(1, 2, 1);
plot(t, true_states(3, :), 'b-', 'LineWidth', 2);
hold on;
plot(t, x_est_history(3, :), 'r--', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('X方向速度');
title('X方向速度估计');
legend('真实速度', 'UKF估计');
grid on;subplot(1, 2, 2);
plot(t, true_states(4, :), 'b-', 'LineWidth', 2);
hold on;
plot(t, x_est_history(4, :), 'r--', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('Y方向速度');
title('Y方向速度估计');
legend('真实速度', 'UKF估计');
grid on;% 不确定性椭圆可视化
figure('Name', 'UKF不确定性椭圆', 'Position', [100, 100, 800, 800]);plot(true_states(1, :), true_states(2, :), 'b-', 'LineWidth', 2);
hold on;
plot(x_est_history(1, :), x_est_history(2, :), 'r--', 'LineWidth', 2);% 绘制几个时间点的不确定性椭圆
time_points = [1, floor(N/3), floor(2*N/3), N];
colors = ['r', 'g', 'm', 'c'];for i = 1:length(time_points)idx = time_points(i);x = x_est_history(1, idx);y = x_est_history(2, idx);P = P_est_history(1:2, 1:2, idx);% 绘制不确定性椭圆error_ellipse(P, [x, y], 'conf', 0.95, 'color', colors(i), 'linewidth', 1.5);
endxlabel('X位置');
ylabel('Y位置');
title('UKF位置估计与95%不确定性椭圆');
legend('真实轨迹', 'UKF估计', '时间点1', '时间点2', '时间点3', '时间点4');
axis equal;
grid on;%% 性能评估
fprintf('性能评估...\n');% 计算均方根误差(RMSE)
position_rmse = sqrt(mean(pos_error.^2));
velocity_rmse = sqrt(mean((true_states(3:4, :) - x_est_history(3:4, :)).^2, 'all'));fprintf('位置估计RMSE: %.4f\n', position_rmse);
fprintf('速度估计RMSE: %.4f\n', velocity_rmse);% 计算平均不确定性
avg_position_uncertainty = mean(sqrt(P_est_history(1,1,:) + P_est_history(2,2,:)), 'all');
avg_velocity_uncertainty = mean(sqrt(P_est_history(3,3,:) + P_est_history(4,4,:)), 'all');fprintf('平均位置不确定性: %.4f\n', avg_position_uncertainty);
fprintf('平均速度不确定性: %.4f\n', avg_velocity_uncertainty);% 计算归一化估计误差平方(NEES)
nees = zeros(1, N+1);
for i = 1:N+1dx = true_states(:, i) - x_est_history(:, i);nees(i) = dx' / P_est_history(:, :, i) * dx;
end
avg_nees = mean(nees);fprintf('平均NEES: %.4f (理论值: %.4f)\n', avg_nees, n);%% 辅助函数：绘制不确定性椭圆
function h = error_ellipse(P, center, varargin)% 解析输入参数p = inputParser;addRequired(p, 'P', @isnumeric);addRequired(p, 'center', @isnumeric);addParameter(p, 'conf', 0.95, @isnumeric);addParameter(p, 'color', 'r', @ischar);addParameter(p, 'linewidth', 1, @isnumeric);parse(p, P, center, varargin{:});% 获取置信区间对应的卡方值s = -2 * log(1 - p.Results.conf);% 计算特征值和特征向量[V, D] = eig(P * s);% 提取长轴和短轴a = sqrt(D(1,1));b = sqrt(D(2,2));% 计算椭圆角度theta = atan2(V(2,1), V(1,1));% 生成椭圆点t = linspace(0, 2*pi, 100);ellipse = [a * cos(t); b * sin(t)];% 旋转椭圆R = [cos(theta), -sin(theta); sin(theta), cos(theta)];ellipse = R * ellipse;% 平移椭圆到中心点ellipse(1, :) = ellipse(1, :) + center(1);ellipse(2, :) = ellipse(2, :) + center(2);% 绘制椭圆h = plot(ellipse(1, :), ellipse(2, :), ...'Color', p.Results.color, ...'LineWidth', p.Results.linewidth);
endfprintf('UKF演示完成!\n');

参考代码 UKF普通滤波 www.youwenfan.com/contentcnd/100274.html

UKF算法详解

1. Sigma点生成

Sigma点的数量为2n+1，其中n是状态维数。Sigma点的生成公式为：

\[\begin{aligned} \mathcal{X}_0 &= \hat{x} \\ \mathcal{X}_i &= \hat{x} + \sqrt{(n+\lambda)P} \quad \text{for } i=1,...,n \\ \mathcal{X}_i &= \hat{x} - \sqrt{(n+\lambda)P} \quad \text{for } i=n+1,...,2n \end{aligned} \]

其中\(\lambda = \alpha^2(n+\kappa) - n\)是缩放参数。

2. 权重计算

UKF使用两组权重：

均值权重\(W_m\)
协方差权重\(W_c\)

权重计算公式为：

\[\begin{aligned} W_m^{(0)} &= \frac{\lambda}{n+\lambda} \\ W_c^{(0)} &= \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta) \\ W_m^{(i)} &= W_c^{(i)} = \frac{1}{2(n+\lambda)} \quad \text{for } i=1,...,2n \end{aligned} \]

3. UKF与EKF的比较

特性	UKF	EKF
非线性处理	无迹变换	一阶泰勒展开
计算复杂度	中等 (2n+1次函数评估)	低 (1次函数评估)
精度	高 (可处理强非线性)	中等 (对强非线性效果差)
实现难度	中等	简单
需要雅可比矩阵	否	是

实际应用建议

参数调优：
- \(\alpha\)：通常设置为小的正数(1e-3到1)
- \(\beta\)：对于高斯分布，最优值为2
- \(\kappa\)：通常设置为0或3-n
数值稳定性：
- 使用Cholesky分解确保协方差矩阵的正定性
- 可以添加小的正则化项防止协方差矩阵变得非正定
计算效率：
- 对于高维系统，考虑使用简化UKF变体
- 可以使用平方根UKF提高数值稳定性
模型选择：
- 确保状态转移和测量模型准确反映系统特性
- 合理设置过程噪声和测量噪声协方差