卡尔曼滤波matlab实例
- 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%
% 程序说明:Kalman滤波用于温度测量的实例
% 详细原理介绍及中文注释请参考:
% 《卡尔曼滤波原理及应用-MATLAB仿真》,电子工业出版社,黄小平著。%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%
function main %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%
N=120;
CON=25;
Xexpect=CON*ones(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;
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
X(k)=F*X(k-1)+G*W(k-1);
Z(k)=H*X(k)+V(k);
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','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');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%