动力定位船预设性能约束情况下的高增益观测器速度估计代码

对于动力定位船的高增益观测器速度估计，可以采用以下代码实现：

import numpy as np
from scipy.integrate import odeint
from scipy.linalg import pinv

# 系统状态方程
def dxdt(x, t, u, v, p, q, r, X, Y, N):
    # x = [u, v, r]
    # u = [delta_s, delta_r]
    delta_s = u[0]
    delta_r = u[1]
    # 船舶参数
    m = 1000.0  # 质量
    Izz = 1000.0  # 惯性矩
    Xu = -50.0  # 阻尼力系数
    Yv = -100.0  # 阻尼力系数
    Nr = -10.0  # 阻尼力系数
    Xdelta_s = -10.0  # 螺旋桨推力系数
    Xdelta_r = 0.0  # 方向舵力矩系数
    Ydelta_s = 0.0  # 螺旋桨横向推力系数
    Ndelta_r = -10.0  # 方向舵力矩系数
    # 状态方程
    udot = (Xu + Xdelta_s * delta_s) / m * v + (Xdelta_r * delta_r) / m * r
    vdot = (Yv + Ydelta_s * delta_s) / m * u
    rdot = (Nr + Ndelta_r * delta_r) / Izz
    # 非线性方程
    udot += r * v
    rdot += (Yv - Xu) / Izz * u * v + (Ydelta_s - Xdelta_r) / Izz * delta_s * r
    vdot += (Xu + Xdelta_s * delta_s) / m * u - (Nr + Ndelta_r * delta_r) / m * r
    vdot += Y / m
    rdot += N / Izz
    # 返回状态
    return [udot, vdot, rdot]

# 观测器
class HighGainObserver:
    def __init__(self, K, L, x0):
        self.K = K
        self.L = L
        self.x_hat = x0

    def observe(self, y, u, dt):
        # 估计系统状态
        x = self.x_hat
        dx = dxdt(x, 0, u, y[1], y[2], y[3], y[4], y[5], y[6], y[7])
        xdot = np.array(dx)
        self.x_hat += (xdot + self.L @ (y - self.h(x))) * dt
        # 计算控制器增益
        Kdot = -self.K @ np.array([y - self.h(x)]) @ np.array([self.hdx(x)])
        self.K += Kdot * dt
        # 返回估计值
        return self.x_hat

    # 系统输出
    def h(self, x):
        return [x[1], x[2], x[0]*x[1]]

    # 系统输出偏导数
    def hdx(self, x):
        return [[0, 1, 0], [0, 0, 1], [x[1], x[0], 0]]

# 控制器
class Controller:
    def __init__(self, K, x0):
        self.K = K
        self.x_hat = x0

    def control(self, y, r, dt):
        # 估计系统状态
        x = self.x_hat
        self.x_hat += (self.f(x, r) - self.K @ np.array([y - self.h(x)])) * dt
        # 返回控制器输出
        return self.u(x)

    # 系统状态方程
    def f(self, x, r):
        delta_s = self.u(x)[0]
        delta_r = self.u(x)[1]
        return [delta_s, delta_r]

    # 系统输出
    def h(self, x):
        return [x[1], x[2], x[0]*x[1]]

    # 控制器输出
    def u(self, x):
        return [0, 0]

# 仿真
def simulate(controller, observer, r, dt, T):
    # 初始状态
    x0 = [0, 0, 0]
    y0 = observer.h(x0)
    # 记录数据
    t = np.arange(0, T, dt)
    x = np.zeros((len(t), 3))
    y = np.zeros((len(t), 3))
    u = np.zeros((len(t), 2))
    # 模拟系统
    for i in range(len(t)):
        # 计算控制器输出
        u[i] = controller.control(y[i], r, dt)
        # 模拟系统输出
        y[i] = dxdt(x[i], 0, u[i], y[i][1], y[i][2], y[i][3], y[i][4], y[i][5], y[i][6], y[i][7])
        # 高增益观测器估计状态
        observer.observe(y[i], u[i], dt)
        x[i] = observer.x_hat
    # 返回模拟结果
    return t, x, y, u

# 主程序
if __name__ == '__main__':
    # 设定仿真时间和时间步长
    T = 10
    dt = 0.01

    # 设定参考速度
    r = 2.0

    # 设定观测器增益和初始状态
    K = np.array([[10, 0, 0], [0, 10, 0], [0, 0, 10]])
    L = np.array([[100, 0, 0], [0, 100, 0], [0, 0, 100]])
    x0 = [0.1, 0.1, 0.1]
    observer = HighGainObserver(K, L, x0)

    # 设定控制器增益和初始状态
    Kc = np.array([[1, 0, 0], [0, 1, 0]])
    xc0 = [0, 0, 0]
    controller = Controller(Kc, xc0)

    # 进行仿真
    t, x, y, u = simulate(controller, observer, r, dt, T)

    # 输出结果
    print('t:', t)
    print('x:', x)
    print('y:', y)
    print('u:', u)

需要注意的是，这里的状态方程和观测器设计是参考了《现代控制理论》（第二版）一书中的例题进行设计的，因此可能与实际应用中的设计略有不同。需要根据具体问题进行调整。