kalman1liwenbin.asv

卡尔曼滤波程序的滤波程序分手那疯了数据分类开始的杰弗里斯

clear;
x0=0;
p0=0;
fai=0.98;
Q=0.01;
gama=1;
H=0.98;
R=0.01;
n=100;
%产生状态和测量
for k=1:n;    
    w(k)=sqrt(Q)*randn(1);     %#ok<AGROW>
    v(k)=sqrt(R)*randn(1);     %#ok<AGROW>
    if k==1;       
     
        
        x(k)=fai*x0+gama*w(k);        
        z(k)=H*x(k)+v(k);    
        
        x_pre(k)=fai*x0;        
        p_pre(k)=fai*p0*fai'+gama*Q*gama';        
        z_pre(k)=H*x_pre(k);        
        K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
        x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
        p_est(k)=(1-K(k)*H)*p_pre(k);  
    else x(k)=fai*x(k-1)+gama*w(k);        
        z(k)=H*x(k)+v(k);    
        
        x_pre(k)=fai*x_est(k-1);        
        p_pre(k)=fai*p_est(k-1)*fai'+gama*Q*gama';        
        z_pre(k)=H*x_pre(k);        
        K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
        x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
        p_est(k)=(1-K(k)*H)*p_pre(k); 
    end
end 
%kalman滤波算法
% for k=1:n;    
%     if k==1;        
%         x_pre(k)=fai*x0;        
%         p_pre(k)=fai*p0*fai'+gama*Q*gama';        
%         z_pre(k)=H*x_pre(k);        
%         K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
%         x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
%         p_est(k)=(1-K(k)*H)*p_pre(k);    
%     else        
%         x_pre(k)=fai*x_est(k-1);        
%         p_pre(k)=fai*p_est(k-1)*fai'+gama*Q*gama';        
%         z_pre(k)=H*x_pre(k);        
%         K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
%         x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
%         p_est(k)=(1-K(k)*H)*p_pre(k);    
%     end
% end 
% figure
% hold on
% plot(x,'r')
% plot(z,'b:')
% plot(x_est,'k-.')
% xlabel('时刻k')
% ylabel('幅值')
% title('状态值、测量值和估计值曲线图')
% legend('状态值','测量值','估计值')%%图例
% hold off
%绘制曲线(状态值和测量值)
figure
hold on
plot(x,'r')
plot(z,'b')
xlabel('时刻k')
ylabel('状态值和测量值')
title('状态值和测量值曲线图')
legend('状态值','测量值')%%图例
hold off
%绘制曲线(状态值和估计值)
figure
hold on
plot(x,'r')
plot(x_est,'b')
xlabel('时刻k')
ylabel('状态值和估计值')
title('状态值和估计值曲线图')
legend('状态值','估计值')
hold off
%绘制曲线(估计误差)
figure
plot(x-x_est,'r')
xlabel('时刻k')
ylabel('估计误差')
title('估计误差曲线图')
figure
%绘制曲线(估计误差协方差)
plot(p_est,'r')
xlabel('时刻k')
ylabel('估计误差协方差')
title('估计误差协方差曲线图')