diff --git a/Project.toml b/Project.toml
index 9102686..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"
@@ -32,6 +31,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,9 +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"]
+test = ["Test", "ControlPlots", "ControlSystemsBase", "Documenter", "Aqua", "NOMAD", "RobustAndOptimalControl"]
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
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")
diff --git a/mwes/mwe_05.jl b/mwes/mwe_05.jl
new file mode 100644
index 0000000..9895ae4
--- /dev/null
+++ b/mwes/mwe_05.jl
@@ -0,0 +1,85 @@
+# 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]
+    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 = 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
new file mode 100644
index 0000000..6acbbb1
--- /dev/null
+++ b/mwes/mwe_06.jl
@@ -0,0 +1,126 @@
+# 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)
+
+# 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(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
+    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
diff --git a/mwes/mwe_07.jl b/mwes/mwe_07.jl
new file mode 100644
index 0000000..dddf9f2
--- /dev/null
+++ b/mwes/mwe_07.jl
@@ -0,0 +1,121 @@
+# Linearize the closed loop system consisting of the winch, kite and upper force controller.
+
+using Pkg
+if ! ("RobustAndOptimalControl" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, 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
+
+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)
+
+# dm = diskmargin(sys)
+# @info "$dm"
+# plot(dm)