Автоматическое дифференцирование

using ControlSystemsBase, ForwardDiff

"An example of nonlinear dynamics"
function f(x, u)
    x1, x2 = x
    u1, u2 = u
    [x2; u1*x1 + u2*x2]
end

x0 = [1.0, 0.0] # Рабочая точка, вокруг которой выполняется линеаризация
u0 = [0.0, 1.0]

A = ForwardDiff.jacobian(x -> f(x, u0), x0)
B = ForwardDiff.jacobian(u -> f(x0, u), u0)

"An example of a nonlinear output (measurement) function"
function g(x, u)
    y = [x[1] + 0.1x[1]*u[2]; x[2]]
end

C = ForwardDiff.jacobian(x -> g(x, u0), x0)
D = ForwardDiff.jacobian(u -> g(x0, u), u0)

linear_sys = ss(A, B, C, D)

StateSpace{Continuous, Float64}
A =
 0.0  1.0
 0.0  1.0
B =
 0.0  0.0
 1.0  0.0
C =
 1.1  0.0
 0.0  1.0
D =
 0.0  0.1
 0.0  0.0

Continuous-time state-space model

using ControlSystemsBase, ForwardDiff, Plots

P = DemoSystems.double_mass_model()

bodeplot(P, title="Bode plot of Double-mass system \$P(s)\$")

Ω = exp10.(-2:0.03:3)
kp,ki,kd,Tf =  1, 0.1, 0.1, 0.01 # параметры регулятора

C  = pid(kp, ki, kd; Tf, form=:parallel, state_space=true) # Построение ПИД-регулятора с фильтром
G  = feedback(P*C) # Замкнутая система
S  = 1/(1 + P*C)   # Функция чувствительности
Gd = c2d(G, 0.1)   # Дискретизация системы
res = step(Gd,15)  # Ступенчатый отклик

mag = bodev(S, Ω)[1]
plot(res, title="Time response", layout = (1,3), legend=:bottomright)
plot!(Ω, mag, title="Sensitivity function", xscale=:log10, yscale=:log10, subplot=2, legend=:bottomright, ylims=(3e-2, Inf))
Ms, _ = hinfnorm(S)
hline!([Ms], l=(:black, :dash), subplot=2, lab="\$M_S = \$ $(round(Ms, digits=3))", sp=2)
nyquistplot!(P*C, Ω, sp=3, ylims=(-2.1,1.1), xlims=(-2.1,1.2), size=(1200,400))

using Optimization, Statistics, LinearAlgebra
using Ipopt, OptimizationMOI; MOI = OptimizationMOI.MOI

function plot_optimized(P, params, res, systems)
    fig = plot(layout=(1,3), size=(1200,400), bottommargin=2Plots.mm)
    for (i,params) = enumerate((params, res))
        ls = (i == 1 ? :dash : :solid)
        lab = (i==1 ? "Initial" : "Optimized")
        C, G = systems(params, P)
		r1, r2 = sim(G)
        mag = reshape(bode(G, Ω)[1], 4, :)'[:, [1, 2, 4]]
        plot!([r1, r2]; title="Time response", subplot=1,
            lab = lab .* [" \$r → e\$" " \$d → e\$"], legend=:bottomright, ls,
            fillalpha=0.05, linealpha=0.8, seriestype=:path, c=[1 3])
        plot!(Ω, mag; title="Sensitivity functions \$S(s), CS(s), T(s)\$",
            xscale=:log10, yscale=:log10, subplot=2, lab, ls,
            legend=:bottomright, fillalpha=0.05, linealpha=0.8, c=[1 2 3], linewidth=i)
        nyquistplot!(P*C, Ω; Ms_circles=Msc, sp=3, ylims=(-2.1,1.1), xlims=(-2.1,1.2), lab, seriescolor=i, ls)
    end
    hline!([Msc], l=:dashdot, c=1, subplot=2, lab="Constraint", ylims=(9e-2, Inf))
    fig
end

"A helper function that creates a PID controller and closed-loop transfer functions"
function systemspid(params, P)
    kp,ki,kd,Tf = params # Оптимизируем параметры в
    C    = pid(kp, ki, kd; form=:parallel, Tf, state_space=true)
    G    = extended_gangoffour(P, C) # [S PS; CS T]
    C, G
end

"A helper function that simulates the closed-loop system"
function sim(G)
    Gd = c2d(G, 0.1, :tustin)   # Дискретизация системы
    res1 = step(Gd[1, 1], 0:0.1:15) # Моделирование S
    res2 = step(Gd[1, 2], 0:0.1:15) # Моделирование PS
    res1, res2
end

"The cost function to optimize"
function cost(params::AbstractVector{T}, (P, systems)) where T
    CSweight = 0.001 # Снижение усиления шума
    C, G = systems(params, P)
    res1, res2 = sim(G)
    R, _ = bodev(G[2, 1], Ω; unwrap=false)
    CS = sum(R .*= Ω)               # частотно-взвешенная чувствительность к шуму
    perf = mean(abs2, res1.y .*= res1.t') + mean(abs2, res2.y .*= res2.t')
    return perf + CSweight * CS # Объединим все цели
end

"The sensitivity constraint to enforce robustness"
function constraints(res, params::AbstractVector{T}, (P, systems)) where T
    C, G = systems(params, P)
    S, _ = bodev(G[1, 1], Ω; unwrap=false)
    res .= maximum(S) # максимальная чувствительность
    nothing
end

Msc = 1.3        # Ограничение для Ms

params  = [kp, ki, kd, 0.01] # Начальное предположение для параметров

solver = Ipopt.Optimizer()
MOI.set(solver, MOI.RawOptimizerAttribute("print_level"), 0)
MOI.set(solver, MOI.RawOptimizerAttribute("max_iter"), 200)
MOI.set(solver, MOI.RawOptimizerAttribute("acceptable_tol"), 1e-1)
MOI.set(solver, MOI.RawOptimizerAttribute("acceptable_constr_viol_tol"), 1e-2)
MOI.set(solver, MOI.RawOptimizerAttribute("acceptable_iter"), 5)
MOI.set(solver, MOI.RawOptimizerAttribute("hessian_approximation"), "limited-memory")

fopt = OptimizationFunction(cost, Optimization.AutoForwardDiff(); cons=constraints)

prob = OptimizationProblem(fopt, params, (P, systemspid);
    lb    = fill(-10.0, length(params)),
    ub    = fill(10.0, length(params)),
    ucons = fill(Msc, 1),
    lcons = fill(-Inf, 1),
)

res = solve(prob, solver)
plot_optimized(P, params, res.u, systemspid)

using ImplicitDifferentiation, ComponentArrays # Оба эти пакета необходимы для загрузки правил неявного дифференцирования.

function triangular(x)
    m = length(x)
    n = round(Int, sqrt(2m-1))
    T = zeros(eltype(x), n, n)
    k = 1
    for i = 1:n, j = i:n
        T[i,j] = x[k]
        k += 1
    end
    T
end
invtriangular(T) = [T[i,j] for i = 1:size(T,1) for j = i:size(T,1)]

function systemslqr(params::AbstractVector{T}, P) where T
    n2 = length(params) ÷ 2
    Qchol = triangular(params[1:n2])
    Rchol = triangular(params[n2+1:2n2])
    Q = Qchol'Qchol
    R = Rchol'Rchol
    L = lqr(P, Q, one(T)*I(1)) # Важно, чтобы последняя матрица имела правильный тип.
    K = kalman(P, R, one(T)*I(1))
    C = observer_controller(P, L, K)
    G = extended_gangoffour(P, C) # [S PS; CS T]
    C, G
end

Q0 = diagm([1.0, 1, 1, 1]) # Начальное предположение штрафа по состоянию ЛКР
R0 = diagm([1.0, 1, 1, 1]) # Начальное предположение ковариации состояния Калмана
params2 = [invtriangular(cholesky(Q0).U); invtriangular(cholesky(R0).U)]

prob2 = OptimizationProblem(fopt, params2, (P, systemslqr);
    lb    = fill(-10.0, length(params2)),
    ub    = fill(10.0, length(params2)),
    ucons = fill(Msc, 1),
    lcons = fill(-Inf, 1),
)

res2 = solve(prob2, solver)
plot_optimized(P, params2, res2.u, systemslqr)