function ret = mpfim(Iteration)

% mpfim: multiple paired forward-inverse models
% by Masahiko Haruno

global Wfm Wim 
global Xt X
global Change Sigma Lambda 
global nex ts dt time nt ut lamb
global Traj Dst Kfb 
global Alpha Betha
global M B K


K = [8; 4; 1];
B = [2; 7; 3]; 
M = [1; 5; 8]; 

init;

for j=1:Iteration
  
  Wim
  Wfm

  Ut=zeros(nex,1);
  fmsqerr = zeros(nex,1);

  Xt = [0; 0; 0];	% [pos; vel, acc]
  X  = [0; 0; 0];	% [pos; vel, acc]

  Fdb = 0;
  time = 0; 

  sel=1;

  for i = 1:nt                     % 1’»þ’ÅÀ’Ë蒤˒ͽ’¬’¤È’À©’¸æ’¤ò’¹Ô’¤¦
    if(mod(i,Change)==0)
      if(sel==nex) sel=1;
      else sel=sel+1;
      end
    end

    Ut = Wim*Dst(i,:)';       % ’µÕ’¥â’¥Ç’¥ë’¤Î’½Ð’ÎÏ 
    UtSum = lamb'*Ut;
    ut = UtSum+Fdb;           % ’½Ð’ÎÏ’¤¹’¤ë’¥È’¥ë’¥¯ 

    % ’°Ì’ÃÖ’¡¢’¬’ÅÙ’¤È’¥È’¥ë’¥¯’¤«’¤é’²Ã’®’Åْͽ’¬
    pacc = Wfm*[Xt(1); Xt(2); ut]; 

    X=Xt;
    Xt = rkstep(Xt, ut, dt, M(sel), B(sel), K(sel));
    Fdb= -Kfb'*(Dst(i,:)'-Xt);

    acc=ones(nex,1)*Xt(3);

    % responsibility signal
    fmerr = pacc - acc;            % ’Îó’¥Ù’¥¯’¥È’¥ë
    Acc(i,:) = [dt*i lamb'*pacc];
    Act(i,:) = [dt*i Xt(3)];
    fmsqerr = (fmerr./Sigma).^2;
    fexperr = exp(-0.5*fmsqerr);
    lamb = fexperr./sum(fexperr+eps);

    Wim = Wim - Alpha.*Fdb.*lamb*Dst(i,:);
    Wfm = Wfm - Betha*lamb.*fmerr*[X(1) X(2) ut];

    Lambda(i,:) = [dt*i lamb'];

  end 

  if(j==1)
    figure(1)
    subplot(2,2,1);
    plot(Lambda(:,1),Lambda(:,2),'r-.'); hold on;
    plot(Lambda(:,1),Lambda(:,3),'b-');
    plot(Lambda(:,1),Lambda(:,4),'y--');
    axis([0 i*dt -0.01 1.01]);
    xlabel('Time (sec)');
    ylabel('Responsibility');
    title('First learning');
    hold off;
    subplot(2,2,3);
    plot(Traj(:,1),Traj(:,4),'r-.'); hold on;
    plot(Acc(:,1),Acc(:,2),'b-.'); hold on;
    plot(Act(:,1),Act(:,2),'y--');
    axis([0 i*dt 0 1]);
    title('First learning');
    xlabel('Time (sec)');
    ylabel('Acceleration');
    hold off;
  end;
end

subplot(2,2,2);
plot(Lambda(:,1),Lambda(:,2),'r-.'); hold on;
plot(Lambda(:,1),Lambda(:,3),'b-');
plot(Lambda(:,1),Lambda(:,4),'y--');
axis([0 i*dt -0.01 1.01]);
xlabel('time (sec)');
ylabel('Responsibility');
title('After learning');
hold off;

subplot(2,2,4);
plot(Traj(:,1),Traj(:,4),'r-.'); hold on;
plot(Acc(:,1),Acc(:,2),'b-.'); hold on;
plot(Act(:,1),Act(:,2),'y--');
axis([0 i*dt 0 1]);
xlabel('time (sec)');
ylabel('Acceleration');
title('After learning');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Xt1 = rkstep(Xt, ut, dt, M, B, K)
% RKSTEP
%  Xt1 = rkstep(Xt, ut, dt)

k1 = dt*Xt(2); l1 = dt*((ut-K*Xt(1)-B*Xt(2))/M);
k2 = dt*(Xt(2)+0.5*l1); l2 = dt*((ut-K*(Xt(1)+0.5*k1)-B*(Xt(2)+0.5*l1))/M);
k3 = dt*(Xt(2)+0.5*l2); l3 = dt*((ut-K*(Xt(1)+0.5*k2)-B*(Xt(2)+0.5*l2))/M);
k4 = dt*(Xt(2)+l3); l4 = dt*((ut-K*(Xt(1)+k3)-B*(Xt(2)+l3))/M);

p = Xt(1)+(k1+2*k2+2*k3+k4)/6;
v = Xt(2)+(l1+2*l2+2*l3+l4)/6;
a = (ut-K*p-B*v)/M;

Xt1 = [p; v; a];  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function init()

global Wfm Wim 
global Xt X
global Change Sigma Lambda 
global nex ts dt time nt ut lamb
global Traj Dst Kfb 
global Alpha Betha
global M B K

	nex = 3; 	% number of experts
	ts =  0.3;    	% trial time
	dt = 0.001;
	nt = ts/dt;

	Sigma = 0.01;  % for learning

	Change = 100;

	Lambda = (1/3)*ones(nt, nex+1);
	lamb = [1/3; 1/3; 1/3]; % ’ÀՒǤ’¿®’¹æ’½é’´ü’²½ 

	Kfb = [0.3; 0.3; 0.3] % ’¥Õ’¥£’¡¼’¥É’¥Ð’¥Ã’¥¯’¥²’¥¤’¥ó

	Alpha=0.01;
	Betha=0.02;

	Traj = zeros(nt, 4);

	% ’½é’´ü’ÃÍ’Àß’Äê’ÍÑ’¤Î’ÃÍ

	KK = [8; 4; 1];
	BB = [2; 7; 3]; 
	MM = [1; 5; 8]; 

	% ’²Ã’®’ÅÙ’¤Î’¤ß’¿ä’Äê

	Wim = [KK BB MM] + 0.5*[rand(nex,1) rand(nex,1) rand(nex,1)];

	%Wim = [KK BB MM] + 5*[rand(nex,1) rand(nex,1) rand(nex,1)]

	% ’ÎÏ’¤ò’¿ä’Äê       
	Wfm = [-KK./MM -BB./MM 1./MM]+0.1*[rand(nex,1) rand(nex,1) rand(nex,1)];
        %Wfm = [-KK./MM -BB./MM 1./MM]+[rand(nex,1) rand(nex,1) rand(nex,1)]
	fid=fopen('abc.dat','r');
        for i=1:nt
          Traj(i,:)=fscanf(fid,'%f',[1,4]);
        end
	status=fclose(fid);

	Dst=zeros(nt,3);
	Dst=Traj(:,2:4); % ’Íý’ÁÛ’µ°’Æ»’¤À’¤±’¼è’¤ê’½Ð’¤¹

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%