46 lines
958 B
Julia
46 lines
958 B
Julia
using NLsolve
|
|
|
|
export nlp_solve, mass_est
|
|
|
|
function mass_est(T)
|
|
ans = 0
|
|
n = Int(length(T)/3)
|
|
for i in 1:n ans += norm(T[i,:]) end
|
|
return ans/n
|
|
end
|
|
|
|
struct Result
|
|
converged::Bool
|
|
zero::Matrix{Float64}
|
|
end
|
|
|
|
function nlp_solve(start::Vector{Float64},
|
|
final::Vector{Float64},
|
|
craft::Sc,
|
|
μ::Float64,
|
|
t0::Float64,
|
|
tf::Float64,
|
|
x0::Matrix{Float64};
|
|
tol=1e-6,
|
|
num_iters=1_000)
|
|
|
|
function f!(F,x)
|
|
try
|
|
F .= 0.0
|
|
F[1:6, 1] .= prop(tanh.(x), start, copy(craft), μ, tf-t0)[2][1:6] .- final[1:6]
|
|
catch e
|
|
F .= 10000000.0
|
|
end
|
|
end
|
|
|
|
result = Result(false, zeros(size(x0)))
|
|
try
|
|
nl_results = nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=num_iters)
|
|
result = Result(converged(nl_results), tanh.(nl_results.zero))
|
|
catch e
|
|
end
|
|
|
|
return result
|
|
|
|
end
|