卡尔曼滤波室内温度估计

该文章内容出于《卡尔曼滤波原理及应用-Matlab仿真》
在网上看了不少与卡尔曼滤波相关的博客、论文，要么是只谈理论、缺乏感性，或者有感性认识，缺乏理论推导。能兼顾二者的少之又少，直到我看到了国外的一篇博文，真的惊艳到我了，不得不佩服作者这种细致入微的精神，翻译过来跟大家分享一下，原文链接：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
CSDN已经被翻译过的链接：
https://blog.csdn.net/u010720661/article/details/63253509?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162919733216780366511849%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=162919733216780366511849&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_alltop_positive~default-1-63253509.pc_search_similar&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4187

% 程序说明：Kalman滤波用于温度测量的实例
function main
N=120;%采样点的个数，时间单位是分钟，可理解为实验进行了60分钟的测量
CON=25;%房间温度在25摄氏度左右室内温度的理论值，受过噪声会波动
%对状态和测量初始化
Xexpect=CONones(1,N);%期望的温度是25摄氏度，但真实温度不可能这样
X=zeros(1,N); %房间各时刻真实温度值
Xkf=zeros(1,N);%卡尔曼滤波处理的状态，也叫估计值
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;%W(k)的方差，预计过程中的误差
R=0.25;%V(k)的方差，观测过程中的误差
W=sqrt(Q)randn(1,N);%方差决定噪声的大小
V=sqrt®randn(1,N);%方差决定噪声的大小
%系统矩阵
F=1;%X(k-1)到X（k）的状态矩阵
G=1;%控制矩阵
H=1;%观测量矩阵
I=eye(1); %本系统状态为1维
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模拟房间温度和测量过程，并过滤
for k=2:N
%第一步：随着时间的推移，房间真实温度波动变化
%预测房间温度
X(k)=FX(k-1)+GW(k-1); % K状态的均值，FX(k-1)为X(k-1)到X(k)的转变，G*W(k-1)为外部控制量

%第二步：随着时间推移，获取实时数据
%对k时刻的温度进行测量
%不知道下X（k）时刻的真实值，只能利用本次的测量值Z（k）和上一次的估计值
%Xkf（k）来做处理降低R的噪声危害（观测量的噪声）尽可能逼近X（k），这是卡尔曼滤波目的所在

Z(k)=H*X(k)+V(k);%测量值的均值
X_pre=F*Xkf(k-1);%状态预测  状态矩阵*上一次的估计值 ，初始时刻将测量值24.9赋值给估计值        
P_pre=F*P(k-1)*F'+Q; %协方差预测 P(k-1)=（真实值-测量值）*（真实值-测量值）   
Kg=P_pre*inv(H*P_pre*H'+R); %计算卡尔曼增益
e=Z(k)-H*X_pre;%误差为测量值减去估计值            
Xkf(k)=X_pre+Kg*e; %K时刻的估计值为k-1时刻的测量值加上        
P(k)=(I-Kg*H)*P_pre;%K时刻的偏差

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,’-ko’,t,Xkf,’-g*’);
legend(‘expected’,‘real’,‘measure’,‘kalman extimate’);
xlabel(‘sample time/s’);
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’);
%%%%%%%%%%%%%%%%%%%%%%%%%