Simulatore senza simulink

Ciao a tutti,
devo implementare un simulatore per un robot planare a 2 bracci senza usare simulink
senza scendere troppo nel merito ho realizzato uno script (che fa parte di altri in un namespace) che dovrebbe realizzare la parte cruciale (ne copio solo uno pezzo):

qcorr rappresenta la posizione corrente delle variabili di giunti, che ho posto inzialmente a [0;0], è un vettore 2x1 che contiene le informazioni sugli angoli in radianti;
qd la posizione desiderata (cioè da raggiungere) ed è fissata a [pi/4;0] (in sostanza il braccio deve stendesi in mezzo al primo quadrante), valgono le stesse considerazioni di qcorr;
B,C,G sono le function che calcolano le matrici caratteristiche del robot;
T è il regolatore cioè secondo la teoria dinamica :
B*q''+C*q'+G+Fv=T (l apostrofo vorrebbe indicare le derivate, Fv è la matrice degli attriti);
posizioni è una matrice in cui salvo le posizioni nel tragitto verso qd;
t è una var global che uso per comunicare a tutte le function i valori delle matrici ad ogni iterazione;
delta è un valore che ho provato a porre sia a 0.05 che a 0.005;

while(qcorr(1)~=qd(1)) || (qcorr(2)~=qd(2))
     posizioni(i,1)=qcorr(1);
     posizioni(i,2)=qcorr(2);
     Br=B(qcorr);
     Cr=C(qcorr,wcorr);
     Gr=G(qcorr);
     [Tr e]=Regolatore(qcorr,wcorr,qd)
     t=[Br Cr Gr Tr]
     fun='odefile';
     y0=[qcorr;wcorr] %condizioni iniziali di posizione e velocità (4x1)
     tstan=[0 delta]; %intervallo in cui calcolare la soluzione dell eq diff
     [t y]=ode45(fun,tstan,y0(3:4,:))
      %l'ultima riga di y contiene l ultima posizione
     [a b]=size(y);
     qk=[y(a,1);y(a,2)] % la nuova posizione
     wcorr=(qk-qcorr)/delta; % calcolo la velocità come variazione di posizione nel tempo
     qcorr=qk;
     i=i+1;

ovviamente non funziona (e grazie, direte voi sennò cosa scrivi a fare qui), ma quello che non riesco a capire è perchè qcorr assume ad ogni iterazione valori sballati, si muove anche al contrario in certe iterazioni (cioè in senso orario, assumedo valori negativi)
in particolare credo di non aver ben compreso il funzionamento della funzione ode45 e di cosa restituisce nel vettore y (i commenti indicano quello che io penso restituisca ma mi sorge il dubbio che non siano deduzioni corrette). Ovviamente ho già letto la documentazione ma nn ho capito granchè.
Copio di seguito lo script che esegue l ode45:

function [ z ] = odefile(t,y0)
global t;
B=t(:,1:2);
C=t(:,3:4);
G=t(:,5);
T=t(:,6);
Fv=[1; 0];
z=B\(T-C*y0-G-Fv);
end

copio infine per completezza la function che implementa il regolatore. Faccio presente che i valori delle matrici Kd e Kp li ho scelti casualmente ma ne ho provati diversi:

function [ T,e ] = Regolatore( q,w,qd )
%Controllore per un manipolatore a due bracci.Riceve posizione e velocità attuali e
%posizione di riferimento

Kp=[1 0;0 1];
Kd=[1 0;0 1];

er=qd-q
e=norm(er);

T=Kp*er+Kd*w;
end

Grazie per l attenzione, ed ogni suggerimento è ben accetto!
Ciao.

Pete