卡尔曼滤波matlab实例

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 程序说明：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');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

本文来源：https://www.bwwdw.com/article/6tu2.html