You will get better responses if you post your question in this forum section:
Matlab: https://www.eng-tips.com/threadminder.cfm?pid=575

When you re-post your question, it will also help immensely if you were to describe the architecture of your control system in more detail than that one diagram.

www.sparweb.ca

Waseem... The reference You enclosed led to a file... 'auv_vinput_11sep.m'… that I could NOT [safely] open.

What design document/text are You referencing, RE that flow/path diagram??????????

NOTE.
My EE-EL-AV database shows there are LOTS of useful texts, documents and specification on this subject that could assist You. LOOK HARDER!

OK... but... Since my EE-EL-AV database was recently identified as a 'corporate trade secret', I can't release it, parts of it, or even list a few references from it... on this or similar non-corporate sites.

Yeah guys... I've had to do engineering liaison/backshop work in Electrical, Electronic and Avionic system items/elements over the decades... since most EEs & ELs are not always 'spark-chasers' [NOT repair/maintenance or failure-investigation oriented].

Regards, Wil Taylor
o Trust - But Verify!
o We believe to be true what we prefer to be true. [Unknown]
o For those who believe, no proof is required; for those who cannot believe, no proof is possible. [variation,Stuart Chase]
o Unfortunately, in science what You 'believe' is irrelevant. ["Orion", Homebuiltairplanes.com forum]

WKT - I opened it; it's just matlab. I have FreeMat, which dies and tanks the application instead of reporting errors; not surprised since development of FreeMat seems to have also died.

clear all
clc
close all
pos=[0,0,0,0,0,0]';
posdesired=pos;
posseq=[pos];
e=[0;0;0;0;0;0];
II = e;

velocity=[0,0,0,0,0,0]';

tau=[0,0,0,0,0,0]';

% Kp=0.22*[1 0 0; 0 1 0; 0 0 1];
% Ki=0.063*[1 0 0; 0 1 0; 0 0 1];

Kp=-3*eye(6);
Ki=-2*eye(6);

% M=[1.1274 0 0; 0 1.1274 0; 0 0 1];
m=180;
Xudot=-30;
Yvdot=-110;
Zwdot=-80;
Ix=25;
Iy=29;
Iz=28;
Kpdot=-15;
Mqdot=-20;
Nrdot=-1;
M=[m-Xudot,0,0,0,0,0;
0,m-Yvdot,0,0,0,0;
0,0,m-Zwdot,0,0,0;
0,0,0,Ix-Kpdot,0,0;
0,0,0,0,Iy-Mqdot,0;
0,0,0,0,0,Iz-Nrdot
];

% D=[0.0414 0 0; 0 0.0414 0; 0 0 0.0568];

dif=[];

Ts=.01

posdseq=[];
vseq=[];
% for position estimation
for i=1:700
posold=pos;
vold=velocity;


% Rv=[cos(posold(3)) -sin(posold(3)) 0; sin(posold(3)) cos(posold(3)) 0; 0 0 1];
J1=[cos(posold(6))*cos(posold(5)) -sin(posold(6))*cos(posold(4))+cos(posold(6))*sin(posold(5))*sin(posold(4)) sin(posold(6))*sin(posold(4))+cos(posold(6))*cos(posold(4))*sin(posold(5));
sin(posold(6))*cos(posold(5)) cos(posold(6))*cos(posold(4))+sin(posold(4))*sin(posold(5))*sin(posold(6)) -cos(posold(6))*sin(posold(5))+sin(posold(5))*sin(posold(6))*cos(posold(4));
-sin(posold(5)) cos(posold(5))*sin(posold(4)) cos(posold(5))*cos(posold(4))];



J2=[1 sin(posold(4))*tan(posold(5)) cos(posold(4))*tan(posold(5));
0 cos(posold(4)) -sin(posold(4));
0 sin(posold(4))/cos(posold(5)) cos(posold(4))/cos(posold(5))];

% J2=[1 sin(90)*tan(90) cos(90)*tan(90);
% 0 cos(90) -sin(90);
% 0 sin(90)/cos(90) cos(90)/cos(90)];

Rv=[J1 zeros(3,3);zeros(3,3) J2];



%control law 1
e=[e posdesired-pos];

vref= Kp*e(:,end);
tau=Ki*(vref-vold);

% tau=Kp*(Rv'*Ki*e(:,end)-vold);


% II = II+(Ts/2)*(e(:,end-1)+e(:,end));
% tau=[tau Rv'*(Kp*e(:,end)+Ki*II)];

% Cv=[0 -1.1274*vold(3) 0;1.1274*vold(3) 0 0;0 0 0];
% Dv=D*diag(vold);

Cv=[0 -m*vold(6) m*vold(5) 0 -Zwdot Yvdot*vold(2);
m*vold(6) 0 -m*vold(4) Zwdot*vold(3) 0 -Xudot*vold(1);
-m*vold(5) m*vold(4) 0 -Yvdot*vold(2) Xudot*vold(1) 0;
0 -Zwdot*vold(3) Yvdot*vold(2) 0 (Iz-Nrdot)*vold(6) -(Iy-Mqdot)*vold(5);
Zwdot*vold(3) 0 -Xudot*vold(1) -(Iz-Nrdot)*vold(6) 0 (Ix-Kpdot)*vold(4);
-Yvdot*vold(2) Xudot*vold(1) 0 (Ix-Kpdot)*vold(4) -(Ix-Kpdot)*vold(4) 0];


Dv=[-200*abs(vold(1)),0,0,0,0,0;
0,-310*abs(vold(2)),0,0,0,0;
0,0,-250*abs(vold(3)),0,0,0;
0,0,0,-80*abs(vold(4)),0,0;
0,0,0,0,-80*abs(vold(5)),0;
0,0,0,0,0,-25*abs(vold(6))];

% gn=[0,0,0,0.04*180*9.8*cos(90)*sin(posold(4)),0.04*180*9.8*sin(90),0]';

pos=posold+Ts*Rv*vold;
velocity = vold - Ts*inv(M)*(Cv+Dv)*vold+Ts*inv(M)*tau(:,end);

if (i>=5 && i< 280)
posdesired=[2 0 0 0 0 0]';
elseif(i>=280 && i<500)
posdesired=[2 2 0 0 0 0]';

else
posdesired=[0 0 0 0 0 0]';

end

i

posseq=[posseq,double(pos)];
vseq=[vseq,double(velocity)];
posdseq=[posdseq,posdesired];
dif=[dif,posdesired-pos];

plot(posdseq(1,:),posdseq(2,:),'g--')
hold on
plot(posseq(1,:),posseq(2,:))
pause(10^-5)
end

does this feel like homework to you ?

another day in paradise, or is paradise one day closer ?

Really bad homework - a control system problem with a failed software implementation in Matlab that is being asked about in a forum that usually deals with neither one?

It seems best as an example warning to others of how not to do any software development.

FWIW it runs in Octave. Personally I'd have thought with a flowchart like that Simulink would have been the goto app. Anyway, have you checked that your AUV block works by itself? I'm not going to look at it, it is the sort of crap code I churn out as a hack.

Cheers

Greg Locock


New here? Try reading these, they might help FAQ731-376: Eng-Tips.com Forum Policies http://eng-tips.com/market.cfm?

Not enough gain, too much damping, on both the velocity signal and the direction signal.

Cheers

Greg Locock


New here? Try reading these, they might help FAQ731-376: Eng-Tips.com Forum Policies http://eng-tips.com/market.cfm?

"it is the sort of crap code I churn out as a hack." … greg, the self confessed hack ! (smile)

another day in paradise, or is paradise one day closer ?

Spar Web recommended redirect of the question to another [more appropriate] forum... but from what I can tell the OP has NOT done so... and has not logged-on since the initial posting.

This does not pass my sniff-check.

Regards, Wil Taylor
o Trust - But Verify!
o We believe to be true what we prefer to be true. [Unknown]
o For those who believe, no proof is required; for those who cannot believe, no proof is possible. [variation,Stuart Chase]
o Unfortunately, in science what You 'believe' is irrelevant. ["Orion", Homebuiltairplanes.com forum]

rb1957 - Unfortunately the motto of my life seems to be "If it is stupid but it works, it isn't stupid".

OK, let's close this one.

Cheers

Greg Locock


New here? Try reading these, they might help FAQ731-376: Eng-Tips.com Forum Policies http://eng-tips.com/market.cfm?