卡尔曼滤波-在温度测量中的应用matlab代码

参考内容：书籍《卡尔曼滤波原理及应用——matlab仿真》

 卡尔曼知识

　　模型建立

　　　　观测方程：Z(k)=H*X(k)+V(k）；

　　　　状态方程：X(k)=A*X(k-1)+W(k-1);

　　其中，X（k）为系统在时刻k的状态，Z（k）为对应状态的测量值。W（k）为输入的白噪声（也是过程误差），V（k）为观测噪声（也是测量误差），W（k），V（k）是均值为零，方差阵各为Q和R的不相关白噪声。A为状态转移矩阵，H为观测矩阵。

　　卡尔曼滤波：

　　　　　　　　　预测                                                              校正

先验估计 ：                            卡尔曼增益：

先验协方差误差 ：      后验估计：

                                                                                      协方差:

 1、模型建立

       假设研究的对象是一个房间的温度。根据经验判定这个房间的温度大概在25℃左右，会受到环境的影响，房间内的温度还会有小幅度的波动。以分钟为单位，定时测量房间温度，这里的1分钟可以理解为采样时间。

　　假设测量温度时，由于环境影响，会引入过程噪声W（k）（假定服从高斯分布），其方差为Q，假设Q=0.01；状态X(k)是在第k分钟的房间温度，由于是一维的。那么该系统的状态方程和观测方程可以写为：

　　　　　　　　　　　　　　X(k)=A*X(k-1)+W(k);

　　　　　　　　　　　　　　Z(k)=H*X(k)+V(k);

因为X（k）是一维变量温度：A=1;H=1；W（k），V（k）的方差分别为Q和R。

此时模型算是已经建立好了，就可以运用kalman滤波了。

2、卡尔曼滤波

　　假如要估计第k时刻的实际温度值，首先要根据第k-1时刻的温度值来预测k时刻的温度。

       （1）假定第k-1时刻的温度值测量值为23.9℃，房间的真实温度为24℃，测量值的偏差为0.1，即协方差P（k-1）=0.1^2;

　　（2）在第k时刻，房间真实温度为24.1℃，温度计在该时刻测量的值为24.5℃，偏差为0.4。此时我们用于估算第k时刻的温度有两个温度值，分别是k-1时刻的23.9℃和k时刻的24.1℃，如何融合这两个数据得到最逼近真实值的估计值呢？

　　首先利用k-1时刻温度值预测第k时刻的温度，其预测偏差为P(k|k-1)=P(K-1)+Q=0.02;计算卡尔曼增益K=0.0741.那么这时可以利用k时刻的观测值，得到温度的估计值为X（k）=23.9+K*（24.5-23.9）=23.944；可见，与k-1时刻的23.9℃和k时刻的24.1℃，卡尔曼得到的值更接近与真实温度24.1℃。此时更新k时刻的偏差P（k）=（I-K*H）*P(k|k-1)=0.0186.(I为单位矩阵，由于是一维的所以是1).最后得到的X(k)和P(k),可以继续对下一时刻的观测数据进行更新处理。

　　　在这里需要注意一点的是我们需要确定kalman滤波器的两个初值，分别为X（0）和P（0）.

3、代码

下面直接上代码。更加直观一点。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%kalman滤波用在一维温度数据测量系统

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
close all
N=120; %采样点的个数，时间单位是分钟，可理解为试验进行了120分钟测量
CON=25; %室内温度理论值，在这个理论值基础上受过程噪声影响会有波动
%对状态和测量的初始化
Xexpect=CON*ones(1,N); %期望的温度是恒定25℃，但真实温度不可能会这样。
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; %测量值初始为24.9
Xkf(1)=Z(1); %初始测量值为24.9，可作为滤波器的初始估计状态
%噪声
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
%第一步随着时间的推移，房间真实温度波动变化，k时刻房间的真实温度，对于温度计
%来说这个真实值是不知道的，但他的存在又是客观事实
X(k)=F*X(k-1)+G*W(k-1);

%第二步，随时间推移，获取实时数据，温度计对k时刻房间温度的测量，kalman滤波是站在温度计
%角度进行的，他不知道此刻真实状态X(k)，只能利用本次测量值Z（k）和上一次估计值Xkf(k)来做处理
%其目标是最大限度地降低测量噪声R的影响，尽可能地逼近X（k），这也就是kalman滤波的目的。
Z(k)=H*X(k)+V(k);

%第三步：kalman滤波，有了k时刻的观测Z（k）和k-1时刻的状态，那么就可以进行滤波了。
X_pre(k)=F*Xkf(k-1); %先验估计
P_pre(k)=F*P(k-1)*F’+Q; %协方差先验估计
Kg=P_pre(k)*H’/(H*P_pre(k)*H’+R); %卡尔曼增益
Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k)); %kalman状态估计值
P(k)=(I-Kg*H)*P_pre(k); %协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算误差
Err_Messure=zeros(1,N); %测量值与真实值之间的偏差
Err_Kalman=zeros(1,N); %Kalman估计值与真实值之间的偏差

for i=1:N
Err_Messure(i)=abs(Z(i)-X(i));
Err_Kalman(i)=abs(Xkf(i)-X(i));
end

t=1:N;
figure; %画图显示
%依次输出理论值，叠加过程噪声的真实值，温度计测量值，kalman滤波值
plot(t,Xexpect,’-b’,t,X,’-r’,t,Z,’-g’,t,Xkf,’-y’);
legend(‘期望值’,’真实值’,’测量值’,’kalman滤波值’);
xlabel(‘采样时间/s’);
ylabel(‘温度值/s’);

%误差分析图
figure;
plot(t,Err_Messure,’-bs’,t,Err_Kalman,’-k*’);
legend(‘测量偏差’,’kalman滤波偏差’);
xlabel(‘采样时间/s’);
ylabel(‘温度偏差值/℃’);

 从图上可以看出，Kalman滤波与温度计直接测量的值相比，大大降低了偏差，虽然卡尔曼滤波误差没有完全消失，但它让状态尽可能逼近真实值。