hi-mel

#= 
Example Source: http://www.matthewpeterkelly.com/tutorials/trajectoryOptimization/cartPoleCollocation.svg
Learning trajectory optimization with a simple cartpole system
=#

using JuMP, Ipopt
using Plots

# Number of time steps, increase to reduce error at computation expense
N = 39

hi-mel

using ForwardDiff
using LinearAlgebra
using StaticArrays
using BenchmarkTools

function levenberg_marquardt(func, desired::SVector, θ_init::SVector, debug=false)
    """
    Finds the optimal input values θ to a function to approximate desired output values
    using the Levenberg-Marquardt Algorithm
    # Arguments