卡尔曼滤波在温度测量中的应用

假设我们要研究的对象是一个房间的温度。
根据经验判断，这个房间的温度大概是25℃左右，可能受空气流通、阳关等因素影响，房间内温度会小幅度地波动。
我们以分钟为单位，定时测量房间温度，这里的1分钟，可以理解为采样时间。假设测量温度时，外界的天气是多云，阳光照射时有时无，同时房间不是100%密封的，可能有微小的与外界空气的交换，即引入过程噪声W(k),其方差为Q,大小恒定为Q=0.01(假如不考虑过程噪声的影响，即真实温度是恒定的，那么这时候Q=0)。

 由公式：
 X(k+1) = ΦX*(k) + Γ*W(k)
 Y(k) = H*X(k)+V(k)

相应地，A=1, Γ=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）=X(k)+V(k)

到此，很容易想到，该系统的状态和观测方程为

            X（k）=ΦX(k-1)+ΓW(k)
            Y(k) = H*X(k)+V(k)

式中，X(k)是一维变量温度；A=1；Γ=1，H=1；W(k)和V(k)的方差为Q和R。

模型建好之后，就可以利用Kalman滤波了。假如要估算第K时刻的实际温度值，首先要根据k-1时刻的温度值来预测k时刻的温度。假定第k-1时刻的温度值测量值为23.9℃，房间真实温度为24.0℃，该测量值的偏差是0.1℃，即协方差P(k-1)=0.1²。
在第k时刻，房间的真实温度是24.1℃，温度计在该时刻测量的值为24.5℃，偏差为0.4℃。我们用于估算第k时刻的温度有两个温度值，分别是k-1时刻23.9℃和k时刻的24.5℃，如何融合这两组数据，得到最逼近真实值的估呢？

首先，利用k-1时刻温度值预测第k时刻的温度，其预计偏差为P（k|k-1）=P(k-1)+Q=0.02，计算Kalman增益K=P(k|k-1)/（P(k|k-1)+R）=0.0741，那么这时候利用k时刻的观测值，得到温度的估计值为X(K)=23.09+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)进行更新和处理。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能说明：Kalman滤波用在一维温度数据测量系统中
function main
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=12;%采样点的个数，时间单位是分钟，可理解为实验进行了60分钟的测量
CON=25;%室内温度的理论值，在这个理论值的基础上受过程噪声会有波动
%对状态和测量初始化
Xexpect=CON*ones(1,N);%期望的温度是恒定的25℃，但真实温度不可能会这样的
X=zeros(1,N);%房间各时刻真实温度值
Xkf=zeros(1,N);%Kalman滤波处理的状态，也叫估计值
Z=zero(1,N);
P=zeros(1,N);%温度计测量值
%赋初值
X(1)=25.1;%假如初始值房间温度为25.1℃
P(1)=0.01;%初始值的协方差
Z(1)=24.9;
Xkf(1)=Z(1);%初始测量值为24.9℃，可作为滤波器的初始估计状态

%噪声
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N);%方差决定噪声的大小
V=sqrt(V)*randn(1,N);%方差决定噪声的大小

%系统矩阵
F=1;
G=1;
H=1;
I=eye(1);%本系统状态为一维
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模拟房间温度和测量过程，并滤波
for k=2:N
    %第一步：随时间推移，房间真实温度波动变化
    %k时刻房间的真实温度，对于温度计来说，这个真实值是不知道的，但是它的存在
    %又是客观事实，读者要深刻领悟这个计算机模拟过程
    X(K)=F*X(k-1)+G*W(k-1);
    
    %第二步：随时间推移，获取实时数据
    %温度计对k时刻房间温度的测量，Kalman滤波是站在温度计的角度进行的，
    %他不知道此刻真实状态X(k)，只能利用本次测量值Z（k）和上一次估计值Xkf（k）来
    %做处理，其目标是最大限度地降低测量噪声R的影响，尽可能地逼近X(K)，
    %这也是Kalman滤波的目的所在
    
    %第三步：Kalman滤波
    %有了k时刻的观测Z（k）和k-1时刻的状态，那么就可以进行滤波了，
    %滤波的过程：
   X_pre=F*Xkf(k-1);
   P_pre=F*P(k-1)*F'+Q;
   Kg=P_pre*inv(H*P_pre*H'+R);
   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','Number Title','off');
   figure %画图显示
%依次输出理论值，叠加过程噪声（受波动影响）的真实值，
%温度计测量值，kalman估计值
   plot(t,Xexpect,'-b',t,X,'-r.','t',Z,'-ko',t,Xkf,'-g*');
   legend('期望值','真实值','观测值','Kalman滤波值');
   xlabel('采样时间/s');
   ylabel('温度值/℃');
%误差分析图
   figure %画图显示
   plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k*');
   legend('测量偏差','Kalman滤波偏差');
   xlabel('采样时间/s');
   ylabel('温度偏差值/℃');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%