Udacity机器人软件工程师课程笔记（二十四) - 控制（其二） - PID优化，梯度下降算法，带噪声的PID控制

7.非理想情况

（1）积分饱和

到目前为止，我们一直使用的“理想”形式的PID控制器很少用于工业中。“时间常数”形式更为常见。

当前说明了理想形式的一些重大缺陷。

在一个经过良好调整的系统中，积分项能够通过使控制作用力与累积误差成比例来消除稳态误差。然而，这种操作模式会带来危险。设想，如果设定值突然大幅度改变但系统动力学相应缓慢。例如，如果当前房间的温度为50°F（10°C），然后将恒温器上的设定值移至75°F（24°C）。加热系统可能需要数十分钟来将温度驱动到所需的设定值。直到跟踪误差为零，积分项将继续增长（即“饱和”），故名积分饱和。
由于累积的误差和缓慢的系统动力学，即使达到设定点，控制器仍将继续运行熔炉（提高温度），从而导致较大的过冲。然后，系统将花费时间来“消除”累积的误差，然后再次达到所需的设定点。

可能导致积分饱和的另一种情况是由于执行器饱和。如果将设定点更改得足够远，则即使执行器以100％输出（即饱和）运行，系统也可能永远无法达到期望的目标。唯一的希望是将设定值提高到可以达到的水平。
已经设计了许多策略来解决积分饱和。一个简单的方法是在控制器输出完全饱和时停止对误差进行积分。

（2）噪音

大多数PID控制器实际上只是PI控制器， 那么微分增益可能带来哪些问题？

想象一下，四轴飞行器徘徊在所需的高度，但是由于有风，所以飞行器上下颠簸。

$$\frac{de}{dt}\approx \frac{e-e_{old}}{\Delta t}$$

此处显示了导数的有限差分形式的方程式，其中$\Delta t$是一个很小的数字，可能在毫秒或微秒的数量级，最终结果是扩大差异。

在实践中，这会在存在高频噪声的情况下造成严重的问题，因为控制器认为空气信号发生了很大变化，并且做出了巨大的控制努力。

虽然可以添加一个低通滤波器来消除一些高频噪声，但是这样做确实会降低微分控制的性能，因为它依赖于对空气变化率的响应。通常，需要大量的实验来平衡效果来获得令人满意的性能。

低通滤波器的一个简单实现是一阶递归滤波器，其中$\alpha$是平滑因子。$\alpha$越小，抑制的高频噪声就越多，这增加了控件设计者必须选择的另一个参数。

此外，递归滤波器还增加了滞后。即，它延迟了控制器向系统的输入，使系统更难控制。但是，由于此滤波器仅使用一个先前的输出值来计算当前值，因此延迟的影响通常是比较小的。

编写一个低通滤波器

import numpy as np
import matplotlib.pyplot as plt

f1 = 5  # Hz
w1 = 2*np.pi*f1  # 角频率
f2 = 50  # Hz
w2 = 2*np.pi*f2  # 角频率

# 创建一个时间点数组
N = 500
t = np.linspace(0, 5, N)

# 创建信号
pure = np.sin(w1*t)  # 纯正弦波
noise = np.sin(w2*t) + np.random.normal(0, 0.2, N)  # 正态分布随机噪声
# 0   = 正态分布的均值
# 0.2 = 正态分布的标准差
# N   = 数组中元素的数目
signal = pure + noise  # 嘈杂的正弦波

# 递归低通滤波器
alpha = 0.01  # 接近零意味着更强的平滑效果

filtered = np.zeros(N)  # 初始化数组以存储值
for i in range(N):
    if i > 1:
        # 用低通文件的代码填写过滤的[i]变量
        filtered[i] = alpha * (signal[i] - filtered[i-1]) + filtered[i-1]


fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(t, pure)
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Amplitude, (m)')
ax1.set_title('5 Hz, Pure Signal')

ax2 = fig.add_subplot(312)
ax2.plot(t, signal)
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Amplitude, (m)')
ax2.set_title('Noisy Signal')

ax3 = fig.add_subplot(313)
ax3.plot(t, filtered)
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Amplitude, (m)')
ax3.set_title('Filtered Signal')

plt.tight_layout()
plt.show()

输出如下：

8.控制设计目标和准则

（1）控制器的主要目标

1. 稳定性。首先，控制器应提高系统稳定性。实际上，有很多方法可以定义和表征稳定性。适用于线性系统的一个常见定义是缩写BIBO。BIBO代表有界输入，有界输出，它表示只要输入低于某个值，那么输出就可以保证也低于某个值。更为严格的定义称为渐近稳定性。如果给出有限的输入或初始条件，则渐近稳定的系统可以保证收敛到固定的有限值（例如，落下的球最终将落在地面上）。显然，这对于现实世界的系统而言是非常理想的属性。

2. 跟踪。跟踪性能也很重要。跟踪是指控制器跟踪或保持参考输入信号的能力。参考点和实际输出之间的差异是跟踪误差。跟踪任务的一个示例是建筑物中的供暖和空调系统。可以通过恒温器设置所需的温度，该恒温器具有集成的传感器以测量当前温度，如果出现跟踪错误，恒温器将激活加热器或空调以将温度推向设定值。

3. 鲁棒性。正如统计学家乔治·博克斯（George Box）曾经写道：“所有模型都是错误的，但有些模型是有用的。”理想情况下，控制器将不会依赖于参数非常精确的非常详细的数学模型。

4. 干扰抑制。噪声和其他测量误差会破坏所有实际系统。虽然尝试并限制不想要的干扰很重要，但设计可容忍干扰的控制器也很重要。

5. 最优性。尽管不是每个控制器都需要，但在许多情况下，优化系统的某些属性非常重要，例如，最小化控制工作，在给定时间间隔内最大化行进距离或最小化跟踪误差。最佳控制需要系统的数学模型，设置控制输入上限和下限的约束方程式以及成本函数。梯度下降 /上升算法是优化成本函数的流行首选方法。

（2）控制器设计流程

成功的控制器依赖于系统的，多步骤的设计过程。大致而言，步骤为：

1. 确定系统的性能规格（例如，最大跟踪误差，超调百分比，建立时间等）
2. 如果可能，开发一个简单的系统数学模型 选择传感器和控制器类型
3. 设计控制器符合性能规格
4. 如果可能，请使用仿真来测试控制器性能
5. 选择硬件/软件来实施控制器
6. 在线调音控制器

控制器的目标是促进稳定性，跟踪性，鲁棒性和抑制干扰，但是现在的问题是，“控制信号$u(t)$应该如何起作用才能实现这些目标？”显然，为了最大程度地减小跟踪误差，控制信号应该是误差的一部分，但是它到底是什么样的呢？

可以断定，$u(t)$应该与误差的大小有关。换句话说，较小的误差应产生较小的控制输入，反之亦然。误差$e(t)$也很重要最终应该变为零，或者至少可以接受的程度。

另一个相当直观的属性是控制输入的变化不应太快。想象一下，如果巡航控制器只能通过完全打开或完全踩下油门和刹车来调节速度，那么旅程将是多么令人不愉快。快速或不稳定的输入至少有可能在执行器上造成不必要的磨损，并可能降低稳定性。最后，如果控制信号不依赖于了解特定的模型参数（例如汽车的质量，空气阻力系数或地形坡度），那将是有益的。

9.调优策略

到目前为止，重点是了解比例误差、积分误差和导数误差的主要影响。现在把我们的注意力转移到更系统的方法来选择PID增益。

多年来，提出了许多不同的参数调优规则和指导原则。一些最古老和仍然被广泛引用的技术是Ziegler-Nichols (ZN)方法。

齐格勒和尼科尔斯发表了与PID控制调谐相关的最有影响力的论文之一，这篇论文的题目是《自动控制器的最佳设置》。
从准确性和简洁性的角度来看，研究自动控制的纯数学方法无疑是最理想的方法。

然而，不幸的是，控制的数学涉及如此令人困惑的指数函数和三角函数的组合，以至于一般的工程师没有足够的时间来钻研这些函数来解决当前的问题。

为了帮助实践控制工程师，齐格勒和尼克尔斯发表了两个经验推导的方法，可以用于PID调整过程，至少作为一个起点。这些规则很容易遵循，涉及到一个系统的过程，并且对于许多系统都足够有效，这一事实无疑对这些方法的流行起了很大作用。

尽管Z-N方法不再被认为是“行业标准”，但是有必要熟悉一下它们，因为它们仍然经常遇到，而且许多现代调优指南是原始调优指南的变体。对调优方法的一些批评是:
(1)建议的增益基于来自系统对测试输入的响应的信息太少，
(2)容易产生太多的超调，
(3)跟踪性能差。

ZN方法1

假设:

1. 该装置对阶跃输入有s形响应。(或称，一阶加死区时间模型)，
2. 是开环稳定的。
   
   ZN方法1的目标是实现良好的干扰抑制，他们将其定义为具有“四分之一衰减比”的系统响应(即，第二次超调峰值的振幅除以第一次超调峰值的振幅= 1/4)。

ZN方法2

假设:

1. 该系统是一个高阶系统，在闭环系统中，只采用比例控制就可以使系统达到稳态振荡

第二种方法的目标是实现具有25%超调的闭环阶跃响应。
这两种方法都假设PID控制器具有以下形式，
$$u(t)=K_p(e(t)+\frac{1}{{\tau }_i}{\int }_0^{t}e(\tau )d\tau +{\tau }_d\frac{de(t)}{dt}$$

其中
$${\tau }_i=\frac{K_P}{K_I}{\tau }_d=\frac{K_D}{K_P}$$

简单的手动调整

在许多情况下，简单的手动调整可以提供相当好的性能。可以通过以下方式开始：

1. 将Kp设置为小且Ki = Kd = 0，
2. 缓慢增加Kp以减少上升时间，
3. 缓慢增加Kd以减少过冲和稳定时间。

通过“手动”调整增益，系统响应越快，稳定性越差。反之亦然。诀窍是要在两者之间找到平衡。但是，使用上述方法找到近似解通常可以作为更系统的调整方法的起点，我们将在下面讨论的梯度下降算法。

10. 梯度下降算法（“Twiddle”）

一种更常的方法是使用梯度下降算法，有时也称为“Twiddle”。前提是要从三个增益的初始猜测向量开始。通常建议为P设置一个小的非零值，为I和D设置一个零。然后，分别对每个收益进行小幅更改，然后测试成本函数是否降低。如果是这样，将沿相同方向不断更改参数，否则尝试沿相反方向调整参数。如果增益值的增加或减少都不会降低成本函数，则可以减小增益增量的大小并重复。整个循环应继续进行，直到增量大小降至某个阈值以下。
使用python实现梯度下降算法：
Twiddle.py:

import numpy as np
import matplotlib.pyplot as plt


def ydot(y, t, dt, r, p):
    ''' ydot 函数返回四旋翼的下一个状态，作为当前状态和控制输入的函数。
    参数:
    -----------
    y = 增广状态向量(7个元素列表)
    t = 时间, (sec)
    dt = 时间间隔, (sec)
    r = 控制器设定值, (m)
    p = PID增益列表 = [kp, ki, kd]
    '''
    # 模型状态
    y1 = y[0]  # 高度, (m)
    y2 = y[1]  # 速度, (m/s)
    y3 = y[2]  # 上次误差, (m)
    y4 = y[3]  # 误差的积分, (m*sec)
    y5 = y[4]  # 比例作用系数, (m/s/s)
    y6 = y[5]  # 积分作用系数, (m/s/s)
    y7 = y[6]  # 微分作用系数, (m/s/s)

    # 当前的位置误差, (m)
    e = r - y1

    # 作用力 (i.e., 发动机推力)
    Kp = p[0]
    Ki = p[1]
    Kd = p[2]
    up = Kp * e  # 比例控制效果
    ui = Ki * (e * dt + y4)  # 积分控制效果
    ud = Kd * (e - y3) / dt  # 微分控制效果
    u = up + ui + ud  # 完整控制效果

    # 执行器限制
    if u > umax:
        u = umax  # 最大可能的输出
    elif u < 0:
        u = 0  # 四轴发动机不能反向运行

    # 状态控制
    # 如果高度 = 0
    if (y1 <= 0.):
        # 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。
        if u <= np.absolute(g * m / c):
            y1dot = 0.
            y2dot = 0.
        else:
            # 否则，如果u>重力，则四轴加速向上
            y1dot = y2
            y2dot = g + c / m * u - 0.75 * y2
    else:  # 或者四轴飞行器已经升空
        y1dot = y2
        y2dot = g + c / m * u - 0.75 * y2

    # 计算新的状态
    y1 += y1dot * dt
    y2 += y2dot * dt
    y3 = e
    y4 += e * dt
    y5 = up
    y6 = ui
    y7 = ud
    return [y1, y2, y3, y4, y5, y6, y7]


def hover(p):
    # 该函数模拟四旋翼在高度设定值发生阶跃变化时的运动。状态作为时间的函数存储在np.array中。
    # 初始条件
    y = [0, 0, 0, 0, 0, 0, 0]
    # y[0] = 初始高度, (m)
    # y[1] = 初始速度, (m/s)
    # y[2] = 当前误差, (m)
    # y[3] = 积分误差, (m*sec)
    # y[4] = 比例控制
    # y[5] = 积分控制
    # y[6] = 微分控制
    # 初始化数组以存储值
    soln = np.zeros((len(time), len(y)))

    j = 0
    for t in time:
        y = ydot(y, t, dt, r, p)
        soln[j, :] = y
        j += 1
    return soln


def cost_fun(soln):

    y0 = soln[:, 0]  # 高度
    rise_time_index = np.argmax(y0 > r)
    RT = time[rise_time_index]

    OS = (np.max(y0) - r) / r * 100
    OS_pen = OS
    if OS < 0:
        OS = 0
        OS_pen = 100
    # return OS_pen**2 + RT**2
    return OS_pen ** 2


def twiddle(tol=0.05):
    # 初始增益估计
    p = [2, 2, 2]
    dp = [0.10, 0.10, 0.10]
    soln = hover(p)
    best_cost = cost_fun(soln)

    # Twiddle算法
    it = 0
    while sum(dp) > tol:
        print("Iteration {}, best error = {}".format(it, best_cost))
        for i in range(len(p)):
            p[i] += dp[i]
            # 不能有负收益
            if p[i] < 0:
                p[i] = 0
            soln = hover(p)
            cost = cost_fun(soln)

            if cost < best_cost:
                best_cost = cost
                dp[i] *= 1.1
            else:
                p[i] -= 2 * dp[i]
                # 不能有负收益
                if p[i] < 0:
                    p[i] = 0
                soln = hover(p)
                cost = cost_fun(soln)

                if cost < best_cost:
                    best_cost = cost
                    dp[i] *= 1.1
                else:
                    p[i] += dp[i]
                    # 不能有负收益
                    if p[i] < 0:
                        p[i] = 0
                    dp[i] *= 0.9
        it += 1
    return p


# 仿真参数
N = 500  # 模拟点的数目
t0 = 0  # 开始时间, (sec)
tf = 30  # 结束时间, (sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)
r = 10  # 高度设置值, (m)

# 模型参数
g = -9.81  # 重力, m/s/s
m = 1.54  # 四轴飞行器重量, kg
c = 10.  # 机电传动常数
umax = 5.0  #  最大控制器输出, (m/s/s)

# 使用梯度下降算法
p = twiddle()
print(p)
soln = hover(p)

# 绘制结果
SP = np.ones_like(time) * r  # 高度设置点
UMAX = np.ones_like(time) * umax
fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
ax2 = fig.add_subplot(312)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')

ax3 = fig.add_subplot(313)
ax3.plot(time, soln[:, 4], '-r', label="Up")
ax3.plot(time, soln[:, 5], '-b', label="Ui")
ax3.plot(time, UMAX, '--k', label="Umax")
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Cont. Effort, (m/s/s)')
ax3.legend(loc='upper left')
plt.tight_layout()
plt.show()

控制台输出如下：

Iteration 0, best error = 1512.7482851742875
Iteration 1, best error = 1328.603631778417
Iteration 2, best error = 1144.8455886580318
Iteration 3, best error = 961.5430897948886
Iteration 4, best error = 783.1182469340695
Iteration 5, best error = 612.6172237006944
Iteration 6, best error = 452.43794261727203
Iteration 7, best error = 305.7984472011611
Iteration 8, best error = 175.35951967585999
Iteration 9, best error = 76.21076989239549
Iteration 10, best error = 18.218737669978054
Iteration 11, best error = 18.218737669978054
Iteration 12, best error = 18.160375187752962
Iteration 13, best error = 0.04143894039440686
Iteration 14, best error = 0.04143894039440686
Iteration 15, best error = 0.04143894039440686
Iteration 16, best error = 0.04143894039440686
Iteration 17, best error = 0.04143894039440686
Iteration 18, best error = 0.04143894039440686
Iteration 19, best error = 0.04143894039440686
Iteration 20, best error = 0.04143894039440686
Iteration 21, best error = 0.04143894039440686
Iteration 22, best error = 0.04143894039440686
Iteration 23, best error = 0.04143894039440686
Iteration 24, best error = 0.04143894039440686
Iteration 25, best error = 0.04143894039440686
Iteration 26, best error = 0.04143894039440686
Iteration 27, best error = 0.04143894039440686
Iteration 28, best error = 0.04143894039440686
Iteration 29, best error = 0.04143894039440686
Iteration 30, best error = 0.001022595671581749
Iteration 31, best error = 0.0001246204006485597
Iteration 32, best error = 0.0001246204006485597
Iteration 33, best error = 0.0001246204006485597
Iteration 34, best error = 0.0001246204006485597
Iteration 35, best error = 0.0001246204006485597
Iteration 36, best error = 0.0001246204006485597
[3.625276313216848, 0.1882787020485744, 1.7111168918908104]

plt输出如下：

11.带噪声的PID控制

考虑实际情况，我们加上噪音来使用前面介绍的几种控制方法进行优化。

程序如下
hover_plot.py

import numpy as np
import matplotlib.pyplot as plt
from PID_controller import PID_Controller
from quad1d_eom import ydot

# 仿真参数
N = 600  # 仿真的点数
t0 = 0  # 起始时间(sec)
tf = 45  # 结束时间(sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0]  # delta t, (sec)

# 核心仿真代码
# 初始条件 (i.e., 初始状态向量)
y = [0, 0]
# y[0] = initial altitude, (m)
# y[1] = initial speed, (m/s)

# 初始化数组以存储值
soln = np.zeros((len(time), len(y)))

# 创建Open_Controller类的实例
controller = PID_Controller()

# 设置kp,ki,kd的值
kp = 1.5
kd = 0.75
ki = 0.12
# 设置最大控制器输出，单位（N）
umax = 5.0

# 设置微分平滑过滤器系数
alpha = 1.0

# 设置控制器的Kp值
controller.setKP(kp)

# 设置控制器的Ki值
controller.setKI(ki)

# 设置控制器的Kd值
controller.setKD(kd)


# 设定高度目标
r = 10.0  # meters
controller.setTarget(r)

# 模拟四轴运动
j = 0  # 计数器
for t in time:
    # 评估下一个时间点的状态
    y = ydot(y, t, controller)
    # 储存结果
    soln[j, :] = y
    j += 1

##################################################################################
# 绘制结果
# 图一:四轴飞行器高度随时间的变化的曲线
SP = np.ones_like(time)*r  # 高度设置点
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')

# 图二:这是四轴直升机的速度随时间的变化曲线
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
ax2.set_xlabel('Time, (sec)')
ax2.set_ylabel('Speed, (m/s)')
plt.tight_layout()
plt.show()

fig2 = plt.figure()
ax3 = fig2.add_subplot(111)
ax3.plot(time, controller.u_p, label='u_p', linewidth=3, color='red')
ax3.plot(time, controller.u_d, label='u_d', linewidth=3, color='blue')
ax3.plot(time, controller.u_i, label='u_i', linewidth=3, color='green')
ax3.set_xlabel('Time, (sec)')
ax3.set_ylabel('Control Effort')
h, l = ax3.get_legend_handles_labels()
ax3.legend(h, l)
plt.tight_layout()
plt.show()

##################
y0 = soln[:, 0]
rise_time_index = np.argmax(y0 > r)
RT = time[rise_time_index]
print("The rise time is {0:.3f} seconds".format(RT))

OS = (np.max(y0) - r)/r*100
if OS < 0:
    OS = 0
print("The percent overshoot is {0:.1f}%".format(OS))

print("The steady state offset at 45 seconds is {0:.3f} meters".format(abs(soln[-1, 0]-r)))

pid_controller

class PID_Controller:
    def __init__(self, kp=0.0, ki=0.0, kd=0.0, max_windup=20,
                 start_time=0, alpha=1., u_bounds=[float('-inf'), float('inf')]):
        self.kp_ = float(kp)
        self.kd_ = float(kd)
        self.ki_ = float(ki)

        # 设置最大收束
        self.max_windup_ = float(max_windup)

        # 设置alpha值
        self.alpha = float(alpha)

        # 设置作用力饱和限制
        self.umin = u_bounds[0]
        self.umax = u_bounds[1]

        # 存储相关数据
        self.last_timestamp_ = 0.0
        self.set_point_ = 0.0
        self.start_time_ = start_time
        self.error_sum_ = 0.0
        self.error_past_ = 0.0

        # 控制效果记录
        self.u_p = [0]
        self.u_d = [0]
        self.u_i = [0]

    # 添加一个reset函数来清除类变量
    def reset(self):
        self.set_point_ = 0.0
        self.kp_ = 0.0
        self.ki_ = 0.0
        self.kd_ = 0.0
        self.error_sum_ = 0.0
        self.last_timestamp_ = 0.0
        self.error_past_ = 0
        self.error_past_past_ = 0
        self.last_windup_ = 0.0

    def setTarget(self, target):
        # 设置飞行高度
        self.set_point_ = float(target)

    def setKP(self, kp):
        # 使用提供的变量设置内部kp_值
        self.kp_ = float(kp)

    def setKD(self, kd):
        # 使用提供的变量设置内部kd_值
        self.kd_ = float(kd)

    def setKI(self, ki):
        # 使用提供的变量设置内部ki_值
        self.ki_ = float(ki)

    def setMaxWindup(self, max_windup):
        # 创建函数来设置max_windup_
        self.max_windup_ = int(max_windup)

    def update(self, measured_value, timestamp):
        delta_time = timestamp - self.last_timestamp_
        if delta_time == 0:
            # Delta time is zero
            return 0

        # 计算误差
        error = self.set_point_ - measured_value

        # 设置 last_timestamp_
        self.last_timestamp_ = timestamp

        # 找到 error_sum_
        self.error_sum_ += error

        # 计算相对误差delta_error
        delta_error = error - self.error_past_

        # 存储本次error，作为下次error_past_使用
        self.error_past_ = error

        # 设置error_sum_范围
        if self.error_sum_ > self.max_windup_:
            self.error_sum_ = self.max_windup_
        elif self.error_sum_ < -self.max_windup_:
            self.error_sum_ = -self.max_windup_

        # 计算比例误差
        p = self.kp_ * error

        # 计算积分误差
        i = self.ki_ * self.error_sum_
        # 计算微分误差，包括误差平滑
        d = self.kd_ * (self.alpha * delta_error / delta_time + (1 - self.alpha) * self.error_past_ / delta_time)

        # 计算总控制效果
        u = p + d + i

        # 设置执行器饱和设置
        if u > self.umax:
            u = self.umax
        elif u < self.umin:
            u = self.umin

        # 记录控制效果
        self.u_p.append(p)
        self.u_d.append(d)
        self.u_i.append(i)

        return u

quad1d_eom.py

import numpy as np


def ydot(y, t, controller):
    '''
    返回下一个时间步长的状态向量

    参数:
    ----------
    y(k) = 状态向量, a length 2 list
      = [高度, 速度]
    t = time, (sec)
    pid = PID Controller类的实例

    return
    -------
    y(k+1) = [y[0], y[1]] = y(k) + ydot*dt
    '''

    # 模型状态
    y0 = y[0]  # 高度, (m)
    y1 = y[1]  # 速度, (m/s)

    # 模型参数
    g = -9.81  # 重力, m/s/s
    m =  1.54  # 四轴飞行器重量, kg
    c =  10.0  # 机电系统的传输常数

    # 时间步长, (sec)
    dt = t - controller.last_timestamp_
    # 控制效果
    u = controller.update(measured_value=y0, timestamp=t)

    # State derivatives
    if (y0 <= 0.):
        # 如果控制输入，u <=重力，飞行器在地面上保持静止，这样可以防止四旋翼在推力太小时“坠落”地面。
        if u <= np.absolute(g*m/c):
            y0dot = 0.
            y1dot = 0.
        else:  # 否则，如果u>重力，则四轴加速向上
            y0dot = y1
            y1dot = g + c/m*u - 0.75*y1
    else:  # 或者四轴飞行器已经升空
        # y0dot为速度大小
        y0dot = y1
        # y1dot为加速度大小，其中0.75*y1为阻力大小
        y1dot = g + c/m*u - 0.75*y1


    y0 += y0dot*dt
    y1 += y1dot*dt

    # 增加一些高度上的噪声
    sigma = 0.1
    y0 = np.random.normal(y0, sigma, 1)

    return [y0, y1]