一维、线性卡尔曼滤波的例程（MATLAB）

这段 MATLAB 代码实现了一维线性卡尔曼滤波器的基本功能，用于估计在存在噪声的情况下目标状态的真实值

一维线性卡尔曼滤波

代码运行

状态量对比：

状态误差对比：

代码介绍

1. 初始化部分

• 清空工作区及命令行：使用 clear、clc 和 close all 清理环境。
• 随机数种子：通过 rng(0) 设置固定的随机数种子，以确保结果可重复。
• 参数设置：
  • T：采样率，设置为1。
  • t：构建时间序列，范围为1到100。
  • Q 和 R：分别定义系统噪声和观测噪声的方差。
  • P：初始状态协方差。

2. 数据生成

• 使用循环生成真实状态 X、未滤波的状态 X_ 和观测值 Z。在每次迭代中，真实值 X 按固定增量递增，未滤波状态 X_ 加上随机噪声生成。

3. 卡尔曼滤波器实现

• 在 EKF（扩展卡尔曼滤波）部分，通过循环更新状态的预测和协方差：
  • Xpre：根据上一个滤波值预测当前状态。
  • Z_hat：通过预测的状态生成对应的观测。
  • 计算增益 Kk，并更新当前的滤波状态 X_kf 和状态协方差 P。

4. 结果可视化

• 绘制真实值、滤波后值、观测值和未滤波值的比较图。
• 计算并绘制滤波前后状态估计的绝对误差对比图。
• 绘制滤波后误差的累计概率密度函数（CDF）图。

5. 统计输出

• 计算并输出滤波前后的误差最大值、平均值和标准差，帮助评估滤波效果。

源代码

% 一维线性卡尔曼滤波
% 2024-12-25/Ver1

clear; %清空工作区变量
clc; %清空命令行内容
close all; %关闭所有窗口（主窗口除外）
rng(0); % 设置固定的随机数种子
%% 初始化
T = 1; %设置采样率
t = T:T:100; %构建时间序列，最后的10是序列总长度
Q = 1;w=sqrt(Q)*randn(size(Q,1),length(t)); %系统噪声
R =9;v=sqrt(R)*randn(size(R,1),length(t)); %观测噪声
P = 1; %状态协方差
P_num = zeros(length(t),size(P,1),size(P,2)); %存放每次迭代的P
P_num(1,:,:) = P; %记录协方差
X=zeros(1,length(t)); %给状态真实值X分配空间
X_=zeros(1,length(t)); %给滤波前的状态分配空间
Z=zeros(1,length(t)); %给观测量序列分配空间
X_kf=zeros(1,length(t)); %准备储存滤波后的值
X(1) = 3; %给状态值初值赋值
X_(1) = X(1) + w(1); %X_是未滤波的状态，由真实值加上误差生成，这里生成其初值
%% 运动模型建立
for i1 = 2:length(t) %生成数据的for循环
    X(i1) = X(i1-1)+1;  %迭代生成真实值
    X_(i1) = X_(i1-1)+1 + w(i1); %迭代生成滤波前的状态
    Z(i1) = X(i1) + v(i1); %迭代生成观测量
end
X_kf(1) = X(1); %用第一时刻的观测值代替第一时刻的滤波值
%% KF迭代
for k = 2 : length(t) %生成数据的for循环，循环次数为时间序列-1
    Xpre = X_kf(k-1)+1+w(k); %预测下一时刻的X
    Z_hat = Xpre; %预测下一时刻的X对应的观测
    F = 1; %状态转移矩阵
    H = 1; %观测矩阵
    PP=F*P*F'+Q; %更新状态协方差
    Kk=PP*H'/(H*PP*H'+R); %计算增益
    X_kf(:,k)=Xpre+Kk*(Z(k)-Z_hat); %状态预测，此状态为滤波输出状态
    P=PP-Kk*H*PP; %更新状态协方差（留作下一时刻使用）
    P_num(k,:,:) = P; %储存状态协方差矩阵
end

%% 结果展示
figure; %新建绘图窗口
plot(t,X,t,X_kf,t,Z,t,X_); %绘制状态的真实值、EKF滤波后的值、观测值对比图
title('状态对比'); %标注图像的标题
legend('理论值','KF滤波后的值','观测值','未滤波的值'); %标注图例

figure; %新建绘图窗口
hold on
plot(abs(X_kf-X),'DisplayName','滤波后');
plot(abs(X_-X),'DisplayName','滤波前'); %绘制误差对比图
plot(abs(Z-X),'DisplayName','观测值');
title('状态估计绝对误差对比'); %标注图像的标题
legend; %标注图例


figure; %新建绘图窗口
cdfplot(abs(X_kf-X)); %绘制滤波后误差的CDF图像
hold on %后续绘制的图像覆盖在前面的图像上
cdfplot(abs(X_-X)); %绘制滤波前误差的CDF图像
cdfplot(abs(Z-X)); %绘制观测误差的CDF图像
legend('KF','滤波前','观测'); %标注图例
title('累计概率密度函数'); %标注图像的标题

fprintf('滤波前的误差最大值:%f\n',max(X_-X)); %计算滤波前误差最大值
fprintf('滤波后的误差最大值:%f\n',max(X_kf-X)); %计算滤波后误差最大值
fprintf('观测误差最大值:%f\n\n',max(Z-X)); %计算观测误差最大值

fprintf('滤波前的误差平均值:%f\n',mean(X_-X)); %计算滤波前误差平均值
fprintf('滤波后的误差平均值:%f\n',mean(X_kf-X)); %计算滤波后误差平均值
fprintf('观测误差平均值:%f\n\n',mean(Z-X)); %计算滤波后误差平均值

fprintf('滤波前的误差标准差:%f\n',std(X_-X)); %计算滤波前误差标准差
fprintf('滤波后的误差标准差:%f\n',std(X_kf-X)); %计算滤波前误差标准差
fprintf('观测误差标准差:%f\n\n',std(Z-X)); %计算观测误差标准差

总结

这段代码展示了线性卡尔曼滤波在一维状态估计中的应用，适用于需要在噪声环境中进行可靠状态估计的场景。通过可视化结果，用户可以直观地观察到滤波的效果和性能。