run_kalman.asv

针对卡尔曼滤波的原创改进算法

% 功能          赋予必要初始值调用卡尔曼滤波程序 以便得到估计值
% 作者          刘宗尧
% 日期          2006-11-23
% 最后修改日期   2006-11-13
% 版本          v1.0
%--------------------------------------------------------------------

% 载入初始化数据
clear;
clc;
load('kalman_data.mat');

% 改变方差大小，以生成不同的测量数据
disp('总的点数')
total=length(x)-1

disp('R,IAF_KF,KF,ME');
test_R=0.5:0.5;
Num=length(test_R);
total_MM=zeros(Num,4);
for i=1:Num
    R=test_R(i);
    SN=S/R;
y=mmeasured(y1,R);
ES0=[y(1),2]';

%---------------------------------------------------------------------
% T 时间间隔
% A 状态转移矩阵
% H 测量矩阵
% Q 系统噪声方差
% R2 测量噪声方差
% P0 初始估计方差
% ES0 状态初始值
% x 测量时间点向量
% y 测量值向量

% 输出值
% ES 状态估计值
% PP 误差方差矩阵
% YY 测量估计值和真实值的误差
% TEMP_Y  理论平均值
% TEMP_P  理论偏差

%--------------------------------------------------------------------------
% tic; % 计时开始
% 1-经过推理
% 0-一般卡尔曼滤波
[ES,PP,YY,TEMP_Y,TEMP_P]=ffilter(A,H,Q,R,P0,ES0,x,y,1,SN);
% toc;
[ES1,PP1,YY1,TEMP_Y1,TEMP_P1]=ffilter(A,H,Q,R,P0,ES0,x,y,0,SN);
% 计时结束
%--------------------------------------------------------------------------

% 描写初始状态
% disp('信号振幅S，噪声振幅R,信噪比')
% S 
% R
% SN


%-----------------------------------------------
% subplot(2,1,1);
plot(x,y1,'k-',x,ES1(1,:),'r-.',x,ES(1,:),'g:');
xlabel('时间');
ylabel('幅值');
legend('真实值','IAF-KF','KF')
axis([0 150 -11 11]);

%--------------------------------------------------------------------------

% subplot(6,1,2);
% plot(x,ES(1,:));
% xlabel('时间');
% ylabel('滤波值');
% ylim([-15,15]);
% %--------------------------------------------------------------------------
% 
% subplot(6,1,3);
% plot(x,y1);
% xlabel('时间');
% ylabel('真实值');
% ylim([-15,15]);
%--------------------------------------------------------------------------
% subplot(2,1,2);

% plot(x,abs(y-y1),'k-',x,abs(ES(1,:)-y1),'r:*',x,abs(ES1(1,:)-y1),'g--^');
% title('ME，IAF-KF，KF误差对比图')
% xlabel('Time(sec)');
% ylabel('error');
% legend('ME','IAF-KF','KF')
% % axis([0 50 0 1]);
% axis normal
% grid on

% E=abs(ES(1,:)-y1);
% F=abs(ES1(1,:)-y1);
% X1=change(x,10);
% E1=change(E,10);
% F1=change(F,10);
% 
% plot(X1,E1,'r+-',X1,F1,'g^:');
% title('IAF-KF，KF误差对比图')
% xlabel('Time(sec)');
% ylabel('error');
% legend('IAF-KF','KF')
% %  axis([0 150 0 1]);
%  axis normal

% subplot(4,1,2);
% plot(x,abs(ES(1,:)-y1)');
% xlabel('时间');
% ylabel('IAF-KF');
% ylim([-10,10]);
% 
% 
% %--------------------------------------------------------------------------
% subplot(4,1,3);
% plot(x,abs(ES1(1,:)-y1));
% xlabel('时间');
% ylabel('KF');
% ylim([-10,10]);
% 
% 
% %--------------------------------------------------------------------------
% subplot(4,1,4);
% plot(x,abs(y-y1));
% xlabel('时间');
% ylabel('ME');
% ylim([-15,15]);

% 
% %----------------------------------------------------------------------

% disp('模糊推理估计平均误差');
IAF_KF=sqrt(sum((ES(1,:)-y1).^2 )/total);
% disp('估计平均误差');
KF=sqrt(sum((ES1(1,:)-y1).^2 )/total);
% disp('测量平均误差');
ME=sqrt(sum((y-y1).^2 )/total);
MM=zeros(1,3);
MM(1)=R;
MM(2)=IAF_KF;
MM(3)=KF;
MM(4)=ME;

total_MM(i,:)=MM;

end
%--------------------------------------------------------------------------
%%-------------------统计点数----------------------------------------------



% fuzzy_wucha=abs(ES(1,:)-y1);
% guji_wucha=abs(ES1(1,:)-y1);
% celiang_wucha=abs(y-y1);
% 
% disp('加入推理好的点数')
% tuili=sum((fuzzy_wucha-celiang_wucha)<=0)
% tuili/total
% 
% disp('未加入推理好的点数')
% no_tuili=sum((guji_wucha-celiang_wucha)<=0)
% no_tuili/total 
% 
% 
% disp('推理比未推理好的点数')
% sum((fuzzy_wucha-guji_wucha)<=0)

disp('ok');