import Pkg; Pkg.activate(@DIR); Pkg.instantiate()
+ +https://github.com/Optimal-Control-16-745/lecture-notebooks/blob/main/Lecture%203/minimization.ipynb
+ +using LinearAlgebra
+using ForwardDiff
+using PyPlot
+"""
+f(x)
+
+
+Scalar objective used to illustrate Newton's method on a nonconvex quartic.
+
+
+f(x) = x^4 + x^3 - x^2 - x
+
+
+This function has both minima and maxima, so a naive Newton step can ascend if
+`∇²f(x) < 0` at the current iterate (see lecture notes on sufficient conditions).
+"""
+
+function f(x)
+ return x.^4 + x.^3 - x.^2 - x
+end
+f (generic function with 1 method)+
"""
+∇f(x)
+
+Analytic gradient (first derivative) of `f`.
+"""
+function ∇f(x)
+ return 4.0*x.^3 + 3.0*x.^2 - 2.0*x - 1.0
+end
+∇f+
"""
+Analytic Hessian (second derivative) of `f`.
+"""
+
+function ∇2f(x)
+ return 12.0*x.^2 + 6.0*x - 2.0
+end
+∇2f (generic function with 1 method)+
x = LinRange(-1.75,1.25,1000)
+1000-element LinRange{Float64, Int64}:
+ -1.75, -1.747, -1.74399, -1.74099, …, 1.24099, 1.24399, 1.247, 1.25
+p = plot(x,f(x))
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x16801bfa0>
+function newton_step(x0)
+ xn = x0 - ∇2f(x0)\∇f(x0)
+end
+newton_step (generic function with 1 method)+
xguess = -1.5
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1685ec460>
+xnew = newton_step(xguess[end])
+xguess = [xguess xnew]
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+2-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1685ec730>
+ PyObject <matplotlib.lines.Line2D object at 0x1685ec820>
+xguess = 0.0
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1693ba920>
+xnew = newton_step(xguess[end])
+xguess = [xguess xnew]
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+2-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1694506d0>
+ PyObject <matplotlib.lines.Line2D object at 0x1694507c0>
+∇2f(0.0)
+-2.0+
function regularized_newton_step(x0)
+ β = 1.0
+ H = ∇2f(x0)
+ while !isposdef(H)
+ H = H + β*I
+ end
+ xn = x0 - H\∇f(x0)
+end
+regularized_newton_step (generic function with 1 method)+
xguess = 0.0
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x170479de0>
+xnew = regularized_newton_step(xguess[end])
+xguess = [xguess xnew]
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+2-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1704eba00>
+ PyObject <matplotlib.lines.Line2D object at 0x1704ebaf0>
+function backtracking_regularized_newton_step(x0)
+ b = 0.1
+ c = 0.5
+ β = 1.0
+ H = ∇2f(x0)
+ while !isposdef(H)
+ H = H + β*I
+ end
+ Δx = -H\∇f(x0)
+
+ α = 1.0
+ while f(x0 + α*Δx) > f(x0) + b*α*∇f(x0)*Δx
+ α = c*α
+ end
+ print(α)
+ xn = x0 + α*Δx
+end
+backtracking_regularized_newton_step (generic function with 1 method)+
xguess = 0.0
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x17058d6c0>
+xnew = backtracking_regularized_newton_step(xguess[end])
+xguess = [xguess xnew]
+plot(x, f(x))
+plot(xguess, f(xguess), "rx")
+0.5+
+2-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x1705df850>
+ PyObject <matplotlib.lines.Line2D object at 0x1705df940>
+import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()
+ Activating project at `~/Desktop/Summer_2025/LearningToControlClass/class02`
+
+https://github.com/Optimal-Control-16-745/lecture-notebooks/blob/main/Lecture%203/root-finding.ipynb
+ +using LinearAlgebra
+using ForwardDiff
+using PyPlot
+"""
+pendulum_dynamics(x)
+
+
+Simple undamped pendulum in state-space form x = [θ; θ̇]:
+
+
+θ̇ = v
+v̇ = -(g/ℓ) sin(θ)
+
+
+Returns the time derivative ẋ = [θ̇; v̇].
+"""
+function pendulum_dynamics(x)
+ l = 1.0
+ g = 9.81
+
+ θ = x[1]
+ θ̇ = x[2]
+
+ θ̈ = -(g/l)*sin(θ)
+
+ return [θ̇; θ̈]
+end
+pendulum_dynamics+
We want one implicit step of Backward Euler:
+x_{n+1} = x_n + h * f(x_{n+1})
+
+Define:
+Fixed-point (Picard) iteration
+Update rule: x_{k+1} = g(x_k)
+Converges locally if the mapping is a contraction:
+The step size h controls stability.
+Damping can help:
+x <- (1 - β) * x + β * g(x), 0 < β <= 1
+
+Newton's method
+Solve the linearized system:
+∂r * Δx = -r
+
+where ∂r = d/dx [ x_n + h f(x) - x ]
+Update rule:
+x <- x + Δx
+
+Near the root, convergence is quadratic (usually fewer iterations than fixed-point).
+"""
+backward_euler_step_fixed_point(fun, x0, h; tol=1e-8, maxiter=10_000)
+
+
+Implicit step via **fixed-point iteration** on
+x = x0 + h * fun(x).
+
+
+Converges only if the fixed point is stable and the initial guess lies in its
+basin of attraction. Returns `(x_next, errors)`, where `errors[k] = ‖r(x_k)‖` and
+r(x) = x0 + h * fun(x) - x.
+"""
+
+function backward_euler_step_fixed_point(fun, x0, h)
+ xn = x0
+ e = [norm(x0 + h.*fun(xn) - xn)]
+ while e[end] > 1e-8
+ xn = x0 + h.*fun(xn)
+ e = [e; norm(x0 + h.*fun(xn) - xn)]
+ end
+
+ return xn, e
+end
+backward_euler_step_fixed_point (generic function with 1 method)+
"""
+backward_euler_step_newton(fun, x0, h; tol=1e-8, maxiter=100)
+
+
+Implicit step via **Newton's method** on the residual
+r(x) = x0 + h * fun(x) - x.
+
+
+Uses ForwardDiff.jacobian to obtain ∂r/∂x at the current iterate. Quadratic
+convergence near the root; typically many fewer iterations than fixed-point.
+Returns `(x_next, errors)`.
+"""
+function backward_euler_step_newton(fun, x0, h)
+ xn = x0
+ r = x0 + h.*fun(xn) - xn
+ e = [norm(r)]
+ while e[end] > 1e-8
+ ∂r = ForwardDiff.jacobian(x -> x0 + h.*fun(x) - x, xn)
+ xn = xn - ∂r\r
+ r = x0 + h.*fun(xn) - xn
+ e = [e; norm(r)]
+ end
+
+ return xn, e
+end
+backward_euler_step_newton+
"""
+backward_euler_fixed_point(fun, x0, Tf, h)
+
+
+Integrate using backward Euler where each implicit step is solved by fixed-point
+iteration. Returns `(x_hist, t_hist)`.
+"""
+function backward_euler_fixed_point(fun, x0, Tf, h)
+ t = Array(range(0,Tf,step=h))
+
+ x_hist = zeros(length(x0),length(t))
+ x_hist[:,1] .= x0
+
+ for k = 1:(length(t)-1)
+ x_hist[:,k+1], e = backward_euler_step_fixed_point(fun, x_hist[:,k], h)
+ end
+
+ return x_hist, t
+end
+backward_euler_fixed_point+
"""
+backward_euler_newton(fun, x0, Tf, h)
+
+
+Integrate using backward Euler where each implicit step is solved with Newton.
+Returns `(x_hist, t_hist)`.
+"""
+function backward_euler_newton(fun, x0, Tf, h)
+ t = Array(range(0,Tf,step=h))
+
+ x_hist = zeros(length(x0),length(t))
+ x_hist[:,1] .= x0
+
+ for k = 1:(length(t)-1)
+ x_hist[:,k+1], e = backward_euler_step_newton(fun, x_hist[:,k], h)
+ end
+
+ return x_hist, t
+end
+backward_euler_newton+
# --- Demo: compare fixed-point vs Newton for the pendulum ---
+x0 = [0.1; 0.0] # small-angle start (near-stable regime)
+Tf = 10.0
+h = 0.01
+
+
+x_hist1, t_hist1 = backward_euler_fixed_point(pendulum_dynamics, x0, Tf, h)
+x_hist2, t_hist2 = backward_euler_newton(pendulum_dynamics, x0, Tf, h)
+
+
+figure()
+plot(t_hist1, x_hist1[1, :], label="θ (fixed-point)")
+plot(t_hist2, x_hist2[1, :], label="θ (Newton)")
+xlabel("time t"); ylabel("angle θ"); title("Backward Euler (implicit) with two inner solvers"); grid(true); legend()
+
+PyObject <matplotlib.legend.Legend object at 0x155d61db0>+
# Numerical check that trajectories agree (up to tolerance/roundoff)
+maximum(abs.(x_hist1 .- x_hist2))
+7.3007510554383426e-6+
# --- Single-step convergence histories ---
+(xn_fp, e_fp) = backward_euler_step_fixed_point(pendulum_dynamics, x0, 0.1)
+(xn_nt, e_nt) = backward_euler_step_newton(pendulum_dynamics, x0, 0.1)
+([0.09107763165756541, -0.08922368342434606], [0.09793658173053843, 3.7830087232931797e-6, 5.2874553670659e-15])+
e_fp
+14-element Vector{Float64}:
+ 0.09793658173053843
+ 0.009793658173053846
+ 0.009564124766684667
+ 0.0009564124766684723
+ 0.0009343853483241293
+ 9.343853483241571e-5
+ 9.128296581455142e-5
+ 9.128296581450979e-6
+ 8.917746787032166e-6
+ 8.917746786990532e-7
+ 8.712050176828967e-7
+ 8.712050177106523e-8
+ 8.511098474606182e-8
+ 8.511098478769519e-9
+e_nt
+3-element Vector{Float64}:
+ 0.09793658173053843
+ 3.7830087232931797e-6
+ 5.2874553670659e-15
+figure()
+semilogy(e_fp, ".-", label="fixed-point residual norm")
+semilogy(e_nt, ".-", label="Newton residual norm")
+xlabel("iteration"); ylabel("‖r(x_k)‖"); title("Convergence per implicit step"); grid(true); legend()
+
+PyObject <matplotlib.legend.Legend object at 0x15f9b95a0>+
import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()
+ Activating project at `~/Desktop/Summer_2025/LearningToControlClass/class02`
+
+using LinearAlgebra
+using ForwardDiff
+using PyPlot
+Q = Diagonal([0.5; 1])
+function f(x)
+ return 0.5*(x-[1; 0])'*Q*(x-[1; 0])
+end
+function ∇f(x)
+ return Q*(x-[1; 0])
+end
+function ∇2f(x)
+ return Q
+end
+∇2f (generic function with 1 method)+
function c(x)
+ return x[1]^2 + 2*x[1] - x[2]
+end
+function ∂c(x)
+ return [2*x[1]+2 -1]
+end
+∂c (generic function with 1 method)+
function plot_landscape()
+ Nsamp = 20
+ Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')
+ Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))
+ Zsamp = zeros(Nsamp,Nsamp)
+ for j = 1:Nsamp
+ for k = 1:Nsamp
+ Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])
+ end
+ end
+ contour(Xsamp,Ysamp,Zsamp)
+
+ xc = LinRange(-3.2,1.2,Nsamp)
+ plot(xc,xc.^2+2.0.*xc,"y")
+end
+
+plot_landscape()
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x13888c220>
+function newton_step(x0,λ0)
+ # Computing the hessian of the lagrangian
+ H = ∇2f(x0) + ForwardDiff.jacobian(x -> ∂c(x)'*λ0, x0)
+ C = ∂c(x0)
+ # Forming KKT system to solve
+ Δz = [H C'; C 0]\[-∇f(x0)-C'*λ0; -c(x0)]
+ # Extracting solutions of KKT solve
+ Δx = Δz[1:2]
+ Δλ = Δz[3]
+ # Applying update
+ return x0+Δx, λ0+Δλ
+end
+newton_step (generic function with 1 method)+
xguess = [-1; -1]
+# Note that we did not have to give an initial feasible guess which is really improtant for other applications in roobitics where we have nonlinear constraints on initial state which is already hard
+λguess = [0.0]
+plot_landscape()
+plot(xguess[1], xguess[2], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x13972d870>
+xnew, λnew = newton_step(xguess[:,end],λguess[end])
+xguess = [xguess xnew]
+λguess = [λguess λnew]
+plot_landscape()
+plot(xguess[1,:], xguess[2,:], "rx")
+"""
+Note no line search implemented, so may have overshot
+
+I am using a linearization of the nonconvex constraint, I am going to have to lie on the tangent line.
+
+I.e I moved towards the minimizer on the linearized constraint!
+"""
+
+"Note no line search implemented, so may have overshot\n\nI am using a linearization of the nonconvex constraint, I am going to have to lie on the tangent line.\n\nI.e I moved towards the minimizer on the linearized constraint!\n"+
xguess = [-3; 2]
+λguess = [0.0]
+plot_landscape()
+plot(xguess[1], xguess[2], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x13b27a260>
+xnew, λnew = newton_step(xguess[:,end],λguess[end])
+xguess = [xguess xnew]
+λguess = [λguess λnew]
+plot_landscape()
+plot(xguess[1,:], xguess[2,:], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x175291510>
+Stuck at infeasible spot and cannot move.
+ +H = ∇2f(xguess[:,end]) + ForwardDiff.jacobian(x -> ∂c(x)'*λguess[end], xguess[:,end]) # Hessian of Lagrangian
+2×2 Matrix{Float64}:
+ -0.287879 0.0
+ 0.0 1.0
+∇2f(xguess[:,end])
+# Hessian of original objective. Recall we want this to postive definite.
+# However the hessian of the lagrangian is not PD, i.e it's indefinite
+# Takeaway is need to regulaize as we did not have convex constraint or maybe just test out guass-newton??
+2×2 Diagonal{Float64, Vector{Float64}}:
+ 0.5 ⋅
+ ⋅ 1.0
+function gauss_newton_step(x0,λ0)
+ H = ∇2f(x0)
+ C = ∂c(x0)
+ Δz = [H C'; C 0]\[-∇f(x0)-C'*λ0; -c(x0)]
+ Δx = Δz[1:2]
+ Δλ = Δz[3]
+ return x0+Δx, λ0+Δλ
+end
+gauss_newton_step (generic function with 1 method)+
xguess = [-3; 2]
+λguess = [0.0]
+plot_landscape()
+plot(xguess[1], xguess[2], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x175306fe0>
+xnew, λnew = gauss_newton_step(xguess[:,end],λguess[end])
+xguess = [xguess xnew]
+λguess = [λguess λnew]
+plot_landscape()
+plot(xguess[1,:], xguess[2,:], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x304019180>
+
+
+import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()
+ Activating project at `~/Desktop/Summer_2025/LearningToControlClass/class02`
+
+using LinearAlgebra
+using ForwardDiff
+using PyPlot
+Q = Diagonal([0.5; 1])
+# Set up QP
+function f(x)
+ return 0.5*(x-[1; 0])'*Q*(x-[1; 0])
+end
+function ∇f(x)
+ return Q*(x-[1; 0])
+end
+function ∇2f(x)
+ return Q
+end
+∇2f (generic function with 1 method)+
A = [-1.0 1.0]
+b = 1.0
+function c(x)
+ return dot(A,x) - b
+end
+function ∂c(x)
+ return A
+end
+∂c (generic function with 1 method)+
function plot_landscape()
+ Nsamp = 20
+ Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')
+ Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))
+ Zsamp = zeros(Nsamp,Nsamp)
+ for j = 1:Nsamp
+ for k = 1:Nsamp
+ Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])
+ end
+ end
+ contour(Xsamp,Ysamp,Zsamp)
+
+ xc = LinRange(-4,3,Nsamp)
+ plot(xc,xc.+1,"y")
+end
+
+# feasible region is defined by inequality constraint
+plot_landscape()
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x14628c220>
+# The residual function; i.e the F.O.N.C that needs to be zero
+function ip_residual(z, ρ)
+ x = z[1:2]
+ σ = z[3]
+ r = [∇f(x) - ∂c(x)'*sqrt(ρ)*exp(-σ);
+ c(x) - sqrt(ρ)exp(σ)]
+end
+ip_residual (generic function with 1 method)+
# this is the actual true stock KKT system
+function kkt_residual(z)
+ x = z[1:2]
+ σ = z[3]
+ λ = sqrt(ρ)*exp(-σ)
+
+ r = [∇f(x) - ∂c(x)'*λ;
+ min(λ, 0)
+ min(c(x),0)
+ λ*c(x)]
+end
+kkt_residual (generic function with 1 method)+
xguess = [-2; 2] # we are in the interior
+σguess = 0.0
+z = [xguess; σguess]
+plot_landscape()
+plot(z[1], z[2], "rx")
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x14cf15810>
+ρ = 1.0
+ip_residual(z,ρ)
+3×1 Matrix{Float64}:
+ -0.5
+ 1.0
+ 2.0
+kkt_residual(z)
+5×1 Matrix{Float64}:
+ -0.5
+ 1.0
+ 0.0
+ 0.0
+ 3.0
+function newton_solve(z0,ρ,tol)
+
+ #initial guess
+ z = z0
+
+ #KKT residual
+ r = ip_residual(z,ρ)
+
+ # while not converged
+ while norm(r) > tol
+ #H = ∇2f(x)
+ #C = ∂c(x)
+
+ #M = [H sqrt(ρ)*C'*exp(-σ);
+ # C -sqrt(ρ)*exp(σ)]
+
+ #Newton step: evaluate jacobian of residal
+ M = ForwardDiff.jacobian(dz->ip_residual(dz,ρ), z)
+ Δz = -M\r
+
+ znew = z + Δz
+ rnew = ip_residual(znew,ρ)
+
+ #Line search with armijo
+ b = 0.1
+ c = 0.5
+ α = 1.0
+ while norm(rnew) > (norm(r) + b*α*dot(r,M*Δz)/norm(r))
+ α = c*α
+ znew = z + α*Δz
+ rnew = ip_residual(znew,ρ)
+ end
+
+ z = znew
+ r = rnew
+ end
+
+ return z
+end
+newton_solve (generic function with 1 method)+
z_iter = z
+3-element Vector{Float64}:
+ -2.0
+ 2.0
+ 0.0
+ρ = 1.0
+z = newton_solve(z_iter[:,end],ρ,1e-10) # a relaxed solution
+z_iter = [z_iter z]
+3×2 Matrix{Float64}:
+ -2.0 -1.0
+ 2.0 1.0
+ 0.0 0.0
+kkt_residual(z)
+# super cool, basically the residual error's are zero everywhere except the complementarity as that is what we relaxed
+5×1 Matrix{Float64}:
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 1.0
+ip_residual(z,ρ)
+3×1 Matrix{Float64}:
+ 0.0
+ 0.0
+ 0.0
+plot_landscape()
+plot(z_iter[1,:], z_iter[2,:], "rx") # Need to keep on driving rho down closer to 0 to approach constraint
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x16f708220>
+ρ = 1.0e-8 #adjust from ρ=1 to ρ=1e-8 to observe convergence along central path. Warm start with prior iter
+z = newton_solve(z_iter[:,end],ρ,1e-10)
+z_iter = [z_iter z]
+3×3 Matrix{Float64}:
+ -2.0 -1.0 -0.333333
+ 2.0 1.0 0.666667
+ 0.0 0.0 -8.80488
+kkt_residual(z) # amazing, super close to now satisfying the original kkt conditions
+5×1 Matrix{Float64}:
+ 2.8343993818680246e-13
+ -2.8343993818680246e-13
+ 0.0
+ 0.0
+ 1.000000001422954e-8
+plot_landscape()
+plot(z_iter[1,:], z_iter[2,:], "rx")
+# the path that the solution follows as you crank rho down is called the "central path"
+
+1-element Vector{PyCall.PyObject}:
+ PyObject <matplotlib.lines.Line2D object at 0x16f77ac20>
+M = ForwardDiff.jacobian(dz->ip_residual(dz,ρ), z)
+3×3 Matrix{Float64}:
+ 0.5 0.0 -0.666667
+ 0.0 1.0 0.666667
+ -1.0 1.0 -1.5e-8
+eigvals(M)
+3-element Vector{Float64}:
+ -0.8526850467837964
+ 0.7169780264777998
+ 1.6357070053059972
+
+