grade.asv

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

% 输入真实值,输出真实值
function vz=compute_out(vx,vy)
gl_out1=grade(-5,5,7,vx);
gl_out2=grade(-6,6,7,vy);

ind_temp=zeros(4,2);

ind_temp(1,:)=induce(gl_out1(1,:),gl_out2(1,:));
ind_temp(2,:)=induce(gl_out1(1,:),gl_out2(2,:));
ind_temp(3,:)=induce(gl_out1(2,:),gl_out2(1,:));
ind_temp(4,:)=induce(gl_out1(2,:),gl_out2(1,:));

        

function ind=induce(in_one,in_two)
%采用两输入单输出的形式
%in_one 输入等级隶属度数组一 1*2
%in_two 输入等级隶属度数组二1*2
%输出   等级隶属度数组1*2
%等级 7 NB NM NS ZE PS PM PB
induc=zeros(7,7);
induc=[7 7 6 6 5 5 4;7 6 6 5 5 4 3;6 6 5 5 4 3 3;6 5 5 4 3 3 2;5 5 4 3 3 2 2;5 4 3 3 2 2 1;4 3 3 2 2 1];
ind(1,1)=induc(in_one(1,1)+4,in_two(1,1)+4)-4;
ind(1,2)=min(in_one(1,2),in_two(1,2));



function gl_out=grade(fielda,fieldb,N,vx)
%gl_out  2*2
%[-x,x]形式
% fielda 论域始点
% filedb 论域终点
% N 划分级别参数 总共有2*N+1级
% 输入真实值
% 输出二维数组 级别+隶属度
gl_out=zeros(2,2);
d=(fieldb-fielda)/(2*N);

%向0取整
jb=fix(vx/d);
% eps 整数最小步进距离 

if jb<=-N 
    gl_out(1,1)=-N;
    gl_out(1,2)=1;
    gl_out(2,1)=0;
    gl_out(2,2)=0;
elseif jb<eps
    gl_out(1,1)=jb;
    gl_out(1,2)=li((jb-1)*d,(jb+1)*d,vx);
    gl_out(2,1)=jb-1;
    gl_out(2,2)=li((jb-2)*d,jb*d,vx);

elseif jb<N
    gl_out(1,1)=jb;
    gl_out(1,2)=li((jb-1)*d,(jb+1)*d,vx);
    gl_out(2,1)=jb+1;
    gl_out(2,2)=li((jb)*d,(jb+2)*d,vx); 

else 
    gl_out(1,1)=N;
    gl_out(1,2)=1;
    gl_out(2,1)=0;
    gl_out(2,2)=0;
end   


%等腰三角形隶属单元函数
function ms=li(a,b,x)
% a 起点
% b 终点
% x 输入
if x<=a 
    ms=0;
elseif x<(a+b)/2
    ms=2*(x-a)/(b-a);
elseif x<b
    ms=2*(x-b)/(a-b);
else
    ms=0;
end