function?[taueplerror]?=?control（hrefestep）
ep0=ep;
%%PID?CONTROLLER?FOR?DYNAMIC?POSITIONING?VESSEL
Kp=[2.0756e4?????????0????????0;
???????????0??2.0564e4????????0;
???????????0?????????0?1.3166e8];
Kd=Kp*30;

Ki=Kp*（0.1^3）;
%ROTATION?MATRIX
psi=est（3）;
invR=eye（3）/[cos（psi）?-sin（psi）?0;
?????????????sin（psi）??cos（psi）?0;
?????????????0?????????0????????1];
pos=[est（4）?est（5）?est（6）]‘;
ep?=ref-pos;
%wrap?to?-pi?pi
num=floor（ep（3）/（2*pi）+0.5）;
ep（3）=ep（3）-num*2*pi;
nu=[est（1）?est（2）?est（3）]‘;
%gain?in?body
gain=Kp*（invR*ep）-Kd*（inv*nu）-Ki*h*0.5*（invR*（ep+ep0））;
tau=（gain）‘;
epl=ep;
error=ep;

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件?????????627??2020-10-01?09:23??control.m