博客

基本原理

教材<现代数字信号处理及其运用> P244-P258

卡尔曼滤波过程

源代码

%---卡尔曼滤波器用在一维温度数据测量中
% 卡尔曼滤波要回答的问题：如何利用系统的观测向量进行滤波处理，得到系统状态向量的估计
clc;
clear all;
close all;
%-------------参数设置
N=200;    % 采样点数
T0=25;    % 室内温度的期望值
t=1:N;    % 持续时间
I=eye(1); % 本系统的状态为一维

%-------------对状态和测量初始化
X_expect=T0*ones(1,N);  % 在观察的时间内，期望的温度是恒定不变的25度
X=zeros(1,N);           % 房间各个时刻温度的真实值
X_est=zeros(1,N);       % 卡尔曼滤波处理的状态，也叫做状态的估计值
X_pre=zeros(1,N);       % 状态的预测值
Z=zeros(1,N);           % 温度计观测（测量）值
P=zeros(1,N);           % 预测误差自相关矩阵

%------------给定初值
X(1)=25.1;            % 假设房间的初始温度值为25.1度  相当于教材的X（0）
P(1)=0.01;            % 预测误差自相关矩阵的初始值    相当于教材的P（0）

Z(1)=24.9;            % 温度计观测的初始值
X_est(1)=Z(1);        % 温度计观测的初始值为24.9度，以此作为滤波器的初始估计状态

Q1=0.01;                % 系统状态噪声的方差
Q2=0.25;                % 观测噪声的方差
v1=sqrt(Q1)*randn(1,N); % 系统状态噪声
v2=sqrt(Q2)*randn(1,N); % 观测噪声

%-----状态方程和观测方程的有关参量，全给1简化问题
F=1;                  % 状态转移矩阵
T=1;                  % 状态噪声输入矩阵
C=1;                  % 观测矩阵

%-------模拟房间温度，并开始卡尔曼滤波过程（根据教材现代数字信号处理及其运用 P255编写）
for m=2:N
    % 第一步：随着时间的推移，房间的真实温度波动变化
    % m时刻房间的真实温度，for 温度计，这个真实值它是不知道的，但是它又是客观存在的
    X(m)=F*X(m-1)+T*v1(m-1);   % 状态方程
    
    % 第二步：随着时间的推移，获取实时数据
    % 温度计对m时刻房间温度的观测（测量），卡尔曼滤波是站在温度计的角度进行的，
    % 它不知道此时的真实状态X_ture(m)，只能利用本次的测量值Z（m）和上一次的估计值X_guji（m-1）来做处理
    % 其目标是最大限度的降低观测噪声的影响，尽可能的逼近X_ture(m)，这也是卡尔曼滤波的目的
    Z(m)=C*X(m)+v2(m);         % 观测方程
    
    % 第三步：卡尔曼滤波
    X_pre=F*X_est(m-1);         % 下一时刻状态的预测值
    P_pre=F*P(m-1)*F'+T*Q1*T';  % 预测状态误差自相关矩阵
      
    a=Z(m)-C*X_pre;             % 新息
    A=C*P_pre*C'+Q2;            % 新息过程的自相关矩阵
    K=P_pre*C'*inv(A);          % 计算卡尔曼增益   inv(X)表示计算方阵X的逆矩阵
    
    X_est(m)=X_pre+K*a;         % 计算下一时刻状态估计值（状态更新）
%     P(m)=(1-K*C)*P_pre;       % 计算估计误差自相关矩阵的更新（协方差更新）(7.3.28)
    P(m)=(1-K*C)*P_pre*(1-K*C)'+K*Q2*K'; % 计算估计误差自相关矩阵的更新（协方差更新）
end

%-----------------误差计算
Err_messure=zeros(1,N);         % 测量值与真实值之间的偏差
Err_kalman=zeros(1,N);          % 卡尔曼估计值和真实值之间的误差
for n=1:N
    Err_messure(n)=abs(Z(n)-X(n));
    Err_kalman(n)=abs(X_est(n)-X(n));
end


%------------------绘图
figure(1)
plot(t,X_expect,'-b',t,X,'-r',t,Z,'-g*',t,X_est,'-ko');
legend('期望值','真实值','观测值','kalman滤波值');
xlabel('采样时间/s');
ylabel('温度值/度');

figure(2)
plot(t,Err_messure,'-b',t,Err_kalman,'-r*');
legend('测量偏差','kalman滤波偏差');
xlabel('采样时间/s');
ylabel('温度偏差值/度');
    

“

仿真结果