matlab编写的船舶动力定位PID控制函数

matlab编写的船舶动力定位PID控制函数，适于DP建模动态模拟分析

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;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件         627  2020-10-01 09:23  control.m