% 清空工作区和命令窗口
clear; clc;
close all;
% 读取CSV文件（请替换为实际文件路径）
full_data = readmatrix('ukf1.csv');
time = full_data(:, 1);
speed = full_data(:, 3);
% 确保时间序列严格递增
[time, sort_idx] = sort(time);
speed = speed(sort_idx);
% 使用前150个点进行最小二乘正弦拟合（UKF初始化）
num_init_points = 150;
t_init = time(1:num_init_points);
y_init = speed(1:num_init_points); % 使用原始数据初始化
% 初始猜测参数：[A, omega, phi]
p0 = [ (0.780+1.045)/2; (1.884+2)/2; 0.0 ];
% 定义拟合函数
fit_func = @(p, t) p(1) * sin(p(2) * t + p(3)) + 2.09 - p(1);
% 设置优化选项
options = optimoptions('lsqcurvefit', ...
'Display', 'off', ...
'MaxFunctionEvaluations', 1000, ...
'MaxIterations', 1000, ...
'TolFun', 1e-8, ...
'TolX', 1e-8);
lb = [0.68, 1.784, -pi]; % 下限
ub = [1.145, 2.1, pi]; % 上限
% 执行最小二乘拟合
p_fit = lsqcurvefit(fit_func, p0, t_init, y_init, lb, ub, options);
% 提取拟合参数
A_fit = p_fit(1);
omega_fit = p_fit(2);
phi_fit = p_fit(3);
v0_fit = 2.09 - p_fit(1);
% 转换为 sin(phi) 和 cos(phi)
s_phi_fit = sin(phi_fit);
c_phi_fit = cos(phi_fit);
% 更新初始状态 x0
x0 = [ (0.780+1.045)/2; (1.884+2)/2; phi_fit; ];
% 显示结果
disp('使用最小二乘法拟合的初始参数：');
disp(['A = ', num2str(A_fit)]);
disp(['omega = ', num2str(omega_fit)]);
disp(['phi = ', num2str(phi_fit)]);
disp(['sin(phi) = ', num2str(s_phi_fit)]);
disp(['cos(phi) = ', num2str(c_phi_fit)]);
disp(['v0 = ', num2str(v0_fit)]);
% 初始状态协方差
P0 = 1e-3 * diag([0.001, 0.001, 0.001]);
% 初始过程噪声协方差 Q
Q = 1e-10 * diag([1, 1, 1]);
% 初始观测噪声协方差 R
R = 1;
% UKF 参数
alpha = 1e-2;
beta = 2;
kappa = 0;
% 创建 UKF 对象
ukf = unscentedKalmanFilter(...
'StateTransitionFcn', @stateTransitionFcn, ...
'MeasurementFcn', @measurementFcn, ...
'State', x0, ...
'StateCovariance', P0, ...
'ProcessNoise', Q, ...
'MeasurementNoise', R, ...
'HasAdditiveMeasurementNoise', true, ...
'Alpha', alpha, ...
'Beta', beta, ...
'Kappa', kappa);
% 初始化变量
innovations = [];
R_estimates = [];
Q_estimates = [];
estimated_params = [];
fit_speed = [];
window_size = 10;
% 更新UKF
for i = 1:length(time)
% 获取当前时间点
current_time = time(i);
% 预测
[x_pred, P_pred] = predict(ukf);
% 计算新息（残差）
y_measured = speed(i);
y_pred = measurementFcn(x_pred, current_time);
y_innovation = y_measured - y_pred;
% 更新观测噪声 R（滑动窗口估计）
innovations = [innovations; y_innovation];
if length(innovations) > window_size
innovations(1) = []; % 移除旧值
end
current_R = max(var(innovations)*1e-1, 1e-8); % 保证不为0
if length(innovations) < window_size
current_R = 0.1;
end
ukf.MeasurementNoise = current_R;
R_estimates = [R_estimates; current_R];
% 校正
ukf.State = correct(ukf, y_measured, current_time);
% 存储结果
estimated_params(i, :) = ukf.State';
fit_speed(i) = measurementFcn(ukf.State, current_time);
end
%% 使用UKF滤波后的数据进行最小二乘拟合（每隔50个点拟合一次）
% 定义拟合函数
fit_func = @(p, t) p(1) * sin(p(2)*t + p(3)) + 2.09 - p(1);
% 初始猜测参数
p0_new = [ (0.780+1.045)/2; (1.884+2)/2; 0 ];
lb = [0.68, 1.784, -pi];
ub = [1.145, 2.1, pi];
options = optimoptions('lsqcurvefit', ...
'Display', 'off', ...
'MaxFunctionEvaluations', 1000, ...
'MaxIterations', 1000, ...
'TolFun', 1e-8, ...
'TolX', 1e-8);
% 初始化存储参数
num_fits = 5;
window_size = 300;
fit_params = zeros(num_fits, 3);
fit_times = zeros(num_fits, 1);
for i = 1:num_fits
start_idx = (i-1)*window_size + 1;
end_idx = i*window_size;
if end_idx > length(time)
end_idx = length(time);
end
t_fit = time(start_idx:end_idx);
y_fit = fit_speed(start_idx:end_idx)'; % UKF滤波后的数据
y_raw = speed(start_idx:end_idx); % 原始数据
% 执行拟合（可放在绘图前或后）
p_fit = lsqcurvefit(fit_func, p0_new, t_fit, y_fit, lb, ub, options);
% 更新初始猜测参数
p0_new = p_fit;
% 保存拟合参数
fit_params(i, :) = p_fit';
fit_times(i) = mean(t_fit);
% 绘图：原始 vs UKF滤波
figure; % 新建一个图
plot(t_fit, y_raw, 'b.', 'DisplayName', '原始数据');
hold on;
plot(t_fit, y_fit, 'r-', 'LineWidth', 1.5, 'DisplayName', 'UKF滤波数据');
title(sprintf('第 %d 次拟合使用的数据（点 %d 到 %d）', i, start_idx, end_idx));
xlabel('时间 (s)');
ylabel('速度 (rad/s)');
legend('Location', 'best');
grid on;
hold off;
end
% 提取参数
A_fit = fit_params(:, 1);
omega_fit = fit_params(:, 2);
phi_fit = fit_params(:, 3);
%% 绘图：参数变化
figure;
subplot(3, 1, 1);
plot(fit_times, A_fit, 'o-');
title('振幅 A 的变化（UKF后拟合）');
xlabel('时间 (s)');
ylabel('A');
subplot(3, 1, 2);
plot(fit_times, omega_fit, 'o-');
title('角频率 \omega 的变化（UKF后拟合）');
xlabel('时间 (s)');
ylabel('\omega');
subplot(3, 1, 3);
plot(fit_times, phi_fit, 'o-');
title('相位 \phi 的变化（UKF后拟合）');
xlabel('时间 (s)');
ylabel('\phi');
sgtitle('基于UKF滤波后数据的最小二乘拟合参数变化');
%% 显示每次拟合结果
disp('每次拟合的参数结果：');
for i = 1:num_fits
fprintf('第 %d 次拟合：A=%.4f, omega=%.4f, phi=%.4f\n', i, fit_params(i, :));
end
%% 函数定义区域
% 状态转移函数（恒等函数）
function x = stateTransitionFcn(x)
x = x(:); % 强制为列向量
end
% 测量函数
function y = measurementFcn(x, t)
A = x(1);
omega = x(2);
phi = x(3);
v0 = 2.09 - A;
y = A * sin(omega*t + phi) + v0;
end
请在原图完整的原始数据中展现各自使用的ukf数据段
本文介绍Xapian和Omega两个组件如何合作提供索引和搜索功能。Xapian负责存储信息及搜索，而Omega则将不同格式的数据转换为Xapian可索引的格式。文中还演示了如何使用Omega建立索引数据库并查询。
扫一扫
1490
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
