算法（2）——卡尔曼滤波：1.线性卡尔曼滤波

%% ========== Kalman滤波用在一维温度数据测量系统中 ========== %%
function Linear_Kalman
%% ========== 初始数据 ========== %%
N = 120; % 采样点的个数
CON = 25; % 室内温度的理论值


%% ========== 对状态和测量初始化 ========== %%
Xexpect = CON * ones(1,N); % 设置期望的温度
X = zeros(1,N); % 房间各时刻真实温度值
Xkf = zeros(1,N); % Kalman滤波处理的状态，也叫估计值
Z = zeros(1,N); % 温度测量值
P = zeros(1,N); % 预设协方差空间


%% ========== 赋初值 ========== %%
X(1) = 25.1; % 假设房间温度初始值为25.1摄氏度
P(1) = 0.01; % 初始值的协方差
Z(1) = 24.9; % 房间初始测量值
Xkf(1) = Z(1); % 将初始测量值作为滤波器的初始估计状态


%% ========== 噪声 ========== %%
Q = 0.01; % 输入白噪声的方差阵
R = 0.25; % 观测白噪声的方差阵
W = sqrt(Q) * randn(1,N); % 由方差决定的输入白噪声的大小
V = sqrt(R) * randn(1,N); % 由方差决定观测白噪声的大小


%% ========== 系统矩阵 ========== %%
F = 1; % 状态转移矩阵
G = 1; % 噪声驱动矩阵
H = 1; % 观测矩阵
I = eye(1); % 本系统状态为一维


%% ========== 模拟房间温度和测量过程，并滤波 ========== %%
for k = 2:N
    % 第一步：随时间推移，房间真实温度的变化
    X(k) = F * X(k - 1) + G * W(k - 1);
    % k时刻房间的真实温度，温度计不知道，但却真实存在，这是计算机模拟过程
    
    % 第二步：随时间推移，获得实时数据
    Z(k) = H * X(k) + V(k);
    % 温度计对k时刻房间温度的测量，Kalman滤波站在温度计的角度进行
    % 它不知道真实状态X（k），只能利用本次测量值Z（k）和上一次估计值
    % 来逼近X（k），这就是Kalman滤波的目的所在
    
    % 第三步：Kalman滤波
    X_pre = F * Xkf(k - 1); % 状态预测
    P_pre = F * P(k - 1) * F' + Q; % 协方差预测
    Kg = P_pre * inv(H * P_pre * H' + R); % 计算Kalman增益
    % ********** inv ********** %
    % inv(A),求矩阵的逆
    e = Z(k) - H * X_pre; % 新息，即测量与预测*观测矩阵后的误差
    Xkf(k) = X_pre + Kg * e; % Kalman滤波估计状态更新
    P(k) = (I - Kg * H) * P_pre; % 协方差更新
end


%% ========== 计算误差 ========== %%
Err_Messure = zeros(1,N); % 测量值与真实值之间的误差
Err_Kalman = zeros(1,N); % Kalman估计值与真实值之间的误差
for k = 1:N
    Err_Messure(k) = abs(Z(k) - X(k));
    Err_Kalman(k) = abs(Xkf(k) - X(k));
end


%% ========== 绘制图形 ========== %%
t = 1:N;
figure; % 画图
% 依次输入理论值，叠加过程噪声（受波动影响）的真实值
% 温度计测量值，Kalman估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','Kalman滤波值');
xlabel('采样时间/s');
ylabel('温度值/oC');
% 误差分析图
figure;
plot(t, Err_Messure,'-b',t, Err_Kalman,'-k*');
legend('测量偏差','Kalman滤波偏差');
xlabel('采样时间/s');
ylabel('温度值/oC');
end

参考书籍“卡尔曼滤波原理及应用”

后续更新。

来源：https://blog.csdn.net/Mr_ZZTC/article/details/101348970