Mat_Learn
/
kalman.m

%   kalman filter
%   X(K)=F*X(K-1)+Q
%   Y(K)=H*(K)+R
%   第一个问题，生成一段随机信号，并滤波

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

%   生成真实信号 x，以及观测 y
%   首先初始化
x = zeros(1, L); % 一行 L 列的数组，每个值都是 0
y = x;
y_2 = x;

%   生成信号，设 x = t^2
for i = 1:L
  x(i) = t(i)^2;
  y(i) = x(i) + normrnd(0, 1); % 正态分布生成函数，参数为期望和标准差
  y_2(i) = x(i) + normrnd(0, 1); % 正态分布生成函数，参数为期望和标准差
end
%%%%%%%%%%%%%信号生成完毕%%%%%%%%%%%%%
% plot(t, x, t, y, 'LineWidth', 2);

%%%%%%%%%%%%%滤波算法%%%%%%%%%%%%%
%%%%%%预测方程观测方程怎么写？%%%%%%

%   观测方程好写 Y(K) = X(K) + R R~N(0, 1)
%   预测方程不好写
%   在这里，可以猜一猜线性表现是怎样的
%   大多数实际问题中，信号是杂乱无章的，怎么办？
%%%%模型一，最粗糙的建模
%   X(K) = X(K-1) + Q;
%   Y(K) = X(K) + R;
%   Q ~ N(0, 1);

F_1 = 1;
H_1 = 1;
Q_1 = 1; % 预测噪声方差，这里给的小意味着对初值比较自信
R_1 = 1; % 观测噪声方差，这里给的大意味着对观测值比较自信


%   初始化 X(K)+
X_plus_1 = zeros(1, L); % plus: + ; minus: - ;

%   我们会经常用到 X_plus, X_minus, P_plus, P_minus;

%   设置一个初值，设置 X_plus_1(1) ~ N(0.01, 0.01^2); 这里方差给的小意味着对初值比较自信
% X_plus_1(1) = 0.01;
X_plus_1(1) = 1;

P_plus_1 = 0.01^2;
%   卡尔曼滤波算法
%   X(K)_minus = F*X(K-1)_plus;
%   P(K)_minus = F*P(K-1)_plus*F' + Q;
%   K = P(K)_minus*H' * inv(H*P(K)_minus*H' + R);
%   X(K)_plus = X(K)_minus + K*(y(K) - H*X(K)_minus);
%   P(K)_plus = P(K)_minus - K*H*P(K)_minus;

% 卡尔曼滤波算法
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


%%%%模型二
%   X(K) = X(K-1) + X'(K-1)*dt + X"(K-1)*dt^2*(1/2!) + Q_2;
%   Y(K) = X(K) + R; R~N(0, 1);
%   此时状态变量 X(K) = [X(K) X'(K) X"(K)]T(列向量)
%   Y(K) = H*X(K) + R; H~N(0, 1);    H = [1 0 0](行向量);
%
%   预测方程
%
%   X(K)  = X(K-1)     +  X'(K-1)*dt  +  X"(K-1)*dt^2*(1/2!) + Q_2;
%   X'(K) = 0 * X(K-1) +  X'(K-1)     +  X"(K-1)*dt + Q_3;
%   X"(K) = 0 * X(K-1) +  0 * X'(K-1) +  X"(K-1)*dt + Q_3;
%
%   多项式信号多求几阶导数，总会比较平缓，
%   X"(K) = X"(K-1)*dt + Q_3; 正是描述平缓的随机过程，这种建模相对来说更精细一些，适用范围也比较广
%
%   F = 1   dt  0.5*dt^2
%       0   1   dt
%       0   0   1
%
%   H = 1   0   0;
%
%   Q = Q_2   0   0
%       0   Q_3   0
%       0   0   Q_3
%
dt = t(2) - t(1);
F_2 = [1 dt 0.5*dt^2; 0 1 dt; 0 0 1]; % 此处要注意矩阵是否病态，若 dt 特别小，精度会丢失
H_2 = [1 0 0];
Q_2 = [1,   0,   0;
       0, 0.1,   0;
       0,   0, 0.01];

R2 = 20;

%   设置初值
X_plus_2 = zeros(3, L);
X_plus_2(:, 1) = [0.01; 0.01; 0.01]; % 初值
P_plus_2 = eye(3); % eye(3) 是 3*3 的单位矩阵

%   开始滤波
for i = 2:L
  % 预测
  X_minus_2 = F_2 * X_plus_2(:, i-1);
  P_minus_2 = F_2 * P_plus_2 * F_2' + Q_2;
  % 增益
  K_2 = (H_2 * P_minus_2 * H_2' + R2) \ (P_minus_2 * H_2');
  % 更新
  X_plus_2(:, i) = X_minus_2 + K_2 * (y(i) - H_2 * X_minus_2);
  P_plus_2 = (eye(3) - K_2 * H_2) * P_minus_2;
end

%%% 优点，可以在线进行滤波，可以在线进行参数调整 %%%
%%% 缺点，需要对信号进行建模，模型不好，滤波效果不好 %%%

%   问题2，两个传感器，进行滤波
%   Y_K(1) = X(K) + R_1;
%   Y_K(2) = X(K) + R_2;
%   假设是模型 1
%   H = [1 1]T (列向量) X = X(K)
%   假设是模型 2
%   H = 1 0 0   X = X(K)    X'(K)    X"(K)
%       1 0 0
%   R = [R_1 0; 0 R_2];
%   Q 和 R 一定是方阵，


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 绘图
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', '卡尔曼滤波估计_1');
plot(t, X_plus_2(1,:), 'p-', 'LineWidth', 2, 'DisplayName', '卡尔曼滤波估计_2');


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);