ffilter.m

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

% 功能1           实现常规卡尔曼滤波算法
% 功能2           加入模糊控制模块
% 作者            刘宗尧
% 日期            2006-11-23
% 最后修改日期    2006-11-23
% 版本            v1.0
function [ES,PP,YY,TEMP_Y,TEMP_P]=ffilter(A,H,Q,R,P0,ES0,x,y,flag,SN)
% function [ES,PP,YY,TEMP_Y,TEMP_P]=ffilter(A,H,Q,R,P0,ES0,x,y) 
% 函数参数:
% 输入
% A 状态转移矩阵
% H 测量矩阵
% Q 系统噪声方差
% R 测量噪声方差
% P0 初始估计方差
% ES0 状态初始值
% x测量时间点
% y测量值
%------------------------------------------------------------------------
% 输出值
% ES 状态估计值
% PP 误差方差矩阵
% YY 测量估计值和真实值的误差
% TEMP_Y  理论平均值
% TEMP_P  理论偏差

%循环初始化
YY{1}=0;
ES(:,1)=ES0;
temp_XX=A*ES0;
temp_PP=A*P0*A'+Q;
PP{1}=temp_PP;
TEMP_P{1}=H*temp_PP*H'+R;

% K的选择很重要 知道y(1)不用的原因
K=temp_PP*H'*(TEMP_P{1})^(-1);
ES(:,2)=temp_XX+K*(y(2)-H*temp_XX);
P=(eye(length(temp_PP))-K*H)*temp_PP*(eye(length(temp_PP))-K*H)'+K*R*K';
TEMP_Y{2}=H*temp_XX;
YY{2}=y(2)-TEMP_Y{2}; 
R0=R;

%------------------------------------------------------------
%从第3个时间点开始构造kalman滤波方程

  ttt=0;
for i=3:length(x)
    temp_XX=A*ES(:,i-1);
    temp_PP=A*P*A'+Q; 
    TEMP_P{i}=H*temp_PP*H'+R;
    K=temp_PP*H'*(TEMP_P{i})^(-1); 
    ES(:,i)=temp_XX+K*(y(i)-H*temp_XX);
    P=(eye(length(temp_PP))-K*H)*temp_PP*(eye(length(temp_PP))-K*H)'+K*R*K';
    TEMP_Y{i}=H*temp_XX;
    YY{i}=y(i)-TEMP_Y{i}; 
% 推理控制原理
% 实际值-理论值  >0 实际测量值比理论值    大    
% 实际值-理论值  <0 实际测量值比理论值    小      

%   控制初始值
    
    NN=15;     % 最邻近的值
    r=0.1;   % 输出控制调节参数
    Grade=1;   % 等级 2*Grade+1级
    field=1;  % 误差范围
    
    
   [in_r,in_p]=get_induce(YY,i,NN);

    s=TEMP_P{i}-in_p;
    fazhi=0.005*R0; %模糊控制阀值 利用0.1*R0控制，可以获得时间上的优势
 if flag==1
%     if abs(s)>fazhi
    
%     kkk=fix(1/SN)+1    
% 
%     [out_grade,mem_value]=T_induce(kkk*(1/SN)*R,kkk*R,s);
        [out_grade,mem_value]=T_induce(0.1,0.3,s);
%         if abs(s)<1/SN*R
%             out_grade=0;
%         end
%  T-S 推理模块
%   mm 二维 级别+隶属度
   
%      if ttt<50
%          ttt=ttt+1
%          s
%          R
%      end
    %运用模糊推理改善结果
   
    if abs(s)>abs(R) 
    R=R-r*out_grade*mem_value*(1-abs(R)/abs(s))*R;
    else
    R=R-r*out_grade*mem_value*abs(s);
    end
     
%     end
%     end
 end
    PP{i}=P;
end

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

% 获得推理向量

function [in_r,in_p]=get_induce(YY,N,NN) 
% 函数参数
% 输入：
% YY    理论值与真实值的误差
% N     当前最近可用测量数据个数
% NN    最邻近的个数
% 输出：
% in_r  误差平均值
% in_p  误差方差

        r_temp=0;
        p_temp=0;
    if N<NN
       
        for j=1:N
            r_temp=YY{j}(1)+r_temp;
            p_temp=YY{j}(1)^2+p_temp; 
        end
        in_r=r_temp/N;
        in_p=sqrt(p_temp/N);
     else
         for k=(N-NN+1):N
            r_temp=YY{k}(1)+r_temp;
            p_temp=YY{k}(1)^2+p_temp;
         end
          in_r=r_temp/NN;
          in_p=sqrt(p_temp/NN);
    end
% 这个很重要,通过统计原理得到的
%     in_p=(1/NN)*sqrt(in_p);
%     in_p=sqrt(in_p);