MATLAB实现简单卡尔曼滤波——稍微做下笔记（2）

clear;close all;clc
% 卡尔曼滤波

A = 1;  % 状态转移矩阵
B = 0;  % 控制输入矩阵
H = 1;  % 观测矩阵
Q = 0.1;    % 过程噪声协方差矩阵，越大，模型不确定性越大，卡尔曼滤波估计值越偏向于测量值，越小，偏向于先验估计
R = 0.01;   % 观测噪声协方差矩阵，越大，观测值不确定性越大，卡尔曼滤波估计值越偏向于先验估计，越小，偏向于测量值

measurements = [196.32	169.13	141.94	131.88	111.85	103.94	104.72	87.05	79.02	71.12];

% 初始状态估计
x_hat = 0;
P = 1;

x_pred = zeros(size(measurements));
x_est = zeros(size(measurements));

for k = 1:length(measurements)
    % 预测步骤-时间更新
    x_pred(k) = A * x_hat;      % 先验估计值
    P_pred = A * P * A' + Q;    % 先验协方差矩阵
    
    % 更新步骤-测量更新
    K = P_pred * H' / (H * P_pred * H' + R);        % 卡尔曼增益矩阵
    z = measurements(k);
    x_hat = x_pred(k) + K * (z - H * x_pred(k));    % 状态估计值
    P = (eye(size(A)) - K * H) * P_pred;            % 状态方差矩阵
    
    % 估计值
    x_est(k) = x_hat;
end

disp('预测值:');
disp(x_pred);
disp('估计值:');
disp(x_est);
disp('观测值:');
disp(measurements);

plot(x_pred)
hold on
plot(x_est)
plot(measurements)
legend('预测值','估计值','观测值')
title(['Q = ',num2str(Q),'   R = ',num2str(R)])