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

卡尔曼滤波在温度测量中的应用
X(k)=AX(k-1)+TW(k-1)
Z(k)=H*X(k)+V(k)

房间温度在25摄氏度左右，测量误差为正负0.5摄氏度，方差0.25，R=0.25。Q=0.01,A=1,T=1,H=1。

假定快时刻的温度值、测量值为23.9摄氏度，房间真实温度为24摄氏度，温度计在该时刻测量值为24.5摄氏度，偏差为0.4摄氏度。利用k-1时刻温度值测量第k时刻的温度，其预计偏差为:
P(k|k-1)=P(k-1)+Q=0.02
卡尔曼增益k=P(k|k-1) / (P(k|k-1) + R)=0.0741

X(k)=23.9+0.0741*(24.1-23.9)=23.915摄氏度。

k时刻的偏差为P(k)=(1-K*H)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。

matlab实现为：

% 程序说明：Kalman滤波用于温度测量的实例
function main
N=120;
CON=25;%房间温度在25摄氏度左右
Xexpect=CONones(1,N);
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);
Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
W=sqrt(Q)randn(1,N);
V=sqrt®randn(1,N);
F=1;
G=1;
H=1;
I=eye(1);
%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
X(k)=FX(k-1)+GW(k-1);
Z(k)=HX(k)+V(k);
X_pre=FXkf(k-1);
P_pre=FP(k-1)F’+Q;
Kg=P_preinv(HP_preH’+R);
e=Z(k)-HX_pre;
Xkf(k)=X_pre+Kge;
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 filter example of temperature measurement in Matlab implementation of Kalman filter algorithm.
% 房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度（即认为连续的两个时刻最多变化0.02度）。
% 温度计的测量误差为0.5度。
% 开始时，房间温度的估计为23.5度，误差为1度。
close all;

% intial parameters
% 计算连续n_iter个时刻
n_iter = 100;
% size of array. n_iter行，1列
sz = [n_iter, 1];
% 温度的真实值
x = 24;
% 过程方差，反应连续两个时刻温度方差。更改查看效果
Q = 4e-4;
% 测量方差，反应温度计的测量精度。更改查看效果
R = 0.25;
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
z = x + sqrt®*randn(sz);
% 对数组进行初始化
% 对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值
xhat = zeros(sz);
% 后验估计的方差
P = zeros(sz);
% 温度的先验估计。即在k-1时刻，对k时刻温度做出的估计
xhatminus = zeros(sz);
% 先验估计的方差
Pminus = zeros(sz);
% 卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度
K = zeros(sz);
% intial guesses
xhat(1) = 23.5; %温度初始估计值为23.5度
P(1) =1; % 误差方差为1

for k = 2:n_iter
% 时间更新（预测）
% 用上一时刻的最优估计值来作为对当前时刻的温度的预测
xhatminus(k) = xhat(k-1);
% 预测的方差为上一时刻温度最优估计值的方差与过程方差之和
Pminus(k) = P(k-1)+Q;
% 测量更新（校正）
% 计算卡尔曼增益
K(k) = Pminus(k)/( Pminus(k)+R );
% 结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差
xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
% 计算最终估计值的方差
P(k) = (1-K(k))*Pminus(k);
end

FontSize = 14;
LineWidth = 3;
figure();
plot(z,‘k+’); %画出温度计的测量值
hold on;
plot(xhat,‘b-’,‘LineWidth’,LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),‘g-’,‘LineWidth’,LineWidth); %画出真实值
legend(‘温度计的测量结果’, ‘后验估计’, ‘真实值’);
xl = xlabel(‘时间(分钟)’);
yl = ylabel(‘温度’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
hold off;
set(gca,‘FontSize’,FontSize);

figure();
valid_iter = 2:n_iter; % Pminus not valid at step 1
% 画出最优估计值的方差
plot(valid_iter,P(valid_iter),‘LineWidth’,LineWidth);
legend(‘后验估计的误差估计’);
xl = xlabel(‘时间(分钟)’);
yl = ylabel(‘℃^2’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
set(gca,‘FontSize’,FontSize);