clear all;

% 给定矩阵值
A = [1 1; 0 1];
B = [1 0; 0 1];
C = [1 0; 0 1];
Q = [0.1 0; 0 0.1];
R = [1 0; 0 1];
H = [1 0; 0 1];
I = eye(2);

% 定义变量数量
nx = 2;
nz = 2;
N = 30;

% 初始化变量
x = zeros(nx, N+1);     % 真实状态
z = zeros(nz, N+1);     % 测量值
hatx = zeros(nx, N+1);  % 后验估计
hatxx = zeros(nx, N+1); % 先验估计
w = zeros(nx, N+1);     % 过程噪声
v = zeros(nz, N+1);     % 测量噪声
K = zeros(nx, nz, N+1); % 卡尔曼增益
P_ = zeros(nx, nx, N+1);% 先验误差协方差
P = zeros(nx, nx, N+1); % 后验误差协方差

% 生成过程噪声和测量噪声
for i = 1:N+1
    w(:,i) = mvnrnd([0;0], Q)';
    v(:,i) = mvnrnd([0;0], R)';
end

% 初始值
x0 = [0; 1];
hatx(:,1) = [0; 1];
P(:,:,1) = [1 0; 0 1];
x(:,1) = x0;
z(:,1) = [0; 0];
hatxx(:,1) = [0; 0];

% 卡尔曼滤波器循环
for i = 1:N
    % 真实状态更新
    x(:,i+1) = A*x(:,i) + B*w(:,i);
    
    % 测量更新
    z(:,i+1) = H*x(:,i+1) + C*v(:,i+1);
    
    % 预测步骤
    hatxx(:,i+1) = A*hatx(:,i);
    P_(:,:,i+1) = A*P(:,:,i)*A' + Q;
    
    % 更新步骤
    K(:,:,i+1) = P_(:,:,i+1)*H'/(H*P_(:,:,i+1)*H' + R);
    hatx(:,i+1) = hatxx(:,i+1) + K(:,:,i+1)*(z(:,i+1) - H*hatxx(:,i+1));
    P(:,:,i+1) = (I - K(:,:,i+1)*H)*P_(:,:,i+1);
end

% 绘图
figure(1);
plot(x(1,:), 'r', 'LineWidth', 1.5);
hold on;
plot(z(1,:), 'b', 'LineWidth', 1.5);
plot(hatx(1,:), 'g', 'LineWidth', 1.5);
plot(hatxx(1,:), 'k', 'LineWidth', 1.5);
legend('实际位置', '测量位置', '后验估计位置', '先验估计位置', 'Location', 'Best', 'FontSize', 12);
grid on;
xlabel('步', 'FontSize', 16);
ylabel('位置', 'FontSize', 16);
title('位置比较', 'FontSize', 16);
set(gca, 'FontSize', 16);

figure(2);
plot(x(2,:), 'r', 'LineWidth', 1.5);
hold on;
plot(z(2,:), 'b', 'LineWidth', 1.5);
plot(hatx(2,:), 'g', 'LineWidth', 1.5);
plot(hatxx(2,:), 'k', 'LineWidth', 1.5);
legend('实际速度', '测量速度', '后验估计速度', '先验估计速度', 'Location', 'Best', 'FontSize', 12);
grid on;
xlabel('步', 'FontSize', 16);
ylabel('速度', 'FontSize', 16);
title('速度比较', 'FontSize', 16);
set(gca, 'FontSize', 16);