%   卡尔曼滤波器
%   X(K) = F*X(K-1) + Q
%   Y(K) = H*X(K) + R

% 生成时间序列
t = 0.1:0.01:1; % 从 0.1 秒开始，间隔 0.01 秒，到 1 秒结束
L = length(t);

% 生成真实信号 x 和观测值 y
x = zeros(1, L);
y = zeros(1, L);

for i = 1:L
  x(i) = t(i)^2;
  y(i) = x(i) + normrnd(0, 0.1); % 添加高斯噪声，标准差为 0.1
end

% 卡尔曼滤波器参数
F_1 = 1;     % 状态转移矩阵
H_1 = 1;     % 观测矩阵
Q_1 = 0.001; % 过程噪声协方差（调小了这个值）
R_1 = 0.1^2; % 测量噪声协方差

% 初始化
X_plus_1 = zeros(1, L);  % 后验状态估计
P_plus_1 = zeros(1, L);  % 后验估计协方差
X_plus_1(1) = 0.01;      % 初始状态估计
P_plus_1(1) = 0.01^2;    % 初始估计协方差

% 卡尔曼滤波算法
for i = 2:L
  % 预测
  X_minus_1 = F_1 * X_plus_1(i-1);
  P_minus_1 = F_1 * P_plus_1(i-1) * F_1' + Q_1;
  
  % 更新
  K_1 = (H_1 * P_minus_1 * H_1' + R_1) \ (P_minus_1 * H_1');
  X_plus_1(i) = X_minus_1 + K_1 * (y(i) - H_1 * X_minus_1);
  P_plus_1(i) = P_minus_1 - K_1 * H_1 * P_minus_1;
end

% 绘图
figure(1);
% 绘制观测值
scatter(t, y, 30, 'r', 'filled', 'DisplayName', '带噪声观测值');
hold on;
plot(t, y, 'r-', 'LineWidth', 1, 'DisplayName', '带噪声观测值（连续）');
% 绘制真实信号
plot(t, x, 'g--', 'LineWidth', 2, 'DisplayName', '真实信号');
% 绘制卡尔曼滤波估计
plot(t, X_plus_1, 'b-', 'LineWidth', 2, 'DisplayName', '卡尔曼滤波估计');

xlabel('时间');
ylabel('值');
title('卡尔曼滤波结果');
legend('Location', 'best');
grid on;

% 计算均方根误差（RMSE）
rmse_original = sqrt(mean((y - x).^2));
rmse_filtered = sqrt(mean((X_plus_1 - x).^2));

fprintf('原始观测的RMSE: %.4f\n', rmse_original);
fprintf('卡尔曼滤波后的RMSE: %.4f\n', rmse_filtered);