matlab正版有翻译,帮忙翻译MATLAB程序 必有重谢 发现金红包

function [OUTrota,OUTposi]=fcn(INrota,INposi,GROUNscale,Bsize,Bposi)

OUTposi=zeros(3,1); OUTrota=zeros(4,1);

Sin=5;Shid1=50;Sout=4;Nsensor=5;

IN=zeros(Sin,1); OUT=zeros(Sout,1); OUTb=zeros(Sout,1);OUTP=zeros(Sout,1);

Lsensor=10;ANsensor=pi/4;INSECTsize=2; DLsensor=0.2; DOTsensor=Lsensor/DLsensor; INsensor=zeros(Nsensor,1);ANGsensor=zeros(Nsensor,1);

ESENSORx=zeros(Nsensor,1);ESENSORz=zeros(Nsensor,1);POSz=0;POSx=0;ROTA=0;Tmin=5;

MAXW=0;MAXW=GROUNscale(1,1)/2; BPrate=0.1; GAUrate=0.1; GAUdeta=0.1; MINjudge=0.15;%parameters of gaussian judgement

BLOposx=0;BLOposz=0; BLOposx=abs(Bposi(1,1));BLOposz=Bposi(3,1); BLOsize=0;BLOsize=Bsize(1,1)/2;

persistent Wh1I WoH1 INcache HDEN HDENcache OUTcache CHOSEcache CHOSE Tb REWcache INIjudge

if isempty(Wh1I)%

Wh1I=0.1*randt(Shid1,Sin);WoH1=0.2*randt(Sout,Shid1+1)-0.1; HDENcache=zeros(Shid1+1,1);  HDEN=zeros(Shid1+1,1);

OUTcache=zeros(Sout,1); INcache=zeros(Sin,1);Tb=20;CHOSEcache=1; CHOSE=1; REWcache=0;INIjudge=0;

end

for n=1:Nsensor

ANGsensor(n,1)=-ANsensor*(Nsensor-1)/2+(n-1)*ANsensor;

end

POSx=INposi(1,1);POSz=INposi(3,1); ROTA=INrota(4,1);

DENin=0; VWdis1=abs(abs(POSx)-MAXW); VWdis2=abs(abs(POSz)-MAXW); VBdis1=sqrt((abs(POSx)-BLOposx)^2+(abs(POSz)-BLOposz)^2); VBdis2=sqrt((abs(POSx)-BLOposz)^2+(abs(POSz)-BLOposx)^2);

for nsensor=1:Nsensor% sensor reading

IN(nsensor,1)=1/Lsensor;

if VWdis1

DOTx=0;DOTz=0;DOTzjudge=0;DOTzjudge=0;

for n=1:DOTsensor

DOTx=n*DLsensor*sin(ROTA-ANGsensor(nsensor,1)); DOTz=n*DLsensor*cos(ROTA-ANGsensor(nsensor,1));

DOTxjudge=DOTx+POSx; DOTzjudge=DOTz+POSz; DISTblock1=sqrt((abs(DOTxjudge)-BLOposx)^2+(abs(DOTzjudge)-BLOposz)^2);

DISTblock2=sqrt((abs(DOTxjudge)-BLOposz)^2+(abs(DOTzjudge)-BLOposx)^2);

if abs(DOTxjudge)>MAXW || abs(DOTzjudge)>MAXW || DISTblock1

IN(nsensor,1)=5/sqrt(DOTx^2+DOTz^2);

break

end

end

end

DENin=DENin+IN(nsensor,1)^2;

end

IN=IN/sqrt(DENin); DENdlength=zeros(Shid1,1);Dlength=zeros(Shid1,Sin); GAUout=zeros(Shid1,1); DENwh1I=zeros(Shid1,1);

for n=1:Shid1

for m=1:Sin

Dlength(n,m)=IN(m,1)-Wh1I(n,m);

DENdlength(n,1)=DENdlength(n,1)+Dlength(n,m)^2; % distance between vectors

DENwh1I(n,1)=DENwh1I(n,1)+Wh1I(n,m)^2;

end

GAUout(n,1)=exp(-DENdlength(n,1)/2/GAUdeta);% first layer

end

MINi=0;MINv=0;judgemin=0; DENlengthjudge=DENdlength;DWh1I=zeros(1,Sin);

while judgemin==0

[MINv,MINi]=min(DENlengthjudge);

if sqrt(DENlengthjudge(MINi,1))0.5

DWh1I(1,1:Sin)=GAUrate*Dlength(MINi,1:Sin);judgemin=1;

elseif sqrt(DENlengthjudge(MINi,1))>MINjudge && sqrt(DENwh1I(MINi,1))>0.5

DENlengthjudge(MINi,1)=3;judgemin=0;

else

DWh1I(1,1:Sin)=Dlength(MINi,1:Sin);judgemin=1;

end

end

Wh1I(MINi,1:Sin)=Wh1I(MINi,1:Sin)+DWh1I(1,1:Sin);

DENdlength(MINi,1)=0;

for m=1:Sin

Dlength(MINi,m)=IN(m,1)-Wh1I(MINi,m);

DENdlength(MINi,1)=DENdlength(MINi,1)+Dlength(MINi,m)^2;% update the distance and gauss

end

GAUout(MINi,1)=exp(-DENdlength(MINi,1)/2/GAUdeta);

HDEN=[GAUout;sqrt(DENin)/5];

NETout=WoH1*HDEN; % linear output layer

OUTPt=0;

for n=1:Sout

OUT(n,1)=NETout(n,1); % Q factor

OUTb(n,1)=exp(OUT(n,1)/Tb);

OUTPt=OUTPt+OUTb(n,1);

end

for n=1:Sout

OUTP(n,1)=OUTb(n,1)/OUTPt;%output probability

end

Bb=0.8; %anneling

judge=0;

judge=rand(1);OUTrota=INrota;OUTrota(2,1)=1;OUTrota(3,1)=0;OUTposi=INposi;REW=0;

if judge

CHOSE=1; OUTrota(4,1)=INrota(4,1)+pi/20;  REW=-0.2;%turn left 20degree

elseif OUTP(1,1)

CHOSE=2;  OUTrota(4,1)=INrota(4,1)-pi/20; REW=-0.2;%turn right 20degree

elseif (OUTP(1,1)+OUTP(2,1))

CHOSE=3; OUTposi(1,1)=POSx+0.15*sin(INrota(4,1));OUTposi(3,1)=POSz+0.15*cos(INrota(4,1));REW=1; %move forward

else

CHOSE=4; OUTposi(1,1)=POSx+0.1*sin(INrota(4,1));OUTposi(3,1)=POSz+0.1*cos(INrota(4,1));REW=0.5; %move forward

end

if VWdis1

REW=-5*DENin;REWcache=REW+REWcache;%-(IN(1,1)+IN(2,1)+IN(3,1));5*(IN(1,1)+IN(2,1)+IN(3,1));

end

if VWdis1<5 || VWdis2<5 || VBdis1<5 || VBdis2<5 %Get back to original point when hit the wall

OUTposi(1,1)=0;OUTposi(3,1)=0; OUTrota(4,1)=0;Tb=Tmin+(Tb-Tmin)*Bb;INIjudge=0;

end

if INIjudge==1

ERROR=1; OUTexpect=0;

while abs(ERROR)>(0.1*abs(OUTexpect)+0.1)

NETout=WoH1*HDENcache; % first layer linear output

for n=1:Sout

OUTcache(n,1)=NETout(n,1); % Q factor

end

OUTexpect=(0.5*max(OUT)+REWcache);%)*0.5+0.5*OUTcache(CHOSEcache,1);

ERROR=OUTexpect-OUTcache(CHOSEcache,1);DWoH1=0;

for n=1:Shid1+1

DWoH1=BPrate*ERROR*HDENcache(n,1);

WoH1(CHOSEcache,n)=WoH1(CHOSEcache,n)+DWoH1;

end

end

i11=1;

end

HDENcache=HDEN;CHOSEcache=CHOSE;REWcache=REW;

INIjudge=1;

end

function ran=randt(S1,S2)

ran=zeros(S1,S2);

for n=1:S1

for m=1:S2

ran(n,m)=rand(1);

end

end

end