Kalman滤波MATLAB实现实例——在温度测量中的应用

参考：《卡尔曼滤波原理及应用MATLAB仿真》

原理介绍

假设我们要研究的对象是一个房间的温度。根据经验判断,这个房间的温度大概在25℃左右,可能受空气流通、阳光等因素影响,房间内温度会小幅度地波动。我们以分钟为单位,定时测量房间温度,这里的1分钟,可以理解为采样时间。假设测量温度时,外界的天气是多云,阳光照射时有时无,同时房间不是100%密封的,可能有微小的与外界空气的交换,即引入过程噪声W(k),其方差为Q,大小假定为Q=0.01(假如不考虑过程噪声的影响,即真实温度是恒定的,那么这时候Q=0)。相应地,A=1,F=1,Q=0.01,状态X(k)是在第k分钟时的房间温度,是一维的。那么该系统的状态方程可以写为
X(k)=X(k-1)+W(k)
现在用温度计开始测量房间的温度,假设温度计的测量误差为±0.5℃,从出厂说明书上我们得知该温度计的方差为0.25。也就是说,温度计第k次测量的数据不是100%准确的,它是有测量噪声V(k)的,并且其方差R=0.25,因此测量方程为Z(k)=Xk)+V(k)。
该系统的状态和观测方程为
X(k)=AX(k-1)+W(k-1)
Z(k)=HX(k)+V(k)
式中,X(k)是一维变量温度;A=1;F=1;H=1;W(k)和V(k)的方差为Q和R。
模型建好以后,就可以利用 Kalman滤波了。假如要估算第k时刻的实际温度值,首先要根据第k-1时刻的温度值来预测k时刻的温度。
(1)假定第k-1时刻的温度值测量值为23.9℃,房间真实温度为24.0℃,该
测量值的偏差是0.1℃,即协方差P(k-1)=0.1^2。
(2)在第k时刻,房间的真实温度是24.1℃,温度计在该时刻测量的值为
24.5℃,偏差为0.4℃。我们用于估算第k时刻的温度有两个温度值,分别是k-1时刻23.9℃和k时刻的24.5℃,如何融合这两组数据,得到最逼近真实值的估计?
首先,利用k1时刻温度值预测第k时刻的温度,其预计偏差为P(k|k-1)=P(k-1)+Q=0.02,计算 Kalman增益
K=P(k|k-1)/(P(k|k-1)+R)=00741,那么这时候利用k时刻的观测值,得到温度的估计值为X(k)=23.9+0.0741×(24.1-23.9)=23.915℃。可见,与23.9℃和24.5℃相比较, Kalman估计值23.915℃更接近真实值24.1℃。此时更新k时刻的偏差P(k)=(1-K*H)P(k|k-1)=0.0186。最后由X(k)=23.915℃和P(k)=0.0186,可以继续对下一时刻观测数据Z(k+1)进行更新和处理。
(3)这样, Kalman滤波器就不断地把方差递归，从而估算出最优的温度值。X(0)和P(0)分别为滤波器初始值。

MATLAB仿真程序

% 程序说明：Kalman滤波用于一维温度测量的实例
function Kalman_main
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=120;%采样点个数，时间单位为分钟
CON=25;%室内温度理论值，房间温度在25摄氏度左右

%对状态和测量初始化
Xexpect=CON*ones(1,N);%期望温度是25摄氏度，但会收到噪声影响
X=zeros(1,N);  %房间各时刻真实温度值
Xkf=zeros(1,N); %估计值
Z=zeros(1,N);  %温度计测量值
P=zeros(1,N); 

%初始化
X(1)=25.1;
P(1)=0.01;%初始化协方差
Z(1)=24.9;
Xkf(1)=Z(1);%初始化测量值24.9，可作为滤波器的初始估计状态

%噪声
Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
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);  %状态方程
    %第二步：随时间推移，获取实时数据
    Z(k)=H*X(k)+V(k); %观测方程
    %第三步：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增益
    e=Z(k)-H*X_pre;      %新息      
    Xkf(k)=X_pre+Kg*e;   %状态更新 
    P(k)=(I-Kg*H)*P_pre; %协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算误差
Err_Messure=zeros(1,N);%量测值与真实值的误差
Err_Kalman=zeros(1,N);%估计与真实值的偏差
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('Name','Kalman Filter Simulation','NumberTitle','off');
%一次画出理论值、真实值、测量值、估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
%误差分析
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');         
xlabel('sample time');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

仿真图

结论

从仿真结果可以看出，kalman滤波与温度计测量的值相比，大大降低了偏差，使得状态尽可能逼近真实值。