MATLAB实现UKF（无迹卡尔曼滤波）
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;
end
fprintf('数据生成完成\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+1
Wm(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:n
X(:, 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+1
X_pred(:, i) = f(X(:, i));
end
% 计算预测状态和协方差
x_pred = zeros(n, 1);
for i = 1:2*n+1
x_pred = x_pred + Wm(i) * X_pred(:, i);
end
P_pred = zeros(n, n);
for i = 1:2*n+1
dx = X_pred(:, i) - x_pred;
P_pred = P_pred + Wc(i) * (dx * dx');
end
P_pred = P_pred + Q; % 添加过程噪声
%% 3. 测量更新
% 将预测的Sigma点通过测量函数
Z_pred = zeros(2, 2*n+1);
for i = 1:2*n+1
Z_pred(:, i) = h(X_pred(:, i));
end
% 计算预测测量值
z_pred = zeros(2, 1);
for i = 1:2*n+1
z_pred = z_pred + Wm(i) * Z_pred(:, i);
end
% 计算测量协方差和状态-测量交叉协方差
P_zz = zeros(2, 2);
P_xz = zeros(n, 2);
for i = 1:2*n+1
dz = 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');
end
P_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) == 0
fprintf(' 已完成 %.1f%%\n', k/N*100);
end
end
fprintf('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);
end
xlabel('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+1
dx = 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);
end
fprintf('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提高数值稳定性
-
模型选择：
- 确保状态转移和测量模型准确反映系统特性
- 合理设置过程噪声和测量噪声协方差
扩展应用
UKF可以应用于各种非线性估计问题，包括：
- 目标跟踪
- 导航系统
- 机器人定位
- 金融时间序列分析
- 生物系统建模
浙公网安备 33010602011771号