其他分享
首页 > 其他分享> > 卡尔曼滤波-在温度测量中的应用matlab代码

卡尔曼滤波-在温度测量中的应用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滤波与温度计直接测量的值相比,大大降低了偏差,虽然卡尔曼滤波误差没有完全消失,但它让状态尽可能逼近真实值。

标签:噪声,kalman,滤波,代码,测量,matlab,时刻,卡尔曼滤波,温度
来源: https://www.cnblogs.com/sbb-first-blog/p/16557251.html