From fb013c100ec41854b6340467cbab06b1da7d0bf2 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 11:01:29 +0200
Subject: [PATCH 01/15] Good system simulation

---
 mwes/mwe_05.jl | 87 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 87 insertions(+)
 create mode 100644 mwes/mwe_05.jl

diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
new file mode 100644
index 0000000..33f6b63
--- /dev/null
+++ b/mwes/mwe_05.jl
@@ -0,0 +1,87 @@
+# Linearize the system consisting of the winch and kite
+# input: set_speed
+# output: speed
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff
+import FiniteDiff: finite_difference_jacobian
+
+if isfile("data/system_tuned.yaml")
+    set = load_settings("system_tuned.yaml")
+else
+    set = load_settings("system.yaml")
+end
+wcs = WCSettings(dt=0.02)
+update(wcs)
+wcs.test = true
+
+winch = Winch(wcs, set)
+
+# find equilibrium speed
+function find_equilibrium_speed(winch, set_speed, force, n=10000)
+    last_v_act = 0.0
+    for v_set in range(0.0, 2*set_speed, n)
+        lim_speed = minimum([v_set, set_speed])
+        set_force(winch, force)
+        set_v_set(winch, lim_speed)
+        v_act = get_speed(winch)
+        on_timer(winch)
+        if v_set > 0 && abs(v_act - last_v_act) < 1e-6
+            return v_act
+        end
+        last_v_act = v_act
+    end
+    set_v_set(winch, set_speed)
+    on_timer(winch)
+    @error "Failed to find equilibrium speed"
+end
+
+function motor_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, force]
+    v_act = x[1]
+    v_set, force = u[1], u[2]
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function calc_force(v_wind, v_ro)
+    (v_wind - v_ro)^2 * 4000.0 / 16.0
+end
+
+function system_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, v_wind]
+    v_act = x[1]
+    v_set, v_wind = u[1], u[2]
+    force = calc_force(v_wind, v_act)
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function linearize(winch, v_set, v_wind)
+    force = calc_force(v_wind, v_set)
+    v_act = find_equilibrium_speed(winch, v_set, force)
+    x0 = [v_act]         # State at operating point
+    u0 = [v_set, v_wind]  # Input at operating point
+    A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0)
+    B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
+    C = [1.0]
+    D = [0.0 0.0]
+    siso_sys = ss(A, B[:, 1], C, D[:, 1])
+end
+
+for v_wind in range(1, 9, length=9)
+    v_set = 0.57*v_wind
+    @info "Linearizing for v_wind: $v_wind m/s"
+    sys_new = linearize(winch, v_set, v_wind)
+    # @info "System: $sys_new"
+    # @info "Eigenvalues: $(eigvals(sys_new))"
+    bode_plot(sys_new; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
+end
+
+# bode_plot(sys_new; to=2, title="Linearized Winch, F=$force N")

From 930ed17744dd02064066761759db6565478a5259 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 11:04:53 +0200
Subject: [PATCH 02/15] Cleanup

---
 mwes/mwe_05.jl | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
index 33f6b63..85d3e94 100644
--- a/mwes/mwe_05.jl
+++ b/mwes/mwe_05.jl
@@ -77,11 +77,7 @@ end
 
 for v_wind in range(1, 9, length=9)
     v_set = 0.57*v_wind
-    @info "Linearizing for v_wind: $v_wind m/s"
+    @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
     sys_new = linearize(winch, v_set, v_wind)
-    # @info "System: $sys_new"
-    # @info "Eigenvalues: $(eigvals(sys_new))"
     bode_plot(sys_new; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
 end
-
-# bode_plot(sys_new; to=2, title="Linearized Winch, F=$force N")

From 51cbf222bff8b6611ae35cf1e7aeac49f3d646b9 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 11:06:21 +0200
Subject: [PATCH 03/15] Cleanup

---
 mwes/mwe_05.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
index 85d3e94..b1372f1 100644
--- a/mwes/mwe_05.jl
+++ b/mwes/mwe_05.jl
@@ -66,7 +66,7 @@ end
 function linearize(winch, v_set, v_wind)
     force = calc_force(v_wind, v_set)
     v_act = find_equilibrium_speed(winch, v_set, force)
-    x0 = [v_act]         # State at operating point
+    x0 = [v_act]          # State at operating point
     u0 = [v_set, v_wind]  # Input at operating point
     A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0)
     B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)

From 3dcf7a093bf802088ef8cb40da87e837ab645440 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 11:10:47 +0200
Subject: [PATCH 04/15] Add mwe_06.jl

---
 mwes/mwe_06.jl | 81 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 81 insertions(+)
 create mode 100644 mwes/mwe_06.jl

diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
new file mode 100644
index 0000000..fdc8c6f
--- /dev/null
+++ b/mwes/mwe_06.jl
@@ -0,0 +1,81 @@
+# Linearize the closed loop system consisting of the winch, kite and upper force controller.
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff
+import FiniteDiff: finite_difference_jacobian
+
+if isfile("data/system_tuned.yaml")
+    set = load_settings("system_tuned.yaml")
+else
+    set = load_settings("system.yaml")
+end
+wcs = WCSettings(dt=0.02)
+update(wcs)
+wcs.test = true
+
+winch = Winch(wcs, set)
+
+# find equilibrium speed
+function find_equilibrium_speed(winch, set_speed, force, n=10000)
+    last_v_act = 0.0
+    for v_set in range(0.0, 2*set_speed, n)
+        lim_speed = minimum([v_set, set_speed])
+        set_force(winch, force)
+        set_v_set(winch, lim_speed)
+        v_act = get_speed(winch)
+        on_timer(winch)
+        if v_set > 0 && abs(v_act - last_v_act) < 1e-6
+            return v_act
+        end
+        last_v_act = v_act
+    end
+    set_v_set(winch, set_speed)
+    on_timer(winch)
+    @error "Failed to find equilibrium speed"
+end
+
+function motor_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, force]
+    v_act = x[1]
+    v_set, force = u[1], u[2]
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function calc_force(v_wind, v_ro)
+    (v_wind - v_ro)^2 * 4000.0 / 16.0
+end
+
+function system_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, v_wind]
+    v_act = x[1]
+    v_set, v_wind = u[1], u[2]
+    force = calc_force(v_wind, v_act)
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function linearize(winch, v_set, v_wind)
+    force = calc_force(v_wind, v_set)
+    v_act = find_equilibrium_speed(winch, v_set, force)
+    x0 = [v_act]          # State at operating point
+    u0 = [v_set, v_wind]  # Input at operating point
+    A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0)
+    B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
+    C = [1.0]
+    D = [0.0 0.0]
+    siso_sys = ss(A, B[:, 1], C, D[:, 1])
+end
+
+for v_wind in range(1, 9, length=9)
+    v_set = 0.57*v_wind
+    @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
+    sys_new = linearize(winch, v_set, v_wind)
+    bode_plot(sys_new; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
+end

From 76ab9f5919db041ccc1470d11d177a2459c8a965 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <u.fechner-1@tudelft.nl>
Date: Mon, 2 Jun 2025 13:23:00 +0200
Subject: [PATCH 05/15] Use v_ro=4m/s

---
 mwes/mwe_02.jl |  2 +-
 mwes/mwe_03.jl |  4 ++--
 mwes/mwe_04.jl | 17 +++++++++--------
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/mwes/mwe_02.jl b/mwes/mwe_02.jl
index b73057c..51b5cbb 100644
--- a/mwes/mwe_02.jl
+++ b/mwes/mwe_02.jl
@@ -47,7 +47,7 @@ function motor_dynamics(x, u)
     return [acc]
 end
 
-v_set = 8.0
+v_set = 4.0
 force = 4000.0
 v_act = find_equilibrium_speed(winch, v_set, force)
 
diff --git a/mwes/mwe_03.jl b/mwes/mwe_03.jl
index cf01442..c61b2a7 100644
--- a/mwes/mwe_03.jl
+++ b/mwes/mwe_03.jl
@@ -49,7 +49,7 @@ function motor_dynamics(x, u)
     return [acc]
 end
 
-v_set = 8.0
+v_set = 4.0
 force = 4000.0
 v_act = find_equilibrium_speed(winch, v_set, force)
 
@@ -68,4 +68,4 @@ D = [0.0 0.0]
 sys = ss(A, B, C, D)
 sys_new = ss(A, B[:, 1], C, D[:, 1])
 
-bode_plot(sys_new; to=2, title="Linearized Winch, F=$force N")
+bode_plot(sys_new; from=0.76, to=2.85, title="Linearized Winch, F=$force N")
diff --git a/mwes/mwe_04.jl b/mwes/mwe_04.jl
index c1efb91..3911ac7 100644
--- a/mwes/mwe_04.jl
+++ b/mwes/mwe_04.jl
@@ -62,12 +62,13 @@ function linearize(winch, v_set, force)
 end
 
 v_set = 4.0
-for force in range(300.0, 3800.0, length=10)
-    @info "Linearizing for force: $force N"
-    sys_new = linearize(winch, v_set, force)
-    # @info "System: $sys_new"
-    # @info "Eigenvalues: $(eigvals(sys_new))"
-    bode_plot(sys_new; from=0.76, to=2.85, title="Linearized Winch, F=300..3800 N")
-end
+# for force in range(300.0, 3800.0, length=10)
+#     @info "Linearizing for force: $force N"
+#     sys_new = linearize(winch, v_set, force)
+#     # @info "System: $sys_new"
+#     # @info "Eigenvalues: $(eigvals(sys_new))"
+#     bode_plot(sys_new; from=0.76, to=2.85, title="Linearized Winch, F=300..3800 N")
+# end
 
-# bode_plot(sys_new; to=2, title="Linearized Winch, F=$force N")
+sys_new = linearize(winch, v_set, 1000.0)
+bode_plot(sys_new; from=0.76, to=2.85, title="Linearized Winch, F=1000 N")

From 68ebddbb0bd53bd38ffb37c0681f4cf46c4c8f9a Mon Sep 17 00:00:00 2001
From: Uwe Fechner <u.fechner-1@tudelft.nl>
Date: Mon, 2 Jun 2025 13:25:37 +0200
Subject: [PATCH 06/15] Get rid of warnings

---
 mwes/mwe_05.jl | 1 +
 mwes/mwe_06.jl | 1 +
 2 files changed, 2 insertions(+)

diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
index b1372f1..69278c0 100644
--- a/mwes/mwe_05.jl
+++ b/mwes/mwe_05.jl
@@ -76,6 +76,7 @@ function linearize(winch, v_set, v_wind)
 end
 
 for v_wind in range(1, 9, length=9)
+    local v_set, sys_new
     v_set = 0.57*v_wind
     @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
     sys_new = linearize(winch, v_set, v_wind)
diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
index fdc8c6f..6947790 100644
--- a/mwes/mwe_06.jl
+++ b/mwes/mwe_06.jl
@@ -74,6 +74,7 @@ function linearize(winch, v_set, v_wind)
 end
 
 for v_wind in range(1, 9, length=9)
+    local v_set, sys_new
     v_set = 0.57*v_wind
     @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
     sys_new = linearize(winch, v_set, v_wind)

From 46ee45544b21e39143b8bffbb37d4fd84703951f Mon Sep 17 00:00:00 2001
From: Uwe Fechner <u.fechner-1@tudelft.nl>
Date: Mon, 2 Jun 2025 13:29:50 +0200
Subject: [PATCH 07/15] Cleanup

---
 mwes/mwe_06.jl | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
index 6947790..697caf2 100644
--- a/mwes/mwe_06.jl
+++ b/mwes/mwe_06.jl
@@ -74,9 +74,9 @@ function linearize(winch, v_set, v_wind)
 end
 
 for v_wind in range(1, 9, length=9)
-    local v_set, sys_new
+    local v_set, sys
     v_set = 0.57*v_wind
     @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
-    sys_new = linearize(winch, v_set, v_wind)
-    bode_plot(sys_new; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
+    sys = linearize(winch, v_set, v_wind)
+    bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
 end

From 44b90cbc97a8bbf67b94a53f317c46e374b3e904 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 15:46:57 +0200
Subject: [PATCH 08/15] Some progress

---
 mwes/mwe_06.jl | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
index 697caf2..c0fcfa5 100644
--- a/mwes/mwe_06.jl
+++ b/mwes/mwe_06.jl
@@ -51,6 +51,18 @@ function calc_force(v_wind, v_ro)
     (v_wind - v_ro)^2 * 4000.0 / 16.0
 end
 
+# create the upper force controller
+# Tf​ can typically be chosen as Ti/NT for a PI controller and Td/N for a PID controller, and N is commonly in the range 2 to 20. 
+# Td = wcs.df_high / wcs.pf_high Kd/Kp
+function upper_force_controller(wcs)
+    # wcs: WCSettings object
+    Td = wcs.df_high / wcs.pf_high
+    N = wcs.nf_high
+    Tf = Td / N
+    C = pid(wcs.pf_high, wcs.if_high, wcs.df_high; form=:parallel, filter_order=1, state_space=true, Tf)
+    return C
+end
+
 function system_dynamics(x, u)
     # x: state vector, e.g., [v_act]
     # u: input vector, e.g., [v_set, v_wind]
@@ -73,10 +85,18 @@ function linearize(winch, v_set, v_wind)
     siso_sys = ss(A, B[:, 1], C, D[:, 1])
 end
 
+function open_loop_system(winch, v_set, v_wind)
+    # Create the open loop system with the upper force controller
+    C = upper_force_controller(winch.wcs)
+    sys = linearize(winch, v_set, v_wind)
+    # sys_open = feedback(C * sys, 1.0; sign=-1)
+    return C * sys
+end
+
 for v_wind in range(1, 9, length=9)
     local v_set, sys
     v_set = 0.57*v_wind
     @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
-    sys = linearize(winch, v_set, v_wind)
+    sys = open_loop_system(winch, v_set, v_wind)
     bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
 end

From dcfe2433b4488de274276a647a36ddc0b25e5a52 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 16:20:46 +0200
Subject: [PATCH 09/15] Refactoring

---
 mwes/mwe_05.jl |  7 ++++---
 mwes/mwe_06.jl | 15 ++++++++++-----
 2 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
index 69278c0..9895ae4 100644
--- a/mwes/mwe_05.jl
+++ b/mwes/mwe_05.jl
@@ -72,13 +72,14 @@ function linearize(winch, v_set, v_wind)
     B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
     C = [1.0]
     D = [0.0 0.0]
-    siso_sys = ss(A, B[:, 1], C, D[:, 1])
+    force = calc_force(v_wind, v_act)
+    siso_sys = ss(A, B[:, 1], C, D[:, 1]) * force/v_act
 end
 
 for v_wind in range(1, 9, length=9)
     local v_set, sys_new
     v_set = 0.57*v_wind
     @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
-    sys_new = linearize(winch, v_set, v_wind)
-    bode_plot(sys_new; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
+    sys = linearize(winch, v_set, v_wind)
+    bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
 end
diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
index c0fcfa5..2570955 100644
--- a/mwes/mwe_06.jl
+++ b/mwes/mwe_06.jl
@@ -82,7 +82,8 @@ function linearize(winch, v_set, v_wind)
     B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
     C = [1.0]
     D = [0.0 0.0]
-    siso_sys = ss(A, B[:, 1], C, D[:, 1])
+    force = calc_force(v_wind, v_act)
+    siso_sys = ss(A, B[:, 1], C, D[:, 1]) * force/v_act
 end
 
 function open_loop_system(winch, v_set, v_wind)
@@ -93,10 +94,14 @@ function open_loop_system(winch, v_set, v_wind)
     return C * sys
 end
 
-for v_wind in range(1, 9, length=9)
-    local v_set, sys
+for v_wind in range(7.5, 9, length=2)
+    global sys
+    local v_set
     v_set = 0.57*v_wind
-    @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
+    # @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
     sys = open_loop_system(winch, v_set, v_wind)
-    bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=1..9 m/s")
+    gm, pm, wgm, wpm = margin(sys; adjust_phase_start=false)
+    @info "Gain margin: $gm, Phase margin: $pm, Gain crossover frequency: $wgm, Phase crossover frequency: $wpm"
+    bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=8..9 m/s")
 end
+sys

From 22874a287f79557b6075af12eae6c3a0cc165189 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 16:41:25 +0200
Subject: [PATCH 10/15] Add RobustAndOptimalControl

---
 Project.toml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/Project.toml b/Project.toml
index 9102686..038cd2f 100644
--- a/Project.toml
+++ b/Project.toml
@@ -32,6 +32,7 @@ NLsolve = "4.5.1"
 NOMAD = "2.4.1"
 Parameters = "0.12.3"
 Pkg = "1.10, 1.11"
+RobustAndOptimalControl = "0.4.39"
 StaticArrays = "1.9.13"
 Statistics = "1.10, 1.11.1"
 StructTypes = "1.10, 1.11"
@@ -44,6 +45,7 @@ julia = "1.10, 1.11"
 [extras]
 Aqua = "4c88cf16-eb10-579e-8560-4a9242c79595"
 ControlPlots = "23c2ee80-7a9e-4350-b264-8e670f12517c"
+RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541"
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 NOMAD = "02130f1c-4665-5b79-af82-ff1385104aa0"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

From fc6671ea5a56651fac8cb201f7e6fba74c899cd8 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 16:41:39 +0200
Subject: [PATCH 11/15] Calc disk margin

---
 mwes/mwe_07.jl | 109 +++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 109 insertions(+)
 create mode 100644 mwes/mwe_07.jl

diff --git a/mwes/mwe_07.jl b/mwes/mwe_07.jl
new file mode 100644
index 0000000..bfca1e8
--- /dev/null
+++ b/mwes/mwe_07.jl
@@ -0,0 +1,109 @@
+# Linearize the closed loop system consisting of the winch, kite and upper force controller.
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, Plots, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl
+import FiniteDiff: finite_difference_jacobian
+
+if isfile("data/system_tuned.yaml")
+    set = load_settings("system_tuned.yaml")
+else
+    set = load_settings("system.yaml")
+end
+wcs = WCSettings(dt=0.02)
+update(wcs)
+wcs.test = true
+
+winch = Winch(wcs, set)
+
+# find equilibrium speed
+function find_equilibrium_speed(winch, set_speed, force, n=10000)
+    last_v_act = 0.0
+    for v_set in range(0.0, 2*set_speed, n)
+        lim_speed = minimum([v_set, set_speed])
+        set_force(winch, force)
+        set_v_set(winch, lim_speed)
+        v_act = get_speed(winch)
+        on_timer(winch)
+        if v_set > 0 && abs(v_act - last_v_act) < 1e-6
+            return v_act
+        end
+        last_v_act = v_act
+    end
+    set_v_set(winch, set_speed)
+    on_timer(winch)
+    @error "Failed to find equilibrium speed"
+end
+
+function motor_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, force]
+    v_act = x[1]
+    v_set, force = u[1], u[2]
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function calc_force(v_wind, v_ro)
+    (v_wind - v_ro)^2 * 4000.0 / 16.0
+end
+
+# create the upper force controller
+# Tf​ can typically be chosen as Ti/NT for a PI controller and Td/N for a PID controller, and N is commonly in the range 2 to 20. 
+# Td = wcs.df_high / wcs.pf_high Kd/Kp
+function upper_force_controller(wcs)
+    # wcs: WCSettings object
+    Td = wcs.df_high / wcs.pf_high
+    N = wcs.nf_high
+    Tf = Td / N
+    C = pid(wcs.pf_high, wcs.if_high, wcs.df_high; form=:parallel, filter_order=1, state_space=true, Tf)
+    return C
+end
+
+function system_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, v_wind]
+    v_act = x[1]
+    v_set, v_wind = u[1], u[2]
+    force = calc_force(v_wind, v_act)
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function linearize(winch, v_set, v_wind)
+    force = calc_force(v_wind, v_set)
+    v_act = find_equilibrium_speed(winch, v_set, force)
+    x0 = [v_act]          # State at operating point
+    u0 = [v_set, v_wind]  # Input at operating point
+    A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0)
+    B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
+    C = [1.0]
+    D = [0.0 0.0]
+    force = calc_force(v_wind, v_act)
+    siso_sys = ss(A, B[:, 1], C, D[:, 1]) * force/v_act
+end
+
+function open_loop_system(winch, v_set, v_wind)
+    # Create the open loop system with the upper force controller
+    C = upper_force_controller(winch.wcs)
+    sys = linearize(winch, v_set, v_wind)
+    # sys_open = feedback(C * sys, 1.0; sign=-1)
+    return C * sys
+end
+
+for v_wind in range(7.5, 9, length=2)
+    global sys
+    local v_set
+    v_set = 0.57*v_wind
+    # @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
+    sys = open_loop_system(winch, v_set, v_wind)
+    # gm, pm, wgm, wpm = margin(sys; adjust_phase_start=false)
+    # @info "Gain margin: $gm, Phase margin: $pm, Gain crossover frequency: $wgm, Phase crossover frequency: $wpm"
+    # bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=8..9 m/s")
+end
+dm = diskmargin(sys)
+@info "$dm"
+plot(dm)

From b982e758cb3b6d02a0b3d1a5791074163ae4ab6e Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 17:30:59 +0200
Subject: [PATCH 12/15] Add RobustAndOptimalControl

---
 Project.toml   |  2 +-
 mwes/mwe_07.jl | 38 +++++++++++++++++++++++++-------------
 2 files changed, 26 insertions(+), 14 deletions(-)

diff --git a/Project.toml b/Project.toml
index 038cd2f..7c558c8 100644
--- a/Project.toml
+++ b/Project.toml
@@ -51,4 +51,4 @@ NOMAD = "02130f1c-4665-5b79-af82-ff1385104aa0"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "ControlPlots", "Documenter", "Aqua", "NOMAD"]
+test = ["Test", "ControlPlots", "Documenter", "Aqua", "NOMAD", "RobustAndOptimalControl"]
diff --git a/mwes/mwe_07.jl b/mwes/mwe_07.jl
index bfca1e8..d0b21fe 100644
--- a/mwes/mwe_07.jl
+++ b/mwes/mwe_07.jl
@@ -1,7 +1,7 @@
 # Linearize the closed loop system consisting of the winch, kite and upper force controller.
 
 using Pkg
-if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+if ! ("RobustAndOptimalControl" ∈ keys(Pkg.project().dependencies))
     using TestEnv; TestEnv.activate()
     using Test
 end
@@ -94,16 +94,28 @@ function open_loop_system(winch, v_set, v_wind)
     return C * sys
 end
 
-for v_wind in range(7.5, 9, length=2)
-    global sys
-    local v_set
-    v_set = 0.57*v_wind
-    # @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
-    sys = open_loop_system(winch, v_set, v_wind)
-    # gm, pm, wgm, wpm = margin(sys; adjust_phase_start=false)
-    # @info "Gain margin: $gm, Phase margin: $pm, Gain crossover frequency: $wgm, Phase crossover frequency: $wpm"
-    # bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=8..9 m/s")
+function margins(sys)
+    margins = []
+    for v_wind in range(1, 9, length=9)
+        global sys
+        local v_set, dm
+        v_set = 0.57*v_wind
+        sys = open_loop_system(winch, v_set, v_wind)
+        dm = diskmargin(sys)
+        push!(margins, dm.margin)
+    end
+    min_margin = minimum(margins)
+    if min_margin < 0.3
+        @error "System is unstable with a minimum margin of: $min_margin"
+    elseif min_margin < 0.5
+        @warn "System is marginally stable with a minimum margin of: $min_margin"
+    else
+        @info "System is stable with a minimum margin of: $min_margin"
+    end
+    return margins
 end
-dm = diskmargin(sys)
-@info "$dm"
-plot(dm)
+margins(sys)
+
+# dm = diskmargin(sys)
+# @info "$dm"
+# plot(dm)

From 85aceeb5cebc8f36a019c782ca86e05baaa34ff7 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 17:37:07 +0200
Subject: [PATCH 13/15] Update MWEs

---
 mwes/mwe_06.jl | 33 ++++++++++++++++++++++++++-------
 mwes/mwe_07.jl |  2 +-
 2 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/mwes/mwe_06.jl b/mwes/mwe_06.jl
index 2570955..6acbbb1 100644
--- a/mwes/mwe_06.jl
+++ b/mwes/mwe_06.jl
@@ -5,7 +5,7 @@ if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
     using TestEnv; TestEnv.activate()
     using Test
 end
-using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff
+using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl
 import FiniteDiff: finite_difference_jacobian
 
 if isfile("data/system_tuned.yaml")
@@ -94,14 +94,33 @@ function open_loop_system(winch, v_set, v_wind)
     return C * sys
 end
 
-for v_wind in range(7.5, 9, length=2)
+function margins(sys)
+    margins = []
+    for v_wind in range(1, 9, length=9)
+        global sys
+        local v_set, dm
+        v_set = 0.57*v_wind
+        sys = open_loop_system(winch, v_set, v_wind)
+        dm = diskmargin(sys)
+        push!(margins, dm.margin)
+    end
+    min_margin = minimum(margins)
+    if min_margin < 0.3
+        @error "System is unstable with a minimum margin of: $min_margin"
+    elseif min_margin < 0.5
+        @warn "System is marginally stable with a minimum margin of: $min_margin"
+    else
+        @info "System is stable with a minimum margin of: $min_margin"
+    end
+    return margins
+end
+margins(sys)
+
+for v_wind in range(7.5, 9, length=3)
     global sys
     local v_set
     v_set = 0.57*v_wind
-    # @info "Linearizing for v_wind: $v_wind m/s, v_ro: $(round(v_set, digits=2)) m/s"
     sys = open_loop_system(winch, v_set, v_wind)
-    gm, pm, wgm, wpm = margin(sys; adjust_phase_start=false)
-    @info "Gain margin: $gm, Phase margin: $pm, Gain crossover frequency: $wgm, Phase crossover frequency: $wpm"
-    bode_plot(sys; from=0.76, to=2.85, title="Linearized System, v_wind=8..9 m/s")
+    bode_plot(sys; from=0.76, to=2.85, title="System with UFC, v_wind=7.5..9 m/s")
 end
-sys
+nothing
diff --git a/mwes/mwe_07.jl b/mwes/mwe_07.jl
index d0b21fe..dddf9f2 100644
--- a/mwes/mwe_07.jl
+++ b/mwes/mwe_07.jl
@@ -5,7 +5,7 @@ if ! ("RobustAndOptimalControl" ∈ keys(Pkg.project().dependencies))
     using TestEnv; TestEnv.activate()
     using Test
 end
-using WinchControllers, WinchModels, KiteUtils, Plots, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl
+using WinchControllers, WinchModels, KiteUtils, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl
 import FiniteDiff: finite_difference_jacobian
 
 if isfile("data/system_tuned.yaml")

From 60e9645814fddad3abae4bf769173d8d9f4335d3 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 17:42:40 +0200
Subject: [PATCH 14/15] Add example stability_ufc

---
 examples/menu.jl          |   1 +
 examples/stability_ufc.jl | 127 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 128 insertions(+)
 create mode 100644 examples/stability_ufc.jl

diff --git a/examples/menu.jl b/examples/menu.jl
index d4b4d92..e21f7b7 100644
--- a/examples/menu.jl
+++ b/examples/menu.jl
@@ -6,6 +6,7 @@ options = ["test_winchcontroller = include(\"test_winchcontroller.jl\")",
            "autotune_ = include(\"autotune.jl\")",
            "test_tuned_winchcontroller = include(\"test_tuned_winchcontroller.jl\")",
            "plot_power = include(\"plot_power.jl\")",
+           "stability_ufc = include(\"stability_ufc.jl\")",
            "help = WinchControllers.help()",
            "quit"]
 
diff --git a/examples/stability_ufc.jl b/examples/stability_ufc.jl
new file mode 100644
index 0000000..8ff0ea1
--- /dev/null
+++ b/examples/stability_ufc.jl
@@ -0,0 +1,127 @@
+# Linearize the closed loop system consisting of the winch, kite and upper force controller.
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl
+import FiniteDiff: finite_difference_jacobian
+
+if isfile("data/system_tuned.yaml")
+    set = load_settings("system_tuned.yaml")
+else
+    set = load_settings("system.yaml")
+end
+wcs = WCSettings(dt=0.02)
+update(wcs)
+wcs.test = true
+
+winch = Winch(wcs, set)
+
+@info "Testing stability of the upper force controller (UFC) with the winch and kite system."
+
+# find equilibrium speed
+function find_equilibrium_speed(winch, set_speed, force, n=10000)
+    last_v_act = 0.0
+    for v_set in range(0.0, 2*set_speed, n)
+        lim_speed = minimum([v_set, set_speed])
+        set_force(winch, force)
+        set_v_set(winch, lim_speed)
+        v_act = get_speed(winch)
+        on_timer(winch)
+        if v_set > 0 && abs(v_act - last_v_act) < 1e-6
+            return v_act
+        end
+        last_v_act = v_act
+    end
+    set_v_set(winch, set_speed)
+    on_timer(winch)
+    @error "Failed to find equilibrium speed"
+end
+
+function motor_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, force]
+    v_act = x[1]
+    v_set, force = u[1], u[2]
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function calc_force(v_wind, v_ro)
+    (v_wind - v_ro)^2 * 4000.0 / 16.0
+end
+
+# create the upper force controller
+# Tf​ can typically be chosen as Ti/NT for a PI controller and Td/N for a PID controller, and N is commonly in the range 2 to 20. 
+# Td = wcs.df_high / wcs.pf_high Kd/Kp
+function upper_force_controller(wcs)
+    # wcs: WCSettings object
+    Td = wcs.df_high / wcs.pf_high
+    N = wcs.nf_high
+    Tf = Td / N
+    C = pid(wcs.pf_high, wcs.if_high, wcs.df_high; form=:parallel, filter_order=1, state_space=true, Tf)
+    return C
+end
+
+function system_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, v_wind]
+    v_act = x[1]
+    v_set, v_wind = u[1], u[2]
+    force = calc_force(v_wind, v_act)
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+function linearize(winch, v_set, v_wind)
+    force = calc_force(v_wind, v_set)
+    v_act = find_equilibrium_speed(winch, v_set, force)
+    x0 = [v_act]          # State at operating point
+    u0 = [v_set, v_wind]  # Input at operating point
+    A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0)
+    B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0)
+    C = [1.0]
+    D = [0.0 0.0]
+    force = calc_force(v_wind, v_act)
+    siso_sys = ss(A, B[:, 1], C, D[:, 1]) * force/v_act
+end
+
+function open_loop_system(winch, v_set, v_wind)
+    # Create the open loop system with the upper force controller
+    C = upper_force_controller(winch.wcs)
+    sys = linearize(winch, v_set, v_wind)
+    # sys_open = feedback(C * sys, 1.0; sign=-1)
+    return C * sys
+end
+
+function margins()
+    margins = []
+    for v_wind in range(1, 9, length=9)
+        local v_set, dm
+        v_set = 0.57*v_wind
+        sys = open_loop_system(winch, v_set, v_wind)
+        dm = diskmargin(sys)
+        push!(margins, dm.margin)
+    end
+    min_margin = minimum(margins)
+    if min_margin < 0.3
+        @error "System is unstable with a minimum margin of: $min_margin"
+    elseif min_margin < 0.5
+        @warn "System is marginally stable with a minimum margin of: $min_margin"
+    else
+        @info "System is stable with a minimum margin of: $(round(min_margin, digits=2))"
+    end
+    return margins
+end
+margins()
+
+for v_wind in range(7.5, 9, length=3)
+    global sys
+    local v_set
+    v_set = 0.57*v_wind
+    sys = open_loop_system(winch, v_set, v_wind)
+    bode_plot(sys; from=0.76, to=2.85, title="System with UFC, v_wind=7.5..9 m/s")
+end
+nothing

From eef95232394a3782ebaad35fa1f74ad3b19bfb57 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <fechner@aenarete.eu>
Date: Mon, 2 Jun 2025 17:46:34 +0200
Subject: [PATCH 15/15] Fix tests

---
 Project.toml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Project.toml b/Project.toml
index 7c558c8..120cf40 100644
--- a/Project.toml
+++ b/Project.toml
@@ -4,7 +4,6 @@ authors = ["Uwe Fechner <u.fechner-1@tudelft.nl> and contributors"]
 version = "0.5.3"
 
 [deps]
-ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
@@ -45,10 +44,11 @@ julia = "1.10, 1.11"
 [extras]
 Aqua = "4c88cf16-eb10-579e-8560-4a9242c79595"
 ControlPlots = "23c2ee80-7a9e-4350-b264-8e670f12517c"
+ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541"
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 NOMAD = "02130f1c-4665-5b79-af82-ff1385104aa0"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "ControlPlots", "Documenter", "Aqua", "NOMAD", "RobustAndOptimalControl"]
+test = ["Test", "ControlPlots", "ControlSystemsBase", "Documenter", "Aqua", "NOMAD", "RobustAndOptimalControl"]