diff --git a/Project.toml b/Project.toml index 32e95861..c3a4bfbe 100644 --- a/Project.toml +++ b/Project.toml @@ -1,12 +1,11 @@ name = "NEOs" uuid = "b41c07a2-2abb-11e9-070a-c3c1b239e7df" authors = ["Jorge A. Pérez Hernández", "Luis Benet", "Luis Eduardo Ramírez Montoya"] -version = "0.10.1" +version = "0.11.0" [deps] Artifacts = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" AutoHashEquals = "15f4f7f2-30c1-5605-9d31-71845cf9641f" -Clustering = "aaaa29a8-35af-508c-8bc3-b662a17a0fe5" DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0" Dates = "ade2ca70-3891-5945-98fb-dc099432e06a" DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" @@ -42,7 +41,6 @@ NEOsRecipesBaseExt = "RecipesBase" [compat] Artifacts = "1" AutoHashEquals = "2" -Clustering = "0.15" DataFrames = "1.5" Dates = "1" DelimitedFiles = "1" diff --git a/pha/apophis.jl b/pha/apophis.jl index a34d67f3..b778fe48 100644 --- a/pha/apophis.jl +++ b/pha/apophis.jl @@ -41,7 +41,7 @@ function parse_commandline() @add_arg_table! s begin "--jd0" - help = "Initial epoch calendar date (UTC)" + help = "Initial epoch calendar date (TDB)" arg_type = DateTime default = DateTime(2020, 12, 17) "--varorder" @@ -51,7 +51,7 @@ function parse_commandline() "--maxsteps" help = "Maximum number of steps during integration" arg_type = Int - default = 10_000 + default = 100_000 "--nyears_bwd" help = "Years in backward integration" arg_type = Float64 @@ -132,7 +132,7 @@ function main(dynamics::D, maxsteps::Int, jd0_datetime::DateTime, nyears_bwd::T, print_header("Main integration", 2) tmax = nyears_bwd*yr println("• Initial time of integration: ", string(jd0_datetime)) - println("• Final time of integration: ", jdtdb2utc(jd0 + tmax)) + println("• Final time of integration: ", jdtdb2dtutc(jd0 + tmax)) params = NEOParameters(;maxsteps, order, abstol, parse_eqs) sol_bwd = NEOs.propagate(dynamics, jd0, nyears_bwd, q0, params) @@ -141,11 +141,11 @@ function main(dynamics::D, maxsteps::Int, jd0_datetime::DateTime, nyears_bwd::T, tmax = nyears_fwd*yr println("• Initial time of integration: ", string(jd0_datetime)) - println("• Final time of integration: ", jdtdb2utc(jd0 + tmax)) + println("• Final time of integration: ", jdtdb2dtutc(jd0 + tmax)) sol_fwd, tvS, xvS, gvS = NEOs.propagate_root(dynamics, jd0, nyears_fwd, q0, params) jldsave("Apophis_fwd.jld2"; sol_fwd, tvS, xvS, gvS) - # sol_fwd = JLD2.load("Apophis_fwd.jld2", "sol_bwd") + # sol_fwd = JLD2.load("Apophis_fwd.jld2", "sol_fwd") println() # Load Sun ephemeris @@ -163,85 +163,90 @@ function main(dynamics::D, maxsteps::Int, jd0_datetime::DateTime, nyears_bwd::T, # Change x, v units, resp., from au, au/day to km, km/sec xvs(et) = auday2kmsec(eph_su(et/daysec)) + ### Process optical astrometry radec_2004_2020 = read_radec_mpc(joinpath(pkgdir(NEOs), "data", "99942_2004_2020.dat")) radec_2020_2021 = read_radec_mpc(joinpath(pkgdir(NEOs), "data", "99942_2020_2021.dat")) - radec = vcat(radec_2004_2020,radec_2020_2021) + radec = vcat(radec_2004_2020,radec_2020_2021) # 7941 optical observations - deldop_2005_2013 = read_radar_jpl(joinpath(pkgdir(NEOs), "data", "99942_RADAR_2005_2013.dat")) - deldop_2021 = read_radar_jpl(joinpath(pkgdir(NEOs), "data", "99942_RADAR_2021.dat")) - deldop = vcat(deldop_2005_2013,deldop_2021) + # Error model + w8s = Veres17(radec).w8s + bias = Eggl20(radec).bias # Compute optical residuals - opt_res_w_all = NEOs.residuals(radec; xvs, xve, xva) + opt_res_w_all = NEOs.residuals(radec, w8s, bias; xvs, xve, xva) res_radec_all = vcat(ra.(opt_res_w_all), dec.(opt_res_w_all)) - w_radec_all = vcat(NEOs.weight_ra.(opt_res_w_all), NEOs.weight_dec.(opt_res_w_all)) + w_radec_all = vcat(NEOs.wra.(opt_res_w_all), NEOs.wdec.(opt_res_w_all)) jldsave("Apophis_res_w_radec.jld2"; res_radec_all, w_radec_all) # JLD2.@load "Apophis_res_w_radec.jld2" - # Compute radar residuals - res_del, w_del, res_dop, w_dop = NEOs.residuals(deldop; xvs, xve, xva, niter=10, tord=10) - jldsave("Apophis_res_w_deldop.jld2"; res_del, w_del, res_dop, w_dop) - # JLD2.@load "Apophis_res_w_deldop.jld2" - - ### Process optical astrometry (filter, weight, debias) - - # construct DataFrame from optical astrometry + # Construct DataFrame from optical astrometry df_radec = DataFrame(radec) - # add residuals and weights to optical astrometry DataFrame - df_radec[!, :res_α] .= res_radec_all[1:round(Int,length(res_radec_all)/2)] - df_radec[!, :res_δ] .= res_radec_all[1+round(Int,length(res_radec_all)/2):end] - df_radec[!, :w_α] .= w_radec_all[1:round(Int,length(res_radec_all)/2)] - df_radec[!, :w_δ] .= w_radec_all[1+round(Int,length(res_radec_all)/2):end] - # filter out biased observations from observatory 217 on 28-Jan-2021 + # Add residuals and weights to optical astrometry DataFrame + nradec = round(Int,length(res_radec_all)/2) + df_radec[!, :res_α] .= res_radec_all[1:nradec] + df_radec[!, :res_δ] .= res_radec_all[1+nradec:end] + df_radec[!, :w_α] .= w_radec_all[1:nradec] + df_radec[!, :w_δ] .= w_radec_all[1+nradec:end] + # Filter out biased observations from observatory 217 on 28-Jan-2021 filter!( x->(Date(x.date) != Date(2021, 1, 28)), df_radec - ) - - # read astrometric errors from Tholen et al. (2013) + ) # 7901 optical observations left + + # Tholen et al. (2013) obs table (432 observations) + _radec_tho13_ = read_radec_mpc(joinpath(pkgdir(NEOs), "test", "data", "99942_Tholen_etal_2013.dat")) + radec_tho13 = DataFrame(_radec_tho13_) + # Veres et al. (2017) relaxation factors for Tholen et al. (2013) optical astrometry + rex_tho13 = NEOs.rexveres17(_radec_tho13_) + # Read astrometric errors from Tholen et al. (2013) tho13_errors = readdlm(joinpath(pkgdir(NEOs), "data", "tholenetal2013_opterror.dat"), ',') - # compute weights - w_α_tho13 = 1 ./ (tho13_errors[:,1].^2 .+ tho13_errors[:,3].^2 .+ tho13_errors[:,5].^2) - w_δ_tho13 = 1 ./ (tho13_errors[:,2].^2 .+ tho13_errors[:,4].^2 .+ tho13_errors[:,6].^2) - # Tholen et al. (2013) obs table - radec_tho13 = DataFrame(read_radec_mpc(joinpath(pkgdir(NEOs), "test", "data", "99942_Tholen_etal_2013.dat"))) - # vector of RA values from Tholen et al. (2013) observations (used for filtering) + # Compute weights + w_α_tho13 = @. 1 / ((tho13_errors[:,1]^2 + tho13_errors[:,3]^2 + tho13_errors[:,5]^2) * rex_tho13^2) + w_δ_tho13 = @. 1 / ((tho13_errors[:,2]^2 + tho13_errors[:,4]^2 + tho13_errors[:,6]^2) * rex_tho13^2) + # Vector of RA values from Tholen et al. (2013) observations (used for filtering) tho13_α = radec_tho13[!,:α] - # set weights in Tholen et al. (2013) astrometry corresponding to associated uncertainties + # Set weights in Tholen et al. (2013) astrometry corresponding to associated uncertainties df_radec[in.(df_radec.α, Ref(tho13_α)),:w_α] = w_α_tho13 df_radec[in.(df_radec.α, Ref(tho13_α)),:w_δ] = w_δ_tho13 - # `opt_res_w_all_new` is a `Vector{OpticalResidual}` with the custom weights from Tholen et al. (2013) astrometry - # since time tags nor observation sites change, relax factors are the same - - # Relaxation factors `relax_factor` account for correlations in optical astrometry data - # for each observation batch, count the number of observations made in the same night by - # the same observatory. This is achieved via inflating uncertainties (i.e., relax weights) - # aprropriately for each residual. - # Ref: Veres et al. (2017) - opt_res_w_all_new = eltype(opt_res_w_all)[] - for i in 1:size(df_radec, 1) - push!(opt_res_w_all_new, - NEOs.OpticalResidual( - opt_res_w_all[i].ξ_α, - opt_res_w_all[i].ξ_δ, - df_radec[i,:w_α], - df_radec[i,:w_δ], - opt_res_w_all[i].relax_factor, - opt_res_w_all[i].outlier) - ) - end + # Assemble optical residuals and weights + res_radec = vcat(df_radec.res_α, df_radec.res_δ) + w_radec = vcat(df_radec.w_α, df_radec.w_δ) - # update optical residuals and weights (relaxation factors on weights are actually applied here) - res_radec, w_radec = NEOs.unfold(opt_res_w_all_new) + ### Process radar astrometry - ### Perform orbital fit to optical and radar astrometry data + deldop_2005_2013 = read_radar_jpl(joinpath(pkgdir(NEOs), "data", "99942_RADAR_2005_2013.dat")) + deldop_2021 = read_radar_jpl(joinpath(pkgdir(NEOs), "data", "99942_RADAR_2021.dat")) + deldop = vcat(deldop_2005_2013,deldop_2021) # 38 radar observations + + # Compute radar residuals + res_del, w_del, res_dop, w_dop = NEOs.residuals(deldop; xvs, xve, xva, niter=10, tord=10) + jldsave("Apophis_res_w_deldop.jld2"; res_del, w_del, res_dop, w_dop) + # JLD2.@load "Apophis_res_w_deldop.jld2" - res = vcat(res_radec, res_del, res_dop) - w = vcat(w_radec, w_del, w_dop) + # Assemble radar residuals and weights + res_deldop = vcat(res_del, res_dop) + w_deldop = vcat(w_del, w_dop) + + ### Perform orbital fit to optical and radar astrometry data - fit_OR8 = newtonls(res, w, zeros(get_numvars()), 10) + # Assemble residuals and weights + res = vcat(res_radec, res_deldop) + w = vcat(w_radec, w_deldop) + # Number of observations and parameters + nobs, npar = length(res), get_numvars() + # Target function and its derivatives + Q = nms(res, w) + GQ = TS.gradient(Q) + HQ = [TS.differentiate(GQ[j], i) for j in 1:npar, i in 1:npar] + x0 = zeros(npar) + dQ, d2Q = GQ(x0), HQ(x0) + # Least squares method and cache + lsmethod = Newton(nobs, npar, Q, GQ, HQ, dQ, d2Q) + lscache = LeastSquaresCache(x0, 1:npar, 10) + # Least squares fit + fit_OR8 = leastsquares!(lsmethod, lscache) x_OR8 = sol_fwd(sol_fwd.t0)(fit_OR8.x) σ_OR8 = sqrt.(diag(fit_OR8.Γ)).*scalings diff --git a/scripts/adam.jl b/scripts/adam.jl index d8f1a580..547ec9b3 100644 --- a/scripts/adam.jl +++ b/scripts/adam.jl @@ -44,7 +44,7 @@ end using NEOs, Dates, TaylorSeries, PlanetaryEphemeris, JLD2 using NEOs: RadecMPC, AdmissibleRegion, PropagationBuffer, OpticalResidual, attr2bary, propres!, boundary_projection, reduce_tracklets, arboundary, - indices + indices, _lsmethods function adam(od::ODProblem{D, T}, i::Int, A::AdmissibleRegion{T}, ρ::T, v_ρ::T, params::NEOParameters{T}; scale::Symbol = :linear, η::T = 25.0, @@ -78,6 +78,9 @@ end res = [zero(OpticalResidual{T, TaylorN{T}}) for _ in eachindex(idxs)] # Origin x0, x1 = zeros(T, 6), zeros(T, 6) + # Least squares cache and methods + lscache = LeastSquaresCache(x0, 1:4, 5) + lsmethods = _lsmethods(res, x0, 1:4) # Gradient of objective function wrt (ρ, v_ρ) g_t = Vector{T}(undef, 2) # First momentum @@ -103,7 +106,7 @@ end propres!(res, od, jd0 - ae[5]/c_au_per_day, q, params; buffer, idxs) iszero(length(res)) && break # Least squares fit - fit = tryls(res, x0, 5, 1:4) + fit = tryls(res, x0, lscache, lsmethods) !fit.success && break x1 .= fit.x # Current Q diff --git a/scripts/mcmov.jl b/scripts/mcmov.jl index a67d9b7f..5e9ae051 100644 --- a/scripts/mcmov.jl +++ b/scripts/mcmov.jl @@ -43,7 +43,7 @@ end using NEOs, Dates, TaylorSeries, PlanetaryEphemeris, JLD2 using NEOs: AdmissibleRegion, OpticalResidual, RadecMPC, PropagationBuffer, reduce_tracklets, argoldensearch, scaled_variables, attr2bary, - propres!, nobs + propres!, nobs, _lsmethods function mcmov(points::Vector{Tuple{T, T}}, A::AdmissibleRegion{T}, radec::Vector{RadecMPC{T}}, params::NEOParameters{T}, @@ -73,6 +73,8 @@ end res = [zero(OpticalResidual{T, TaylorN{T}}) for _ in eachindex(radec)] # Origin x0 = zeros(T, 6) + # Least squares cache and methods + lscache, lsmethods = LeastSquaresCache(x0, 1:4, 10), _lsmethods(res, x0, 1:4) # Manifold of variations mov = Matrix{T}(undef, 13, length(points)) # Iterate mov points @@ -90,7 +92,7 @@ end # Propagation and residuals propres!(res, od, jd0, q, params; buffer) # Least squares fit - fit = newtonls(res, x0, 10, 1:4) + fit = tryls(res, x0, lscache, lsmethods) # Objective function Q = nms(res) # Save results @@ -156,10 +158,12 @@ function main() push!(points_per_worker[k], point) k = mod1(k+1, Nw) end - println("• ", sum(length, points_per_worker), " domain points in the manifold of variations") + Np = sum(length, points_per_worker) + println("• ", Np, " points in the manifold of variations") # Manifold of variations movs = pmap(p -> mcmov(p, A, radec, params, varorder), points_per_worker) mov = reduce(hcat, movs) + println("• ", count(!isnan, view(mov, 13, :)), "/", Np, " points converged") # Save results tmpdesig = radec[1].tmpdesig diff --git a/scripts/names.jl b/scripts/names.jl index d4032615..b0f32ffd 100644 --- a/scripts/names.jl +++ b/scripts/names.jl @@ -48,7 +48,7 @@ function isodcompatible(radec::Vector{RadecMPC{T}}) where {T <: Real} # Filter out incompatible observations filter!(radec) do r hascoord(r.observatory) && !issatellite(r.observatory) && - date(r) >= Date(2000) + date(r) > DateTime(2000, 1, 1, 12) end length(radec) < 3 && return false # There is at least one set of 3 tracklets with a < 15 days timespan @@ -94,7 +94,7 @@ function main() # Delete repeated NEOs unique!(provdesig) # We can only process asteroids discovered between 2000 and 2024 - filter!(desig -> "2000" <= desig[1:4] <= "2024", provdesig) + # filter!(desig -> "2000" <= desig[1:4] <= "2024", provdesig) if 0 < N < length(provdesig) # Shuffle the provisional designations list diff --git a/scripts/orbitdetermination.jl b/scripts/orbitdetermination.jl index 69bdf8b0..cf7edd5f 100644 --- a/scripts/orbitdetermination.jl +++ b/scripts/orbitdetermination.jl @@ -34,8 +34,8 @@ end @everywhere begin using NEOs, Dates, JLD2 - using NEOs: AdmissibleRegion, RadecMPC, reduce_tracklets, numberofdays, - issatellite + using NEOs: AdmissibleRegion, RadecMPC, NEOSolution, reduce_tracklets, + numberofdays, issatellite function radecfilter(radec::Vector{RadecMPC{T}}) where {T <: Real} # Eliminate observations before oficial discovery @@ -45,7 +45,7 @@ end # Filter out incompatible observations filter!(radec) do r hascoord(r.observatory) && !issatellite(r.observatory) && - date(r) >= Date(2000) + date(r) > DateTime(2000, 1, 1, 12) end length(radec) < 3 && return false, radec # Find the first set of 3 tracklets with a < 15 days timespan @@ -60,8 +60,8 @@ end return numberofdays(radec) <= 15.0, radec end - # Default naive initial conditions for iod - function initcond(A::AdmissibleRegion{T}) where {T <: Real} + # Naive initial conditions for iod + function initcond1(A::AdmissibleRegion{T}) where {T <: Real} v_ρ = sum(A.v_ρ_domain) / 2 return [ (A.ρ_domain[1], v_ρ, :log), @@ -71,6 +71,37 @@ end ] end + function initcond2(A::AdmissibleRegion{T}) where {T <: Real} + v_ρ = sum(A.v_ρ_domain) / 2 + return [ + (A.ρ_domain[1], v_ρ, :linear), + (10^(sum(log10, A.ρ_domain) / 2), v_ρ, :linear), + (sum(A.ρ_domain) / 2, v_ρ, :linear), + (A.ρ_domain[2], v_ρ, :linear), + ] + end + + function ioditer() + # Parameters + params = Vector{NEOParameters{Float64}}(undef, 2) + params[1] = NEOParameters( + coeffstol = Inf, bwdoffset = 0.042, fwdoffset = 0.042, # Propagation + gaussorder = 6, gaussQmax = 2.0, # Gauss method + adamiter = 500, adamQtol = 1e-5, tsaQmax = 2.0, # ADAM + jtlsiter = 20, lsiter = 10, # Least squares + outrej = true, χ2_rec = 7.0, χ2_rej = 8.0, # Outlier rejection + fudge = 10.0, max_per = 34.0 + ) + params[2] = NEOParameters(params[1]; + coeffstol = 10.0, adamiter = 200, adamQtol = 0.01, + lsiter = 5 + ) + # Naive initial conditions + initcond = [initcond1, initcond2] + # Initial orbit determination iterator + return enumerate(Iterators.product(params, initcond)) + end + # Initial orbit determination routine function iod(neo::String, filename::String) # Download optical astrometry @@ -80,50 +111,29 @@ end !flag && return false # Orbit determination problem od = ODProblem(newtonian!, radec) - # Parameters - params = NEOParameters( - coeffstol = Inf, bwdoffset = 0.007, fwdoffset = 0.007, - gaussorder = 6, gaussQmax = 1.0, - adamiter = 500, adamQtol = 1e-5, tsaQmax = 2.0, - jtlsiter = 20, newtoniter = 10 - ) + # Pre-allocate solutions + sols = [zero(NEOSolution{Float64, Float64}) for _ in 1:4] # Start of computation init_time = now() - # Initial orbit determination - sol = orbitdetermination(od, params; initcond) - # Termination condition - if length(sol.res) == length(radec) && nrms(sol) < 1.5 - # Time of computation - Δ = (now() - init_time).value - # Save orbit - jldsave(filename; sol = sol, Δ = Δ) - # Sucess flag - return true + # Initial orbit determination cycle + for (i, j) in ioditer() + # Unfold + params, initcond = j + # Initial orbit determination + sols[i] = orbitdetermination(od, params; initcond) + # Termination condition + length(sols[i].res) == length(radec) && nrms(sols[i]) < 2.0 && break end - # Parameters - params = NEOParameters(params; - coeffstol = 10.0, bwdoffset = 0.007, fwdoffset = 0.007, - gaussorder = 6, gaussQmax = 1.0, - adamiter = 200, adamQtol = 0.01, tsaQmax = 2.0, - jtlsiter = 20, newtoniter = 5 - ) - # Initial orbit determination - _sol_ = orbitdetermination(od, params; initcond) # Time of computation Δ = (now() - init_time).value - # Choose best orbit - if length(sol.res) == length(_sol_.res) == length(radec) - sol = min(sol, _sol_) - elseif length(sol.res) == length(radec) - sol = sol - elseif length(_sol_.res) == length(radec) - sol = _sol_ - else - sol = zero(NEOSolution{Float64, Float64}) - end + # Select complete solutions + mask = findall(s -> length(s.res) == length(radec), sols) + isempty(mask) && return false + # Choose best solution + _, k = findmin(nrms, view(sols, mask)) # Save orbit - iszero(sol) && return false - jldsave(filename; sol = sol, Δ = Δ) + k = mask[k] + jldsave(filename; sol = sols[k], Δ = Δ, k = k) return true end end diff --git a/src/NEOs.jl b/src/NEOs.jl index cc0e6974..68cc6c4e 100644 --- a/src/NEOs.jl +++ b/src/NEOs.jl @@ -26,9 +26,9 @@ using PlanetaryEphemeris: TaylorInterpCallingArgs, TaylorInterpolant, daysec, yr numberofbodies, kmsec2auday, meanmotion, meananomaly, selecteph, getinterpindex using Healpix: ang2pixRing, Resolution using StatsBase: mean, std +using LinearAlgebra: inv! using LsqFit: curve_fit, vcov using Roots: find_zeros -using Clustering: kmeans using HORIZONS: smb_spk using OhMyThreads: tmap, tmap! using TaylorIntegration: RetAlloc, _determine_parsing!, _taylorinteg! @@ -60,7 +60,7 @@ export loadpeeph, bwdfwdeph export obsposECEF, obsposvelECI # Optical astrometry processing export UniformWeights, Veres17, Farnocchia15, Eggl20, compute_radec, unfold, - residuals, outlier + residuals, isoutlier # Radar astrometry processing export compute_delay, radar_astrometry # Asteroid dynamical models @@ -70,7 +70,8 @@ export propagate, propagate_lyap, propagate_root # Osculating export pv2kep, yarkp2adot # Least squares -export project, chi2, nms, nrms, diffcorr, newtonls, levenbergmarquardt, tryls +export LeastSquaresCache, Newton, DifferentialCorrections, LevenbergMarquardt, + project, chi2, nms, nrms, leastsquares, leastsquares!, tryls # NEOSolution export epoch, sigmas, snr, jplcompare, uncertaintyparameter # Too Short Arc @@ -82,13 +83,13 @@ export NEOParameters, ODProblem, curvature, issinglearc, orbitdetermination # B plane export valsecchi_circle, bopik, mtp # Outlier rejection -export outlier_rejection +export outlier_rejection! include("constants.jl") include("observations/observations.jl") include("propagation/propagation.jl") include("orbitdetermination/orbitdetermination.jl") -include("postprocessing/outlier_rejection.jl") +include("postprocessing/bplane.jl") include("init.jl") include("deprecated.jl") diff --git a/src/observations/process_radec.jl b/src/observations/process_radec.jl index dd334c94..3158ebb1 100644 --- a/src/observations/process_radec.jl +++ b/src/observations/process_radec.jl @@ -47,7 +47,7 @@ function evaluate(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, end return res_new end -(res::Vector{OpticalResidual{T, TaylorN{T}}})(x::Vector{T}) where {T <: Real} = +(res::AbstractVector{OpticalResidual{T, TaylorN{T}}})(x::Vector{T}) where {T <: Real} = evaluate(res, x) # Print method for OpticalResidual @@ -55,7 +55,7 @@ end # α: -138.79801 δ: -89.80025 # α: -134.79450 δ: -91.42509 (outlier) function show(io::IO, x::OpticalResidual) - outlier_flag = outlier(x) ? " (outlier)" : "" + outlier_flag = isoutlier(x) ? " (outlier)" : "" print(io, "α: ", @sprintf("%+.5f", cte(x.ξ_α)), " δ: ", @sprintf("%+.5f", cte(x.ξ_δ)), outlier_flag) end @@ -96,7 +96,11 @@ ra(res::OpticalResidual) = res.ξ_α dec(res::OpticalResidual) = res.ξ_δ wra(res::OpticalResidual) = res.w_α wdec(res::OpticalResidual) = res.w_δ -outlier(res::OpticalResidual) = res.outlier +isoutlier(res::OpticalResidual) = res.outlier +nout(res::AbstractVector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} = + count(isoutlier, res) +notout(res::AbstractVector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} = + count(!isoutlier, res) euclid3D(x::Vector{T}) where {T <: Real} = sqrt(dot3D(x, x)) function euclid3D(x::Vector{TaylorN{T}}) where {T <: Real} @@ -371,7 +375,7 @@ function residuals(radec::AbstractVector{RadecMPC{T}}, w8s::AbstractVector{T}, # Type of asteroid ephemeris U = typeof(a1_et1) # Vector of residuals - res = Vector{OpticalResidual{T, U}}(undef, length(radec)) + res = [zero(OpticalResidual{T, U}) for _ in eachindex(radec)] residuals!(res, radec, w8s, bias; xva, kwargs...) return res @@ -382,7 +386,7 @@ function residuals!(res::Vector{OpticalResidual{T, U}}, bias::AbstractVector{Tuple{T, T}}; xva::AstEph, kwargs...) where {AstEph, T <: Real, U <: Number} - tmap!(res, radec, w8s, bias) do obs, w8, bias + tmap!(res, radec, w8s, bias, isoutlier.(res)) do obs, w8, bias, outlier # Observed ra/dec α_obs = rad2arcsec(ra(obs)) # arcsec δ_obs = rad2arcsec(dec(obs)) # arcsec @@ -398,7 +402,7 @@ function residuals!(res::Vector{OpticalResidual{T, U}}, δ_obs - δ_comp - δ_corr, w8, w8, - false + outlier ) end diff --git a/src/orbitdetermination/admissibleregion.jl b/src/orbitdetermination/admissibleregion.jl index e647ec76..3990b419 100644 --- a/src/orbitdetermination/admissibleregion.jl +++ b/src/orbitdetermination/admissibleregion.jl @@ -308,7 +308,7 @@ function _georangerates(coeffs::Vector{T}, ρ::S) where {T, S <: Real} if d > 0 return [-sqrt(arG(coeffs, ρ)), sqrt(arG(coeffs, ρ))] elseif d == 0 - return zero(T) * arG(coeffs, ρ) + return zero(T) * [arG(coeffs, ρ)] else return Vector{T}(undef, 0) end diff --git a/src/orbitdetermination/gaussinitcond.jl b/src/orbitdetermination/gaussinitcond.jl index 9308ecf4..003c962a 100644 --- a/src/orbitdetermination/gaussinitcond.jl +++ b/src/orbitdetermination/gaussinitcond.jl @@ -280,7 +280,7 @@ function gauss_method(observatories::Vector{ObservatoryMPC{T}}, dates::Vector{Da c = -(μ_S^2)*(B*B) # Solve Lagrange equation - sol = solve_lagrange(a, b, c; niter = params.newtoniter) + sol = solve_lagrange(a, b, c; niter = params.lsiter) # Number of solutions n_sol = length(sol) @@ -482,7 +482,7 @@ function gaussiod(od::ODProblem{D, T}, params::NEOParameters{T}) where {D, T <: # Jet transport initial conditions q = solG[i].statevect .+ params.eph_su(jd0 - JD_J2000) # Jet Transport Least Squares - _sol_ = jtls(od, jd0, q, 0, params, true) + _sol_ = jtls(od, jd0, q, od.tracklets, params, true) # Update solution sol = updatesol(sol, _sol_, od.radec) # Termination condition @@ -491,7 +491,7 @@ function gaussiod(od::ODProblem{D, T}, params::NEOParameters{T}) where {D, T <: od.tracklets[2].nobs < 2 && continue jd0 = _adam!(od, 2, q, jd0, params) # Jet Transport Least Squares - _sol_ = jtls(od, jd0, q, 0, params, true) + _sol_ = jtls(od, jd0, q, od.tracklets, params, true) # Update solution sol = updatesol(sol, _sol_, od.radec) # Termination condition diff --git a/src/orbitdetermination/least_squares.jl b/src/orbitdetermination/least_squares.jl deleted file mode 100644 index 3e4067c7..00000000 --- a/src/orbitdetermination/least_squares.jl +++ /dev/null @@ -1,820 +0,0 @@ -@doc raw""" - LeastSquaresFit{T <: Real} - -A least squares fit. - -# Fields -- `success::Bool`: whether the routine converged or not. -- `x::Vector{T}`: deltas that minimize the objective function. -- `Γ::Matrix{T}`: covariance matrix. -- `routine::Symbol`: minimization routine (`:newton` or `:diffcorr`). -""" -@auto_hash_equals struct LeastSquaresFit{T <: Real} - success::Bool - x::Vector{T} - Γ::Matrix{T} - routine::Symbol - # Inner constructor - function LeastSquaresFit{T}(success::Bool, x::Vector{T}, Γ::Matrix{T}, - routine::Symbol) where {T <: Real} - return new{T}(success, x, Γ, routine) - end -end -# Outer constructor -function LeastSquaresFit(success::Bool, x::Vector{T}, Γ::Matrix{T}, - routine::Symbol) where {T <: Real} - return LeastSquaresFit{T}(success, x, Γ, routine) -end - -# Definition of zero LeastSquaresFit{T} -function zero(::Type{LeastSquaresFit{T}}) where {T <: Real} - return LeastSquaresFit{T}(false, Vector{T}(undef, 0), Matrix{T}(undef, 0, 0), :zero) -end - -# Print method for LeastSquaresFit -# Examples: -# Succesful Newton -# Succesful differential corrections -function show(io::IO, fit::LeastSquaresFit{T}) where {T <: Real} - success_s = fit.success ? "Succesful" : "Unsuccesful" - if fit.routine == :newton - routine_s = "Newton" - elseif fit.routine == :diffcorr - routine_s = "Differential Corrections" - elseif fit.routine == :lm - routine_s = "Levenberg-Marquardt" - else - routine_s = "Least Squares" - end - print(io, success_s, " ", routine_s) -end - -@doc raw""" - carpino_smoothing(n::T) where {T <: Real} - -Fudge term for rejection condition in [`outlier_rejection`](@ref). - -!!! reference - See page 253 of https://doi.org/10.1016/S0019-1035(03)00051-4. -""" -carpino_smoothing(n::T) where {T <: Real} = 400*(1.2)^(-n) - -@doc raw""" - outlier_rejection(ξs::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}; - χ2_rec::T = 7.0, χ2_rej::T = 8.0, α::T = 0.25, max_per::T = 10.) where {T <: Real} - -Outlier rejection algorithm. - -# Arguments - -- `ξs::Vector{OpticalResidual{T}}`: vector of residuals. -- `fit::LeastSquaresFit{T}`: least squares fit. -- `χ2_rec::T`: recovery condition. -- `χ2_rej::T`: rejection condition. -- `α::T`: scaling factor for maximum chi. -- `max_per::T`: maximum allowed drop percentage. - -!!! reference - See https://doi.org/10.1016/S0019-1035(03)00051-4. -""" -function outlier_rejection(res::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}; - χ2_rec::T = 7., χ2_rej::T = 8., α::T = 0.25, max_per::T = 10.) where {T <: Real} - - # Number of residuals - L = length(res) - # Evaluate residuals - eval_res = res(fit.x) - # Vector of χ2 - χ2s = Vector{T}(undef, L) - # Maximum χ2 (over non outliers) - χ2_max = zero(T) - # Number of non outliers - N_sel = 0 - - # Compute χ2s - for i in eachindex(χ2s) - # Weights of current residual - w_α, w_δ = res[i].w_α / res[i].relax_factor, res[i].w_δ / res[i].relax_factor - # Current observation covariance matrix - γ = [w_α zero(T); zero(T) w_δ] - # Current model matrix - A = hcat(TS.gradient(res[i].ξ_α)(fit.x), TS.gradient(res[i].ξ_δ)(fit.x)) - # Outlier sign - outlier_sign = res[i].outlier*2-1 - # Current residual covariance matrix - γ_ξ = γ + outlier_sign*(A')*fit.Γ*A - # Current residual - ξ = [eval_res[i].ξ_α, eval_res[i].ξ_δ] - # Current chi2 - χ2s[i] = ξ' * inv(γ_ξ) * ξ - # Update N_sel - if !res[i].outlier - N_sel += 1 - # Update maximum χ2 - if χ2s[i] > χ2_max - χ2_max = χ2s[i] - end - end - end - - # Maximum allowed drops - max_drop = ceil(Int, max_per * L / 100) - # Number of dropped residuals - N_drop = 0 - # New outliers - new_outliers = outlier.(res) - # Sort χ2s - idxs = sortperm(χ2s, rev = true) - # Rejection threshold - χ2_rej = max(χ2_rej + carpino_smoothing(N_sel), α*χ2_max) - - for i in idxs - if χ2s[i] > χ2_rej && N_drop < max_drop - new_outliers[i] = true - N_drop += 1 - elseif χ2s[i] < χ2_rec - new_outliers[i] = false - end - end - - return OpticalResidual.(ra.(res), dec.(res), weight_ra.(res), weight_dec.(res), - relax_factor.(res), new_outliers) -end - -@doc raw""" - project(y::Vector{TaylorN{T}}, fit::LeastSquaresFit{T}) where {T <: Real} - -Project `fit`'s covariance matrix into `y`. -""" -function project(y::Vector{TaylorN{T}}, fit::LeastSquaresFit{T}) where {T <: Real} - J = Matrix{T}(undef, get_numvars(), length(y)) - for i in eachindex(y) - J[:, i] = TS.gradient(y[i])(fit.x) - end - return (J') * fit.Γ * J -end - -@doc raw""" - chi2(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} - chi2(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - -Returns the chi square -```math -\chi^2 = \sum_{i=1}^m \frac{ \xi_i^2}{\sigma_i^2}, -``` -where ``\mathbf{w} = (1/\sigma_1^2,\ldots,1/\sigma_m^2)^T`` and ``\mathbf{\xi} = (\xi_1,\ldots,\xi_m)^T`` -are the vectors of weights and residuals respectively. - -# Arguments - -- `res::Vector{U}/Vector{OpticalResidual{T, U}}`: vector of residuals. -- `w::Vector{T}`: vector of weights. -""" -function chi2(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} - # Have as many residuals as weights - @assert length(res) == length(w) - # Chi square - return sum(w .* (res.^2)) -end -function chi2(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - _res_, _w_ = unfold(res) - return chi2(_res_, _w_) -end - -@doc raw""" - nms(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} - nms(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - nms(res::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}) where {T <: Real} - -Return the normalized chi square. See [`chi2`](@ref). -""" -nms(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} = chi2(res, w) / length(res) - -function nms(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - _res_, _w_ = unfold(res) - return nms(_res_, _w_) -end - -function nms(res::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}) where {T <: Real} - _res_, _w_ = unfold(res) - return nms(_res_(fit.x), _w_) -end - -@doc raw""" - nrms(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} - nrms(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - nrms(res::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}) where {T <: Real} - -Returns the normalized root mean square error -```math -\texttt{NRMS} = \sqrt{\frac{\chi^2}{m}}, -``` -where ``\chi^2`` is the chi square and ``\mathbf{\xi} = (\xi_1,\ldots,\xi_m)^T`` is the vector -of residuals. - -See also [`chi2`](@ref). - -# Arguments - -- `res::Vector{U}/Vector{OpticalResidual{T, U}}`: Vector of residuals. -- `w::Vector{T}`: Vector of weights. -- `fit::LeastSquaresFit{T}`: least squares fit. - -""" -function nrms(res::Vector{U}, w::Vector{T}) where {T <: Real, U <: Number} - # Have as many residuals as weights - @assert length(res) == length(w) - # Normalized root mean square error - return sqrt( chi2(res, w)/length(res) ) -end - -function nrms(res::Vector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} - _res_, _w_ = unfold(res) - return nrms(_res_, _w_) -end - -function nrms(res::Vector{OpticalResidual{T, TaylorN{T}}}, fit::LeastSquaresFit{T}) where {T <: Real} - _res_, _w_ = unfold(res) - return nrms(_res_(fit.x), _w_) -end - -@doc raw""" - BHC(res::Vector{TaylorN{T}}, w::Vector{T}, npar::Int) where {T <: Real} - -Returns the ``\mathbf{B}``, ``\mathbf{H}`` and ``\mathbf{C}`` arrays -```math -\mathbf{B} = \frac{\partial\mathbf{\xi}}{\partial\mathbf{x}_0}(\mathbf{x}_0), \quad -\mathbf{H} = \frac{\partial^2\mathbf{\xi}}{\partial\mathbf{x}_0^2}(\mathbf{x}_0) \quad \text{and} \quad -\mathbf{C} = \mathbf{B}^T\mathbf{W}\mathbf{B}, -``` -where ``\mathbf{x}_0 = (x_1,\ldots,x_n)^T`` and ``\mathbf{\xi} = (\xi_1,\ldots,\xi_m)^T`` -are the vectors of initial conditions and residuals respectively; and -``\mathbf{W} = \text{diag}(1/\sigma_1^2,\ldots,1/\sigma_m^2)`` is the weights matrix. - -``\mathbf{B}`` is called the design matrix and is of size ``m\times n``, ``\mathbf{H}`` is a three index -array of size ``m\times n\times n`` and ``\mathbf{C}`` is called the normal matrix and is of size ``n\times n``. - -# Arguments - -- `res::Vector{TaylorN{T}}`: vector of residuals. -- `w::Vector{T}`: vector of weights. -- `npar::Int`: degrees of freedom ``n``. - -!!! reference - See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. -""" -function BHC(res::Vector{TaylorN{T}}, w::Vector{T}, npar::Int) where {T <: Real} - # Number of observations - nobs = length(res) - - # Allocate memory for the three arrays - B_mat = Matrix{TaylorN{T}}(undef, nobs, npar) - H_mat = Array{TaylorN{T}}(undef, nobs, npar, npar) - C_mat = Array{TaylorN{T}}(undef, npar, npar) - - # Design matrix B - for i in 1:nobs - # Gradient of the i-th residual with respect to the initial conditions x_0 - B_mat[i,:] .= TaylorSeries.gradient(res[i]) - end - # H matrix - for i in 1:nobs - for j in 1:npar - # Gradient of the (i, j)-th element of B with respect to to the initial - # conditions x_0 - H_mat[i,j,:] .= TaylorSeries.gradient(B_mat[i,j]) - end - end - # Normal matrix C - sqrtw_B = sqrt.(w) .* B_mat - C_mat .= (sqrtw_B') * sqrtw_B - - return B_mat, H_mat, C_mat -end - -@doc raw""" - ξTH(w, res, H_mat, npar) - -Returns ``\mathbf{\xi}^T\mathbf{W}\mathbf{H}``, where ``\mathbf{\xi} = (\xi_1,\ldots,\xi_m)^T`` -is the vector of residuals, ``\mathbf{W} = \text{diag}(1/\sigma_1^2,\ldots,1/\sigma_m^2)`` is -the weights matrix and ``\mathbf{H}`` is the matrix of second derivatives of ``\mathbf{\xi}`` -with respect to the initial conditions. - -See also [`BHC`](@ref). - -# Arguments - -- `w`: Vector of weights. -- `res`: Vector or residuals. -- `H_mat`: matrix of second derivatives of ``\mathbf{\xi}`` with respect to the initial conditions. -- `npar`: Degrees of freedom ``n``. -""" -function ξTH(w, res, H_mat, npar) - # Allocate memory for output - ξTHv = Array{Float64}(undef, npar, npar) - - for j in 1:npar - for i in 1:npar - # transpose(ξ) * W * H matrix - ξTHv[i,j] = (w .* res)' * (H_mat[:,i,j]) - end - end - - return ξTHv -end - -@doc raw""" - diffcorr(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 5) where {T <: Real} - diffcorr(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5) where {T <: Real} - -Differential corrections subroutine for least-squares fitting. Returns an -`LeastSquaresFit` with the `niters`-th -correction -```math -\mathbf{x}_{k+1} = \mathbf{x}_k - \mathbf{C}^{-1}\mathbf{D}, -``` -and the covariance matrix -```math -\mathbf{\Gamma} = \mathbf{C}^{-1}, -``` -where ``\mathbf{C} = \mathbf{B}^T\mathbf{W}\mathbf{B}`` is the normal matrix and -``\mathbf{D} = \mathbf{B}^T\mathbf{W}\mathbf{\xi}``, with ``\mathbf{\xi} = (\xi_1,\ldots,\xi_m)^T`` -the vector of residuals, ``\mathbf{B}`` the design matrix and ``\mathbf{W} = \text{diag}(1/\sigma_1^2,\ldots,1/\sigma_m^2)`` -the weights matrix. - -See also [`BHC`](@ref). - -# Arguments - -- `res::Vector{TaylorN{T}/Vector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. -- `w::Vector{T}`: vector of weights. -- `x_0::Vector{T}`: first guess. -- `niters::Int`: number of iterations. - -!!! reference - See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. -""" -function diffcorr(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 5, idxs::AbstractVector{Int} = eachindex(x0)) where {T <: Real} - # Degrees of freedom - npar = length(x0) - # Design matrix B, H array and normal matrix C - B_mat, H_mat, C_mat = BHC(res, w, npar) - # D matrix: transpose(B) * W * ξ - D_mat = B_mat' * (w .* res) - # ξTH_mat = ξTH(w, res, H_mat, npar) - # Vector of x - x = Matrix{T}(undef, npar, niters + 1) - # First guess - for i in axes(x, 2) - x[:, i] .= x0 - end - # Vector of errors - error = Vector{T}(undef, niters + 1) - # Error of first guess - error[1] = T(Inf) - # Iteration - for i in 1:niters - # Current x - xi = x[:, i] - # D matrix evaluated in xi - D = D_mat(xi)[idxs] - # C matrix evaluated in xi - C = C_mat(xi)[idxs, idxs] #.+ ξTH_mat(xi) - # Update rule - Δx = - inv(C)*D - # New x - x[idxs, i+1] = xi[idxs] + Δx - # Error - error2 = ( (Δx') * (C*Δx) ) / npar - if error2 ≥ 0 - error[i+1] = sqrt(error2) - # The method do not converge - else - return LeastSquaresFit(false, x[:, i+1], inv(C), :diffcorr) - end - end - # Index with the lowest error - i = argmin(error) - # x with the lowest error - x_new = x[:, i] - # Normal C matrix evaluated in x_new - C = C_mat(x_new) - # Covariance matrix - Γ = inv(C[idxs, idxs]) - - if any(diag(Γ) .< 0) - return LeastSquaresFit(false, x_new, Γ, :diffcorr) - else - return LeastSquaresFit(true, x_new, Γ, :diffcorr) - end -end - -function diffcorr(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5, idxs::AbstractVector{Int} = eachindex(x0)) where {T <: Real} - # Unfold residuals and weights - _res_, _w_ = unfold(res) - - return diffcorr(_res_, _w_, x0, niters, idxs) -end - -@doc raw""" - newtonls(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 5) where {T <: Real} - newtonls(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5) where {T <: Real} - -Newton method subroutine for least-squares fitting. Returns an `LeastSquaresFit` -with the `niters`-th iteration -```math -\mathbf{x}_{k+1} = \mathbf{x}_k - -\left(\frac{\partial^2 Q}{\partial\mathbf{x}_0^2}\right)^{-1} -\frac{\partial Q}{\partial\mathbf{x}_0}, -``` -and the covariance matrix -```math -\mathbf{\Gamma} = \mathbf{C}^{-1}, -``` -where ``\mathbf{C} = \frac{m}{2}\frac{\partial^2 Q}{\partial\mathbf{x}_0^2}`` is the normal -matrix, ``Q = \frac{\chi^2}{m}`` is the mean square residual, ``m`` is the number of -observations and ``\mathbf{x}_0 = (x_1,\ldots,x_n)`` is the vector of initial conditions. - -See also [`chi2`](@ref). - -# Arguments - -- `res::Vector{TaylorN{T}/Vector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. -- `w::Vector{T}`: vector of weights. -- `x_0::Vector{T}`: first guess. -- `niters::Int`: number of iterations. - -!!! reference - See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. -""" -function newtonls(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 5, idxs::AbstractVector{Int} = eachindex(x0)) where {T <: Real} - # Number of observations - nobs = length(res) - # Degrees of freedom - npar = length(x0) - # Mean square residual - Q = chi2(res, w)/nobs - # Vector of x - x = Matrix{T}(undef, npar, niters + 1) - # First guess - for i in axes(x, 2) - x[:, i] .= x0 - end - # Vector of errors - error = Vector{T}(undef, niters + 1) - # Error of first guess - error[1] = T(Inf) - # Iteration - for i in 1:niters - # Current x - xi = x[:, i] - # Gradient of Q with respect to x - dQ = TaylorSeries.gradient(Q)(xi)[idxs] - # Hessian of Q with respect to x - d2Q = TaylorSeries.hessian(Q, xi)[idxs, idxs] - # Newton update rule - Δx = - inv(d2Q)*dQ - # New x - x[idxs, i+1] = xi[idxs] + Δx - # Normal matrix - C = d2Q/(2/nobs) # C = d2Q/(2/m) - # Error - error2 = ( (Δx') * (C*Δx) ) / npar - if error2 ≥ 0 - error[i+1] = sqrt(error2) - # The method do not converge - else - return LeastSquaresFit(false, x[:, i+1], inv(C), :newton) - end - end - # TO DO: study Gauss method solution dependence on jt order - # TO DO: try even varorder - # TO DO: study optimal number of iterations - - # Index with the lowest error - i = argmin(error) - # x with the lowest error - x_new = x[:, i] - # Normal matrix - C = TaylorSeries.hessian(Q, x_new)/(2/nobs) # C = d2Q/(2/m) - # Covariance matrix - Γ = inv(C[idxs, idxs]) - - if any(diag(Γ) .< 0) - return LeastSquaresFit(false, x_new, Γ, :newton) - else - return LeastSquaresFit(true, x_new, Γ, :newton) - end -end - -function newtonls(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5, idxs::AbstractVector{Int} = eachindex(x0)) where {T <: Real} - # Unfold residuals and weights - _res_, _w_ = unfold(res) - - return newtonls(_res_, _w_, x0, niters, idxs) -end - -# In-place Levenberg-Marquardt hessian -function lmhessian!(_d2Q_::AbstractMatrix{T}, d2Q::AbstractMatrix{T}, λ::T) where {T <: AbstractFloat} - k = 1 + λ - for j in axes(d2Q, 2) - for i in axes(d2Q, 1) - if i == j - _d2Q_[i, j] = k * d2Q[i, j] - else - _d2Q_[i, j] = d2Q[i, j] - end - end - end - return nothing -end - -@doc raw""" - levenbergmarquardt(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 500) where {T <: Real} - levenbergmarquardt(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 500) where {T <: Real} - -Levenberg-Marquardt method subroutine for least-squares fitting. Returns an `LeastSquaresFit` -with the best iteration of `niters` iterations. - -See also [`chi2`](@ref). - -# Arguments - -- `res::Vector{TaylorN{T}/Vector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. -- `w::Vector{T}`: vector of weights. -- `x_0::Vector{T}`: first guess. -- `niters::Int`: number of iterations. - -!!! reference - See section 15.5.2 of https://books.google.com.mx/books?id=1aAOdzK3FegC&lpg=PP1&pg=PP1#v=onepage&q&f=false. -""" -function levenbergmarquardt(res::Vector{TaylorN{T}}, w::Vector{T}, x0::Vector{T}, - niters::Int = 500, idxs::AbstractVector{Int} = eachindex(x0), - λ::T = 0.001) where {T <: Real} - # Number of observations - nobs = length(res) - # Degrees of freedom - npar = length(x0) - # Normalized mean square residual - Q = chi2(res, w)/nobs - # Vector of Qs - Qs = fill(T(Inf), niters + 1) - # Gradient of Q with respect to x - dQ = TaylorSeries.gradient(Q) - # Pre-allocate memory - _dQ_ = zeros(T, npar) - _d2Q_ = zeros(T, npar, npar) - x = Matrix{T}(undef, npar, niters + 1) - # First guess - for i in axes(x, 2) - x[:, i] .= x0 - end - # Iteration - for i in 1:niters - # Current x - xi = x[:, i] - # Current Q - Qs[i] = Q(xi) - # Convergence condition - i > 1 && abs(Qs[i] - Qs[i-1]) / Qs[i-1] < 0.001 && break - # Gradient of Q with respect to x - _dQ_ .= dQ(xi) - # Hessian of Q with respect to x - d2Q = TaylorSeries.hessian(Q, xi) - # Choose λ - for _ in 1:niters - # Modified Hessian - lmhessian!(_d2Q_, d2Q, λ) - # Levenberg-Marquardt step - Δx = - inv(_d2Q_[idxs, idxs]) * _dQ_[idxs] - # Update point - x[idxs, i+1] = xi[idxs] + Δx - # Choose λ - if 0 < Q(x[:, i+1]) < Qs[i] && isposdef(_d2Q_[idxs, idxs]) - λ /= 10 - x[idxs, i+1] = xi[idxs] + Δx - break - else - λ *= 10 - end - end - end - # Index with the lowest error - i = argmin(Qs) - # x with the lowest error - x_new = x[:, i] - # Hessian of Q with respect to x - d2Q = TaylorSeries.hessian(Q, x_new) - # Normal matrix - if isposdef(d2Q) - C = d2Q/(2/nobs) - else - lmhessian!(_d2Q_, d2Q, λ) - C = _d2Q_/(2/nobs) # C = d2Q/(2/m) - end - # Covariance matrix - Γ = inv(C[idxs, idxs]) - - if any(diag(Γ) .< 0) - return LeastSquaresFit(false, x_new, Γ, :lm) - else - return LeastSquaresFit(true, x_new, Γ, :lm) - end -end - -function levenbergmarquardt(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 500, idxs::AbstractVector{Int} = eachindex(x0), - λ::T = 0.001) where {T <: Real} - # Unfold residuals and weights - _res_, _w_ = unfold(res) - - return levenbergmarquardt(_res_, _w_, x0, niters, idxs, λ) -end - -@doc raw""" - tryls(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5) where {T <: Real} - -Return the best least squares fit between three routines: [`newtonls`](@ref), -[`diffcorr`](@ref) and [`levenbergmarquardt`](@ref). - -# Arguments - -- `res::Vector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. -- `x_0::Vector{T}`: first guess. -- `niters::Int`: number of iterations. -""" -function tryls(res::Vector{OpticalResidual{T, TaylorN{T}}}, x0::Vector{T}, - niters::Int = 5, idxs::AbstractVector{Int} = eachindex(x0), - order::Vector{Symbol} = [:newton, :diffcorr, :lm]) where {T <: Real} - # Allocate memory - fit = zero(LeastSquaresFit{T}) - # Least squares methods in order - for i in eachindex(order) - if order[i] == :newton - fit = newtonls(res, x0, niters, idxs) - elseif order[i] == :diffcorr - fit = diffcorr(res, x0, niters, idxs) - elseif order[i] == :lm - fit = levenbergmarquardt(res, x0, niters, idxs) - end - fit.success && break - end - - return fit -end - -# TO DO: update / deprecate the following three functions - -@doc raw""" - newtonls_Q(Q, nobs, x0, niters=5) - -Does the same as `newtonls`, but recives ``Q`` as an argument, instead of computing it. -Returns the `niters`-th iteration and the covariance matrix ``\Gamma``. - -See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. - -See also [`newtonls`](@ref). - -# Arguments - -- `Q`: Mean square residual. -- `nobs`: Number of observations. -- `x_0`: First guess for the initial conditions. -- `niters`: Number of iterations. -""" -function newtonls_Q(Q, nobs, x0, niters=5) - # Number of observations - npar = length(x0) - # First guess - x_new = x0 - # Iteration - for i in 1:niters - # Gradient of Q with respect to x_0 - dQ = TaylorSeries.gradient(Q)(x_new) - # Hessian of Q with respect to x_0 - d2Q = TaylorSeries.hessian(Q, x_new) - # Newton update rule - Δx = - inv(d2Q)*dQ - x_new = x_new + Δx - # Normal matrix - C = d2Q/(2/nobs) # C = d2Q/(2/m) - @show sqrt(((Δx')*(C*Δx))/npar) - end - # Normal matrix - C = TaylorSeries.hessian(Q, x_new)/(2/nobs) # C = d2Q/(2/m) - # Covariance matrix - Γ = inv(C) - - return x_new, Γ -end - -@doc raw""" - newtonls_6v(res, w, x0, niters=5) - -Specialized version of `newtonls` on 6 variables for parametrized orbit determination -with respect to ``A_2`` Yarkovsky non-gravitational coefficient. Returns the `niters`-th -iteration and the covariance matrix ``\Gamma``. - -See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. - -See also [`newtonls`](@ref). - -# Arguments - -- `res`: Vector of residuals. -- `w`: Vector of weights. -- `x_0`: First guess for the initial conditions. -- `niters`: Number of iterations. -""" -function newtonls_6v(res, w, x0, niters=5) - # Have as many residuals as weights - @assert length(res) == length(w) - # Number of observations - nobs = length(res) - # Degrees of freedom - npar = 6 # length(x0) - # Mean square residual - Q = chi2(res, w)/nobs - # First guess - x_new = x0 - # Iteration - for i in 1:niters - # Gradient of Q with respect to x_0 - dQ = TaylorSeries.gradient(Q)(x_new)[1:6] - # Hessian of Q with respect to x_0 - d2Q = TaylorSeries.hessian(Q, x_new)[1:6,1:6] - # Newton update rule - Δx = - inv(d2Q)*dQ - x_new[1:6] = x_new[1:6] + Δx - # Normal matrix - C = d2Q/(2/nobs) # C = d2Q/(2/m) - @show sqrt(((Δx')*(C*Δx))/npar) - end - # Normal matrix - C = TaylorSeries.hessian(Q, x_new)/(2/nobs) # C = d2Q/(2/m) - # Covariance matrix - Γ = inv(C[1:6,1:6]) - - return x_new, Γ -end - -@doc raw""" - newtonls_A2(res, w, x0, niters=5) - -Specialized version of `newtonls` with the Newton method only over the seventh degree of -freedom, i.e., with respect to ``A_2`` Yarkovsky non-gravitational coefficient. Returns the -`niters`-th iteration, the covariance matrix ``\Gamma`` and the normal matrix ``C``. - -See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. - -See also [`newtonls`](@ref). - -# Arguments - -- `res`: Vector of residuals. -- `w`: Vector of weights. -- `x_0`: First guess for the initial conditions. -- `niters`: Number of iterations. -""" -function newtonls_A2(res, w, x0, niters=5) - # Have as many residuals as weights - @assert length(res) == length(w) - # Number of observations - nobs = length(res) - # Degrees of freedom - npar = length(x0) - # Mean square residual - Q = chi2(res, w)/nobs - # First guess - x_new = x0 - # Iteration - for i in 1:niters - # Gradient of Q with respect to x_0 - dQ = TaylorSeries.gradient(Q)(x_new) - # Hessian of Q with respect to x_0 - d2Q = TaylorSeries.hessian(Q, x_new) - # Newton update rule - Δx = - inv(d2Q)*dQ - x_new[7] = x_new[7] + Δx[7] - # Normal matrix - C = d2Q/(2/nobs) # C = d2Q/(2/m) - @show sqrt(((Δx')*(C*Δx))/npar) - end - # Normal matrix - C = TaylorSeries.hessian(Q, x_new)/(2/nobs) # C = d2Q/(2/m) - # Covariance matrix - Γ = inv(C) - - return x_new, Γ, C -end diff --git a/src/orbitdetermination/leastsquares/fit.jl b/src/orbitdetermination/leastsquares/fit.jl new file mode 100644 index 00000000..404f0cd0 --- /dev/null +++ b/src/orbitdetermination/leastsquares/fit.jl @@ -0,0 +1,70 @@ +@doc raw""" + AbstractLeastSquares{T <: Real} + +Supertype for the least squares API. +""" +abstract type AbstractLeastSquares{T <: Real} end + +@doc raw""" + LeastSquaresFit{T} <: AbstractLeastSquares{T} + +A least squares fit. + +## Fields + +- `success::Bool`: whether the routine converged or not. +- `x::Vector{T}`: deltas that minimize the objective function. +- `Γ::Matrix{T}`: covariance matrix. +- `routine::Symbol`: minimization routine. +""" +@auto_hash_equals struct LeastSquaresFit{T} <: AbstractLeastSquares{T} + success::Bool + x::Vector{T} + Γ::Matrix{T} + routine::Symbol + # Inner constructor + LeastSquaresFit(success::Bool, x::Vector{T}, Γ::Matrix{T}, + routine::Symbol) where {T <: Real} = new{T}(success, x, Γ, routine) +end + +# Outer constructor +LeastSquaresFit(::Type{T}, id::AbstractString) where {T <: Real} = + LeastSquaresFit(false, Vector{T}(undef, 0), Matrix{T}(undef, 0, 0), Symbol(id)) + +# Definition of zero LeastSquaresFit{T} +zero(::Type{LeastSquaresFit{T}}) where {T <: Real} = + LeastSquaresFit(false, Vector{T}(undef, 0), Matrix{T}(undef, 0, 0), + Symbol("Least Squares")) + +# Print method for LeastSquaresFit +# Examples: +# Succesful Newton +# Succesful Differential Corrections +function show(io::IO, fit::LeastSquaresFit) + success_s = fit.success ? "Succesful" : "Unsuccesful" + routine_s = String(fit.routine) + print(io, success_s, " ", routine_s) +end + +@doc raw""" + project(y::Vector{TaylorN{T}}, fit::LeastSquaresFit{T}) where {T <: Real} + +Project the covariance matrix of `fit` into `y`. +""" +function project(y::Vector{TaylorN{T}}, fit::LeastSquaresFit{T}) where {T <: Real} + J = Matrix{T}(undef, get_numvars(), length(y)) + for i in eachindex(y) + J[:, i] = TS.gradient(y[i])(fit.x) + end + return (J') * fit.Γ * J +end + +# Target functions +chi2(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + fit::LeastSquaresFit{T}) where {T <: Real} = chi2(res(fit.x)) + +nms(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + fit::LeastSquaresFit{T}) where {T <: Real} = nms(res(fit.x)) + +nrms(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + fit::LeastSquaresFit{T}) where {T <: Real} = nrms(res(fit.x)) \ No newline at end of file diff --git a/src/orbitdetermination/leastsquares/methods.jl b/src/orbitdetermination/leastsquares/methods.jl new file mode 100644 index 00000000..1eaab753 --- /dev/null +++ b/src/orbitdetermination/leastsquares/methods.jl @@ -0,0 +1,504 @@ +include("targetfunctions.jl") +include("fit.jl") + +@doc raw""" + AbstractLeastSquaresCache{T} <: AbstractLeastSquares{T} + +Supertype for the least squares caches API. +""" +abstract type AbstractLeastSquaresCache{T} <: AbstractLeastSquares{T} end + +struct LeastSquaresCache{T} <: AbstractLeastSquaresCache{T} + x0::Vector{T} + xs::Matrix{T} + Qs::Vector{T} + idxs::Vector{Int} + maxiter::Int + # Inner constructor + function LeastSquaresCache(x0::Vector{T}, idxs::AbstractVector{Int}, + maxiter::Int = 25) where {T <: Real} + xs = Matrix{T}(undef, length(x0), maxiter + 1) + Qs = Vector{T}(undef, maxiter + 1) + return new{T}(x0, xs, Qs, idxs, maxiter) + end +end + +include("outlierrejection.jl") + +@doc raw""" + AbstractLeastSquaresMethod{T} <: AbstractLeastSquares{T} + +Supertype for the least squares methods API. +""" +abstract type AbstractLeastSquaresMethod{T} <: AbstractLeastSquares{T} end + +# Least squares main loop +function leastsquares!(ls::LS, cache::LeastSquaresCache{T}) where {T <: Real, + LS <: AbstractLeastSquaresMethod{T}} + # Allocate memory for least squares fit + fit = LeastSquaresFit(T, getid(ls)) + # Unfold + x0, xs, Qs, idxs = cache.x0, cache.xs, cache.Qs, cache.idxs + maxiter = cache.maxiter + # Initial conditions + for j in axes(xs, 2) + xs[:, j] .= x0 + end + Qs[1] = TS.evaluate(ls.Q, x0) + Qs[1] < 0 && return fit + Qs[2:end] .= T(Inf) + # Main loop + for i in 1:maxiter + # Least squares step + Δx, flag = lsstep(ls, xs[:, i]) + !flag && return fit + # Update rule + xs[idxs, i+1] .= xs[idxs, i] + Δx + Qs[i+1] = TS.evaluate(ls.Q, xs[:, i+1]) + Qs[i+1] < 0 && return fit + # TO DO: break if Q has not changed significantly in + # 3-5 iterations + end + # Choose best iteration + i = argmin(Qs) + # Normal matrix + C = normalmatrix(ls, xs[:, i]) + # Covariance matrix + # TO DO: use pinv for badly conditioned normal matrices + Γ = inv(C) + # Update fit + if all(diag(Γ) .> 0) + fit = LeastSquaresFit(true, xs[:, i], Γ, Symbol(getid(ls))) + end + + return fit +end + +@doc raw""" + leastsquares(method::LS, res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T} [, idxs::AbstractVector{Int}]; kwargs...) where {LS, T <: Real} + +Use least squares method `LS` to minimize the normalized mean square residual of `res`, +starting from initial guess `x0`. If `idxs` (default: `eachindex(x0)`) is given, +perform the minimization over a subset of the parameters. + +## Keyword arguments + +- `maxiter::Int`: maximum number of iterations (default: `25`). +""" +function leastsquares(method::LS, res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int} = eachindex(x0); + maxiter::Int = 25) where {LS, T <: Real} + # Consistency checks + @assert length(idxs) ≤ length(x0) == get_numvars() + # Initialize least squares method and cache + ls = method(res, x0, idxs) + cache = LeastSquaresCache(x0, idxs, maxiter) + # Main loop + fit = leastsquares!(ls, cache) + + return fit +end + +mutable struct Newton{T} <: AbstractLeastSquaresMethod{T} + nobs::Int + npar::Int + Q::TaylorN{T} + GQ::Vector{TaylorN{T}} + HQ::Matrix{TaylorN{T}} + dQ::Vector{T} + d2Q::Matrix{T} +end + +@doc raw""" + Newton(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + +Return a `Newton{T}` object for least squares minimization. + +See also [`leastsquares`](@ref). + +## Arguments + +- `res::AbstractVector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. +- `x_0::Vector{T}`: initial guess. +- `idxs::AbstractVector{Int}`: subset of parameters for fit. + +!!! reference + See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. +""" +function Newton(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + nobs, npar = 2*notout(res), length(idxs) + # Mean square residual and its gradient + Q = nms(res) + GQ = TaylorSeries.gradient(Q)[idxs] + # Hessian + HQ = [zero(Q) for _ in 1:npar, _ in 1:npar] + for j in eachindex(idxs) + for i in eachindex(idxs) + HQ[i, j] = TS.differentiate(GQ[idxs[j]], idxs[i]) + end + end + # Evaluate gradient and hessian + dQ = TS.evaluate(GQ, x0) + d2Q = TS.evaluate(HQ, x0) + + return Newton{T}(nobs, npar, Q, GQ, HQ, dQ, d2Q) +end + +getid(::Newton) = "Newton" + +function lsstep(ls::Newton{T}, x::Vector{T}) where {T <: Real} + # Evaluate gradient and hessian + TS.evaluate!(ls.GQ, x, ls.dQ) + TS.evaluate!(ls.HQ, x, ls.d2Q) + # Invert hessian + lud2Q = lu!(ls.d2Q; check = false) + !issuccess(lud2Q) && return Vector{T}(undef, 0), false + invd2Q = inv!(lud2Q) + # Newton update rule + Δx = -invd2Q * ls.dQ + # Normal matrix + C = (ls.nobs/2) * ls.d2Q + # Error metric + error = (Δx') * C * Δx / ls.npar + + return Δx, error > 0 +end + +function normalmatrix(ls::Newton{T}, x::Vector{T}) where {T <: Real} + TS.evaluate!(ls.HQ, x, ls.d2Q) + return (ls.nobs/2) * ls.d2Q # C = d2Q / (2/m) +end + +function update!(ls::Newton{T}, res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + ls.nobs, ls.npar = 2*notout(res), length(idxs) + # Mean square residual and its gradient + ls.Q = nms(res) + TS.zero!.(ls.GQ) + ls.GQ .= TaylorSeries.gradient(ls.Q)[idxs] + # Hessian + TS.zero!.(ls.HQ) + for j in eachindex(idxs) + for i in eachindex(idxs) + ls.HQ[i, j] = TS.differentiate(ls.GQ[idxs[j]], idxs[i]) + end + end + # Evaluate gradient and hessian + TS.evaluate!(ls.GQ, x0, ls.dQ) + TS.evaluate!(ls.HQ, x0, ls.d2Q) + + return nothing +end + +mutable struct DifferentialCorrections{T} <: AbstractLeastSquaresMethod{T} + nobs::Int + npar::Int + Q::TaylorN{T} + D::Vector{TaylorN{T}} + C::Matrix{TaylorN{T}} + Dx::Vector{T} + Cx::Matrix{T} +end + +@doc raw""" + DifferentialCorrections(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + +Return a `DifferentialCorrections{T}` object for least squares minimization. + +See also [`leastsquares`](@ref). + +## Arguments + +- `res::AbstractVector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. +- `x_0::Vector{T}`: initial guess. +- `idxs::AbstractVector{Int}`: subset of parameters for fit. + +!!! reference + See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371. +""" +function DifferentialCorrections(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + nobs, npar = 2*notout(res), length(idxs) + # Mean square residual and its gradient + Q = nms(res) + # D matrix and normal matrix C + D, C = DCVB(res, idxs) + # Deprecated term + # C -> C + ξTH(res, idxs) + # Evaluate D and C matrices + Dx = TS.evaluate(D, x0) + Cx = TS.evaluate(C, x0) + + return DifferentialCorrections{T}(nobs, npar, Q, D, C, Dx, Cx) +end + +getid(::DifferentialCorrections) = "Differential Corrections" + +function lsstep(ls::DifferentialCorrections{T}, x::Vector{T}) where {T <: Real} + # Evaluate D and C matrices + TS.evaluate!(ls.D, x, ls.Dx) + TS.evaluate!(ls.C, x, ls.Cx) + # Invert C matrix + luCx = lu!(ls.Cx; check = false) + !issuccess(luCx) && return Vector{T}(undef, 0), false + invCx = inv!(luCx) + # Differential corrections update rule + Δx = -invCx * ls.Dx + # Error metric + error = (Δx') * ls.Cx * Δx / ls.npar + + return Δx, error > 0 +end + +function normalmatrix(ls::DifferentialCorrections{T}, x::Vector{T}) where {T <: Real} + TS.evaluate!(ls.C, x, ls.Cx) + return ls.Cx +end + +function update!(ls::DifferentialCorrections{T}, + res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + ls.nobs, ls.npar = 2*notout(res), length(idxs) + # Mean square residual and its gradient + ls.Q = nms(res) + # D matrix and normal matrix C + ls.D, ls.C = DCVB(res, idxs) + # Deprecated term + # C -> C + ξTH(res, idxs) + # Evaluate D and C matrices + TS.evaluate!(ls.D, x0, ls.Dx) + TS.evaluate!(ls.C, x0, ls.Cx) + + return nothing +end + +# D matrix and normal matrix C +# See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371 +function DCVB(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + nobs, npar = 2*notout(res), length(idxs) + # Allocate memory + V = Vector{TaylorN{T}}(undef, nobs) + D = Vector{TaylorN{T}}(undef, npar) + B = Matrix{TaylorN{T}}(undef, nobs, npar) + BT = Matrix{TaylorN{T}}(undef, npar, nobs) + C = Matrix{TaylorN{T}}(undef, npar, npar) + # Tranpose of the normalized design matrix B + for (j, k) in enumerate(findall(!isoutlier, res)) + # Normalized j-th ra and dec residuals + V[2j-1] = sqrt(res[k].w_α) * res[k].ξ_α + V[2j] = sqrt(res[k].w_δ) * res[k].ξ_δ + # Gradient of the j-th ra and dec residuals with respect to + # to the initial conditions x_0[idxs] + for i in eachindex(idxs) + BT[i, 2j-1] = TS.differentiate(V[2j-1], idxs[i]) + BT[i, 2j] = TS.differentiate(V[2j], idxs[i]) + end + end + # D matrix: transpose(B) * W * ξ + mul!(D, BT, V) + # Normal matrix C + transpose!(B, BT) + mul!(C, BT, B) + + return D, C, V, B +end + +# Deprecated term in differential corrections +# This function is not used, but it is included here for completeness +# See sections 5.2 and 5.3 of https://doi.org/10.1017/CBO9781139175371 +function ξTH(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + B::AbstractMatrix{TaylorN{T}}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + nobs, npar = 2*notout(res), length(idxs) + # Allocate memory + ξTHv = Array{T}(undef, npar, npar) + A = Matrix{T}(undef, 1, nobs) + H = Array{TaylorN{T}}(undef, nobs, npar, npar) + # H matrix and auxiliary array + for (i, k) in enumerate(findall(!isoutlier, res)) + A[1, 2i-1] = res[k].w_α * res[k].ξ_α + A[1, 2i] = res[k].w_δ * res[k].ξ_δ + for j in 1:npar + # Gradient of the (i, j)-th element of B with respect to the initial + # conditions x_0 + H[i, j, :] .= TS.gradient(B[i, j])[idxs] + end + end + # Deprecated term ξTH + for j in 1:npar + for i in 1:npar + # transpose(ξ) * W * H matrix + ξTHv[i, j] = A * H[:, i, j] + end + end + + return ξTHv, H +end + +mutable struct LevenbergMarquardt{T} <: AbstractLeastSquaresMethod{T} + nobs::Int + npar::Int + idxs::Vector{Int} + λ::T + Q::TaylorN{T} + GQ::Vector{TaylorN{T}} + HQ::Matrix{TaylorN{T}} + dQ::Vector{T} + d2Q::Matrix{T} +end + +@doc raw""" + LevenbergMarquardt(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + +Return a `LevenbergMarquardt{T}` object for least squares minimization. + +See also [`leastsquares`](@ref). + +## Arguments + +- `res::AbstractVector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. +- `x_0::Vector{T}`: initial guess. +- `idxs::AbstractVector{Int}`: subset of parameters for fit. + +!!! reference + See section 15.5.2 of https://numerical.recipes. +""" +function LevenbergMarquardt(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + nobs, npar = 2*notout(res), length(idxs) + # Damping factor + λ = T(0.001) + # Mean square residual and its gradient + Q = nms(res) + GQ = TaylorSeries.gradient(Q)[idxs] + # Hessian + HQ = [zero(Q) for _ in 1:npar, _ in 1:npar] + for j in eachindex(idxs) + for i in eachindex(idxs) + HQ[i, j] = TS.differentiate(GQ[idxs[j]], idxs[i]) + end + end + # Evaluate gradient and hessian + dQ = TS.evaluate(GQ, x0) + d2Q = TS.evaluate(HQ, x0) + + return LevenbergMarquardt{T}(nobs, npar, idxs, λ, Q, GQ, HQ, dQ, d2Q) +end + +getid(::LevenbergMarquardt) = "Levenberg-Marquardt" + +function lsstep(ls::LevenbergMarquardt{T}, x::Vector{T}) where {T <: Real} + # Evaluate gradient and hessian + TS.evaluate!(ls.GQ, x, ls.dQ) + TS.evaluate!(ls.HQ, x, ls.d2Q) + # Modified Hessian + for i in axes(ls.d2Q, 1) + ls.d2Q[i, i] *= (1 + ls.λ) + end + # Invert hessian + lud2Q = lu!(ls.d2Q; check = false) + !issuccess(lud2Q) && return Vector{T}(undef, 0), false + invd2Q = inv!(lud2Q) + # Levenberg-Marquardt update rule + Δx = -invd2Q * ls.dQ + _x_ = deepcopy(x) + _x_[ls.idxs] .+= Δx + # Update λ + if 0 < ls.Q(_x_) < ls.Q(x) + ls.λ /= 10 + else + ls.λ *= 10 + Δx .= zero(T) + end + + return Δx, true +end + +function normalmatrix(ls::LevenbergMarquardt{T}, x::Vector{T}) where {T <: Real} + TS.evaluate!(ls.HQ, x, ls.d2Q) + return (ls.nobs/2) * ls.d2Q # C = d2Q / (2/m) +end + +function update!(ls::LevenbergMarquardt{T}, + res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int}) where {T <: Real} + # Number of observations and degrees of freedom + ls.nobs, ls.npar = 2*notout(res), length(idxs) + # Damping factor + ls.λ = T(0.001) + # Mean square residual and its gradient + ls.Q = nms(res) + TS.zero!.(ls.GQ) + ls.GQ .= TaylorSeries.gradient(ls.Q)[idxs] + # Hessian + TS.zero!.(ls.HQ) + for j in eachindex(idxs) + for i in eachindex(idxs) + ls.HQ[i, j] = TS.differentiate(ls.GQ[idxs[j]], idxs[i]) + end + end + # Evaluate gradient and hessian + TS.evaluate!(ls.GQ, x0, ls.dQ) + TS.evaluate!(ls.HQ, x0, ls.d2Q) + + return nothing +end + +_lsmethods(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int} = eachindex(x0)) where {T <: Real} = + [Newton(res, x0, idxs), DifferentialCorrections(res, x0, idxs), + LevenbergMarquardt(res, x0, idxs)] + +@doc raw""" + tryls(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T} [, idxs::AbstractVector{Int}]; kwargs...) where {T <: Real} + +Try the least squares minimization of the normalized mean square residual of `res`, +starting from initial guess `x0`. If `idxs` (default: `eachindex(x0)`) is given, +perform the minimization over a subset of the parameters. + +See also [`LeastSquaresCache`](@ref), [`AbstractLeastSquaresMethod`](@ref), +[`_lsmethods`](@ref) and [`leastsquares!`](@ref). + +## Keyword Arguments + +- `maxiter::Int`: maximum number of iterations (default: `25`). +- `cache::LeastSquaresCache{T}`: least squares cache (default: + `LeastSquaresCache(x0, idxs, maxiter)`). +- `methods::Vector{AbstractLeastSquaresMethod}`: least squares routines to try + (default: `_lsmethods(res, x0, idxs)`). +""" +function tryls(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, cache::LeastSquaresCache{T}, + methods::Vector{AbstractLeastSquaresMethod{T}}) where {T <: Real} + # Allocate memory + fit = zero(LeastSquaresFit{T}) + # Least squares methods in order + for i in eachindex(methods) + update!(methods[i], res, x0, cache.idxs) + fit = leastsquares!(methods[i], cache) + fit.success && break + end + + return fit +end + +function tryls(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x0::Vector{T}, idxs::AbstractVector{Int} = eachindex(x0); + maxiter::Int = 25) where {T <: Real} + cache = LeastSquaresCache(x0, idxs, maxiter) + methods = _lsmethods(res, x0, idxs) + return tryls(res, x0, cache, methods) +end \ No newline at end of file diff --git a/src/orbitdetermination/leastsquares/outlierrejection.jl b/src/orbitdetermination/leastsquares/outlierrejection.jl new file mode 100644 index 00000000..f7b9375b --- /dev/null +++ b/src/orbitdetermination/leastsquares/outlierrejection.jl @@ -0,0 +1,125 @@ +struct OutlierRejectionCache{T} <: AbstractLeastSquaresCache{T} + mask::BitVector + eval_res::Vector{OpticalResidual{T, T}} + χ2s::Vector{T} + ξ::Vector{T} + γ::Matrix{T} + A::Matrix{T} + γ_ξ::Matrix{T} + L::Int + # Inner constructor + function OutlierRejectionCache(::Type{T}, L::Int) where {T <: Real} + mask = BitVector(undef, L) + eval_res = Vector{OpticalResidual{T, T}}(undef, L) + χ2s = Vector{T}(undef, L) + ξ = Vector{T}(undef, 2) + γ = zeros(T, 2, 2) + A = Matrix{T}(undef, get_numvars(), 2) + γ_ξ = Matrix{T}(undef, 2, 2) + return new{T}(mask, eval_res, χ2s, ξ, γ, A, γ_ξ, L) + end +end + +@doc raw""" + outlier_rejection!(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x::Vector{T}, Γ::Matrix{T} [, cache::OutlierRejectionCache{T}]; + kwargs...) where {T <: Real} + +Carpino et al. (2003) outlier rejection algorithm. + +See also [`carpino_smoothing`](@ref). + +## Arguments + +- `res::AbstractVector{OpticalResidual{T, TaylorN{T}}}`: vector of residuals. +- `x::Vector{T}`: evaluation deltas. +- `Γ::Matrix{T}`: covariance matrix. +- `cache::OutlierRejectionCache{T}`: see [`OutlierRejectionCache`](@ref). + +## Keyword arguments + +- `χ2_rec::T`: recovery threshold (default: `7.0`). +- `χ2_rej::T`: rejection threshold (default: `8.0`). +- `fudge::T`: rejection fudge term coefficient (default: `400.0`). +- `α::T`: scaling factor for maximum chi (default: `0.25`). +- `max_per::T`: maximum allowed drop percentage (default: `10.0`). + +!!! reference + See https://doi.org/10.1016/S0019-1035(03)00051-4. +""" +function outlier_rejection!(res::AbstractVector{OpticalResidual{T, TaylorN{T}}}, + x::Vector{T}, Γ::Matrix{T}, cache::OutlierRejectionCache{T} = + OutlierRejectionCache(T, length(res)); χ2_rec::T = 7.0, χ2_rej::T = 8.0, + fudge::T = 400.0, α::T = 0.25, max_per::T = 10.0) where {T <: Real} + # Number of residuals + L = length(res) + # Unfold + mask, eval_res, χ2s, ξ, γ, A, γ_ξ = view(cache.mask, 1:L), view(cache.eval_res, 1:L), + view(cache.χ2s, 1:L), cache.ξ, cache.γ, cache.A, cache.γ_ξ + # Outliers mask + map!(isoutlier, mask, res) + # Evaluate residuals + map!(r -> r(x), eval_res, res) + # Maximum χ2 (over non outliers) + χ2_max = zero(T) + # Compute χ2s + @inbounds for i in eachindex(χ2s) + # Outlier flag + outlier = mask[i] + # Current observation covariance matrix + γ[1, 1], γ[2, 2] = inv(res[i].w_α), inv(res[i].w_δ) + # Current model matrix + A[:, 1] = TS.gradient(res[i].ξ_α)(x) + A[:, 2] = TS.gradient(res[i].ξ_δ)(x) + # Outlier sign + outlier_sign = 2 * outlier - 1 + # Current residual covariance matrix + γ_ξ .= γ + outlier_sign * (A') * Γ * A + # Current residual + ξ .= eval_res[i].ξ_α, eval_res[i].ξ_δ + # Current chi2 + χ2s[i] = abs(ξ' * inv(γ_ξ) * ξ) + # Update maximum χ2 + if !outlier + χ2_max = max(χ2_max, χ2s[i]) + end + end + # Number of dropped (outliers) and selected residuals + N_drop = count(mask) + N_sel = L - N_drop + # Maximum allowed drops + max_drop = ceil(Int, max_per * L / 100) + # Sort χ2s + idxs = sortperm(χ2s, rev = true) + # Rejection threshold + χ2_rej = max(χ2_rej + carpino_smoothing(N_sel, fudge), α*χ2_max) + # Rejection / recovery loop + @inbounds for i in idxs + # Reject + if χ2s[i] > χ2_rej && N_drop < max_drop && !mask[i] + res[i] = OpticalResidual(res[i].ξ_α, res[i].ξ_δ, res[i].w_α, + res[i].w_δ, true) + N_drop += 1 + # Recover + elseif χ2s[i] < χ2_rec && mask[i] + res[i] = OpticalResidual(res[i].ξ_α, res[i].ξ_δ, res[i].w_α, + res[i].w_δ, false) + N_drop -= 1 + end + end + + return nothing +end + +@doc raw""" + carpino_smoothing(n::Int [, A::T]) where {T <: Real} + +Carpino et al. (2003) rejection condition fudge term +with coefficient `A` (default: `400.0`). + +See also [`outlier_rejection`](@ref). + +!!! reference + See page 253 of https://doi.org/10.1016/S0019-1035(03)00051-4. +""" +carpino_smoothing(n::Int, A::T = 400.0) where {T <: Real} = A*(1.2)^(-n) \ No newline at end of file diff --git a/src/orbitdetermination/leastsquares/targetfunctions.jl b/src/orbitdetermination/leastsquares/targetfunctions.jl new file mode 100644 index 00000000..999b9f3a --- /dev/null +++ b/src/orbitdetermination/leastsquares/targetfunctions.jl @@ -0,0 +1,49 @@ +@doc raw""" + chi2(res::AbstractVector{OpticalResidual{T, U}} [, + fit::LeastSquaresFit{T}]) where {T <: Real, U <: Number} + +Return the chi square of `res` +```math +\chi^2 = \sum_{i=1}^m w_i \xi_i^2, +``` +where ``\xi_i`` is the i-th optical residual with statistical weight ``w_i``. +If `fit` is given, evaluate the residuals in `fit.x`. +""" +chi2(res::AbstractVector{U}, w::AbstractVector{T}) where {T <: Real, U <: Number} = + sum(w .* (res .^ 2)) + +chi2(x::OpticalResidual{T, U}) where {T <: Real, U <: Number} = + !x.outlier * (x.w_α * x.ξ_α^2 + x.w_δ * x.ξ_δ^2) + +chi2(res::AbstractVector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} = + sum(chi2.(res)) + +@doc raw""" + nms(res::AbstractVector{OpticalResidual{T, U}} [, + fit::LeastSquaresFit{T}]) where {T <: Real, U <: Number} + +Return the normalized mean square error of `res`. If `fit` is given, +evaluate the residuals in `fit.x`. + +See [`chi2`](@ref). +""" +nms(res::AbstractVector{U}, w::AbstractVector{T}) where {T <: Real, U <: Number} = + chi2(res, w) / length(res) + +nms(res::AbstractVector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} = + chi2(res) / (2 * notout(res)) + +@doc raw""" + nrms(res::AbstractVector{OpticalResidual{T, U}} [, + fit::LeastSquaresFit{T}]) where {T <: Real, U <: Number} + +Return the normalized root mean square error of `res`. If `fit` is given, +evaluate the residuals in `fit.x`. + +See also [`chi2`](@ref) and [`nms`](@ref). +""" +nrms(res::AbstractVector{U}, w::AbstractVector{T}) where {T <: Real, U <: Number} = + sqrt(nms(res, w)) + +nrms(res::AbstractVector{OpticalResidual{T, U}}) where {T <: Real, U <: Number} = + sqrt(nms(res)) \ No newline at end of file diff --git a/src/orbitdetermination/neosolution.jl b/src/orbitdetermination/neosolution.jl index 26aec639..456e1a18 100644 --- a/src/orbitdetermination/neosolution.jl +++ b/src/orbitdetermination/neosolution.jl @@ -270,7 +270,7 @@ function uncertaintyparameter(od::ODProblem{D, T}, sol::NEOSolution{T, T}, # Propagation and residuals bwd, fwd, res = propres(od, jd0, q, params) # Orbit fit - fit = tryls(res, x0, params.newtoniter) + fit = tryls(res, x0; maxiter = params.lsiter) # Residuals space to barycentric coordinates jacobian. J = Matrix(TS.jacobian(dq)) # Update solution diff --git a/src/orbitdetermination/orbitdetermination.jl b/src/orbitdetermination/orbitdetermination.jl index fd07b2e6..36fb722a 100644 --- a/src/orbitdetermination/orbitdetermination.jl +++ b/src/orbitdetermination/orbitdetermination.jl @@ -1,5 +1,5 @@ include("osculating.jl") -include("least_squares.jl") +include("leastsquares/methods.jl") include("odproblem.jl") include("neosolution.jl") include("admissibleregion.jl") @@ -87,24 +87,53 @@ function propres!(res::Vector{OpticalResidual{T, U}}, od::ODProblem{D, T}, end end -# Initial subset of radec for jtls -function _initialtracklets(tracklets::Vector{Tracklet{T}}, i::Int) where {T <: Real} - if iszero(i) - tin = deepcopy(tracklets) - tout = Vector{Tracklet{T}}(undef, 0) - rin = sort!(reduce(vcat, indices.(tracklets))) - else - tin = [tracklets[i]] - tout = sort(tracklets, by = x -> abs( (x.date - tracklets[i].date).value ))[2:end] - rin = sort!(indices(tracklets[i])) - while length(rin) < 3 && !isempty(tout) - tracklet = popfirst!(tout) - push!(tin, tracklet) - sort!(tin) - rin = vcat(rin, tracklet.indices) - sort!(rin) +# Decide whether to start jtls or not +function isjtlsfit(od::ODProblem{D, T}, jd0::V, q::Vector{TaylorN{T}}, + params::NEOParameters{T}) where {D, T <: Real, V <: Number} + # Try plain propagation and residuals + bwd, fwd, res = propres(od, jd0, cte.(q), params) + isempty(res) && return false + # Check that orbit crosses all admissible regions + mask = BitVector(undef, length(od.tracklets)) + for i in eachindex(od.tracklets) + A = AdmissibleRegion(od.tracklets[i], params) + At0 = dtutc2days(A.date) + if At0 <= jd0 - JD_J2000 + q0 = bwd(At0) + else + q0 = fwd(At0) end + ρ, v_ρ = bary2topo(A, q0) + mask[i] = (ρ, v_ρ) in A + end + + return all(mask) +end + +# Initial subset of radec for jtls +function _initialtracklets(trksa::AbstractVector{Tracklet{T}}, + trksb::AbstractVector{Tracklet{T}}) where {T <: Real} + # Starting tracklets + tin = sort!(intersect(trksa, trksb)) + # Remaining tracklets + tout = sort!(setdiff(trksa, trksb)) + # Consistency check + @assert sort!(union(tin, tout)) == trksa + # Sort tout by absolute time to tin + et = mean(@. dtutc2et(date(tin))) + dts = @. abs(dtutc2et(date(tout)) - et) + permute!(tout, sortperm(dts)) + # Starting observations + rin = sort!(reduce(vcat, indices.(tin))) + # jtls needs at least three observations + while length(rin) < 3 && !isempty(tout) + tracklet = popfirst!(tout) + push!(tin, tracklet) + sort!(tin) + rin = vcat(rin, indices(tracklet)) + sort!(rin) end + return tin, tout, rin end @@ -117,7 +146,7 @@ function addradec!(::Val{true}, rin::Vector{Int}, fit::LeastSquaresFit{T}, params::NEOParameters{T}) where {T <: Real} while !isempty(tout) extra = indices(tout[1]) - fit_new = tryls(res[rin ∪ extra], x0, params.newtoniter) + fit_new = tryls(res[rin ∪ extra], x0; maxiter = params.lsiter) !fit_new.success && break fit = fit_new tracklet = popfirst!(tout) @@ -137,7 +166,7 @@ function addradec!(::Val{false}, rin::Vector{Int}, fit::LeastSquaresFit{T}, params::NEOParameters{T}) where {T <: Real} if nrms(res[rin], fit) < params.tsaQmax && !isempty(tout) extra = indices(tout[1]) - fit_new = tryls(res[rin ∪ extra], x0, params.newtoniter) + fit_new = tryls(res[rin ∪ extra], x0; maxiter = params.lsiter) !fit_new.success && return rin, fit fit = fit_new tracklet = popfirst!(tout) @@ -151,8 +180,9 @@ function addradec!(::Val{false}, rin::Vector{Int}, fit::LeastSquaresFit{T}, end @doc raw""" - jtls(od::ODProblem{D, T}, jd0::V, q::Vector{TayloN{T}}, i::Int, - params::NEOParameters{T} [, mode::Bool]) where {D, T <: Real, V <: Number} + jtls(od::ODProblem{D, T}, jd0::V, q::Vector{TayloN{T}}, + tracklets::AbstractVector{Tracklet{T}}, params::NEOParameters{T} + [, mode::Bool]) where {D, T <: Real, V <: Number} Compute an orbit via Jet Transport Least Squares. @@ -161,64 +191,86 @@ Compute an orbit via Jet Transport Least Squares. - `od::ODProblem{D, T}`: orbit determination problem. - `jd0::V`: reference epoch [Julian days TDB]. - `q::Vector{TaylorN{T}}`: jet transport initial condition. -- `i::Int`: index of `tracklets` to start least squares fit. +- `tracklets::AbstractVector{Tracklet{T}}`: initial tracklets for fit. - `params::NEOParameters{T}`: see `Jet Transport Least Squares Parameters` of [`NEOParameters`](@ref). - `mode::Bool`: `addradec!` mode (default: `true`). """ -function jtls(od::ODProblem{D, T}, jd0::V, q::Vector{TaylorN{T}}, i::Int, - params::NEOParameters{T}, mode::Bool = true) where {D, T <: Real, V <: Number} +function jtls(od::ODProblem{D, T}, jd0::V, q::Vector{TaylorN{T}}, + tracklets::AbstractVector{Tracklet{T}}, params::NEOParameters{T}, + mode::Bool = true) where {D, T <: Real, V <: Number} + # Decide whether to start jtls or not + start = isjtlsfit(od, jd0, q, params) + start || return zero(NEOSolution{T, T}) # Plain initial condition - q0 = constant_term.(q) + q0 = cte.(q) # JT tail dq = q - q0 - # Vector of O-C residuals - res = [zero(OpticalResidual{T, TaylorN{T}}) for _ in eachindex(od.radec)] # Propagation buffer buffer = PropagationBuffer(od, jd0, 1, nobs(od), q, params) # Origin x0 = zeros(T, 6) + # Vector of O-C residuals + res = [zero(OpticalResidual{T, TaylorN{T}}) for _ in eachindex(od.radec)] + # Least squares cache and methods + lscache = LeastSquaresCache(x0, 1:6, params.lsiter) + lsmethods = _lsmethods(res, x0, 1:6) # Initial subset of radec for orbit fit - tin, tout, rin = _initialtracklets(od.tracklets, i) - # Residuals space to barycentric coordinates jacobian - J = Matrix(TS.jacobian(dq)) - # Best orbit - best_sol, best_Q = zero(NEOSolution{T, T}), T(Inf) - # Convergence flag - flag = false + tin, tout, rin = _initialtracklets(od.tracklets, tracklets) + # Allocate memory for orbits + sols = [zero(NEOSolution{T, T}) for _ in 1:params.jtlsiter] + Qs = fill(T(Inf), params.jtlsiter) + # Outlier rejection + if params.outrej + orcache = OutlierRejectionCache(T, nobs(od)) + outs = zeros(Int, params.jtlsiter) + end # Jet transport least squares - for _ in 1:params.jtlsiter + for i in 1:params.jtlsiter # Initial conditions q = q0 + dq # Propagation & residuals bwd, fwd = propres!(res, od, jd0, q, params; buffer) iszero(length(res)) && break # Orbit fit - fit = tryls(res[rin], x0, params.newtoniter) + fit = tryls(res[rin], x0, lscache, lsmethods) !fit.success && break # Incrementally add observations to fit rin, fit = addradec!(Val(mode), rin, fit, tin, tout, res, x0, params) - # NRMS - Q = nrms(res, fit) - if abs(best_Q - Q) < 0.1 - flag = true + # Outlier rejection + if params.outrej + outlier_rejection!(view(res, rin), fit.x, fit.Γ, orcache; + χ2_rec = params.χ2_rec, χ2_rej = params.χ2_rej, + fudge = params.fudge, max_per = params.max_per) + end + # Update solution + Qs[i] = nrms(res, fit) + J = Matrix(TS.jacobian(dq, fit.x)) + sols[i] = evalfit(NEOSolution(tin, bwd, fwd, res[rin], fit, J)) + if params.outrej + outs[i] = notout(res) + end + # Convergence conditions + if i > 1 + C1 = abs(Qs[i-1] - Qs[i]) < 0.01 + C2 = params.outrej ? (outs[i-1] == outs[i]) : true + C1 && C2 && break end - # Update NRMS and initial conditions - best_Q = Q - J .= TS.jacobian(dq, fit.x) - best_sol = evalfit(NEOSolution(tin, bwd, fwd, res[rin], fit, J)) - flag && break + i > 2 && issorted(view(Qs, i-2:i)) && break # Update initial condition - q0 .= q(fit.x) + TS.evaluate!(q, fit.x, q0) end - - # Case: all solutions were unsuccesful - if isinf(best_Q) - return zero(NEOSolution{T, T}) - # Case: at least one solution was succesful + # Find complete solutions + mask = findall(s -> length(s.res) == length(od.radec), sols) + # Choose best solution + if isempty(mask) + _, k = findmin(nrms, sols) else - return best_sol + _, k = findmin(nrms, view(sols, mask)) + k = mask[k] end + + return sols[k] end # Default naive initial conditions for iod @@ -329,6 +381,5 @@ function orbitdetermination(od::ODProblem{D, T}, sol::NEOSolution{T, T}, # Jet Transport initial condition q = q0 + dq # Jet Transport Least Squares - _, i = findmin(t -> abs(dtutc2days(t.date) - sol.bwd.t0), od.tracklets) - return jtls(od, jd0, q, i, params) + return jtls(od, jd0, q, sol.tracklets, params, true) end \ No newline at end of file diff --git a/src/orbitdetermination/tooshortarc.jl b/src/orbitdetermination/tooshortarc.jl index cd444a3f..62cbc9cb 100644 --- a/src/orbitdetermination/tooshortarc.jl +++ b/src/orbitdetermination/tooshortarc.jl @@ -50,6 +50,9 @@ function adam(od::ODProblem{D, T}, i::Int, A::AdmissibleRegion{T}, ρ::T, v_ρ:: res = [zero(OpticalResidual{T, TaylorN{T}}) for _ in eachindex(idxs)] # Origin x0, x1 = zeros(T, 6), zeros(T, 6) + # Least squares cache and methods + lscache = LeastSquaresCache(x0, 1:4, 5) + lsmethods = _lsmethods(res, x0, 1:4) # Gradient of objective function wrt (ρ, v_ρ) g_t = Vector{T}(undef, 2) # First momentum @@ -75,7 +78,7 @@ function adam(od::ODProblem{D, T}, i::Int, A::AdmissibleRegion{T}, ρ::T, v_ρ:: propres!(res, od, jd0 - ae[5]/c_au_per_day, q, params; buffer, idxs) iszero(length(res)) && break # Least squares fit - fit = tryls(res, x0, 5, 1:4) + fit = tryls(res, x0, lscache, lsmethods) !fit.success && break x1 .= fit.x # Current Q @@ -154,7 +157,7 @@ function tsaiod(od::ODProblem{D, T}, params::NEOParameters{T}; # Jet Transport initial condition q = [q0[k] + scalings[k] * TaylorN(k, order = params.tsaorder) for k in 1:6] # Jet Transport Least Squares - _sol_ = jtls(od, jd0, q, i, params, false) + _sol_ = jtls(od, jd0, q, od.tracklets[i:i], params, false) # Update solution sol = updatesol(sol, _sol_, od.radec) # Termination condition diff --git a/src/postprocessing/outlier_rejection.jl b/src/postprocessing/outlier_rejection.jl deleted file mode 100644 index 187a29e2..00000000 --- a/src/postprocessing/outlier_rejection.jl +++ /dev/null @@ -1,160 +0,0 @@ -include("bplane.jl") - -@doc raw""" - residual_norm(x::OpticalResidual{T, T}) where {T <: Real} - -Return the contribution of `x` to the nrms. -""" -residual_norm(x::OpticalResidual{T, T}) where {T <: Real} = - x.w_α * x.ξ_α^2 + x.w_δ * x.ξ_δ^2 - -@doc raw""" - outlier_rejection(od::ODProblem{D, T}, sol::NEOSolution{T, T}, - params::NEOParameters{T}) where {D, T <: Real} - -Refine an orbit, computed by [`orbitdetermination`](@ref), -via propagation and/or outlier rejection. - -## Arguments - -- `od::ODProblem{D, T}`: an orbit determination problem. -- `sol::NEOSolution{T, T}`: orbit to be refined. -- `params::NEOParameters{T}`: see `Outlier Rejection Parameters` - of [`NEOParameters`](@ref). - -!!! warning - This function will change the (global) `TaylorSeries` variables. -""" -function outlier_rejection(od::ODProblem{D, T}, sol::NEOSolution{T, T}, - params::NEOParameters{T}) where {D, T <: Real} - # Check consistency between od and sol - @assert od.tracklets == sol.tracklets - # Julian day (TDB) to start propagation - jd0 = sol.bwd.t0 + PE.J2000 - # Initial conditions (T) - q0 = sol(sol.bwd.t0) - # Scaling factors - scalings = abs.(q0) ./ 10^6 - # Jet transport perturbation - dq = scaled_variables("δx", scalings; order = params.jtlsorder) - # Initial conditions (jet transport) - q = q0 .+ dq - # Propagation and residuals - bwd, fwd, res = propres(od, jd0, q, params) - iszero(length(res)) && return zero(NEOSolution{T, T}) - # Origin - x0 = zeros(T, 6) - # Orbit fit - fit = tryls(res, x0, params.newtoniter) - # Residuals space to barycentric coordinates jacobian - J = Matrix(TS.jacobian(dq)) - # NRMS (with 0 outliers) - Q_0 = nrms(res, fit) - - if Q_0 < 1 - return evalfit(NEOSolution(sol.tracklets, bwd, fwd, res, fit, J)) - end - - # Number of observations - N_radec = length(od.radec) - # Maximum allowed outliers - max_drop = ceil(Int, N_radec * params.max_per / 100) - # Boolean mask (0: included in fit, 1: outlier) - new_outliers = BitVector(zeros(Int, N_radec)) - - # Drop loop - for _ in 1:max_drop - # Contribution of each residual to nrms - norms = residual_norm.(res(fit.x)) - # Iterate norms from largest to smallest - idxs = sortperm(norms, rev = true) - - for j in idxs - if !new_outliers[j] - # Drop residual - new_outliers[j] = true - # Update residuals - res = OpticalResidual.(ra.(res), dec.(res), wra.(res), wdec.(res), - new_outliers) - # Update fit - fit = tryls(res, x0, params.newtoniter) - break - end - end - end - - # Outliers - idxs = Vector{Int}(undef, max_drop) - # NRMS - Qs = Vector{T}(undef, max_drop + 1) - # Number of outliers - N_outliers = Vector{T}(undef, max_drop + 1) - - # Recovery loop - for i in 1:max_drop - # NRMS of current fit - Qs[i] = nrms(res, fit) - # Number of outliers in current fit - N_outliers[i] = float(max_drop - i + 1) - # Contribution of each residual to nrms - norms = residual_norm.(res(fit.x)) - # Minimum norm among outliers - j = findmin(norms[new_outliers])[2] - # Find residual with minimum norm - j = findall(new_outliers)[j] - # Add j-th residual to outliers list - idxs[i] = j - # Recover residual - new_outliers[j] = false - # Update residuals - res = OpticalResidual.(ra.(res), dec.(res), wra.(res), wdec.(res), - new_outliers) - # Update fit - fit = tryls(res, x0, params.newtoniter) - end - # Add 0 outliers fit - Qs[end] = Q_0 - N_outliers[end] = zero(T) - - # Outlier rejection cannot reduce Q - if all(Qs .> 1.) - # Reset boolean mask - new_outliers[1:end] .= false - # Update residuals - res = OpticalResidual.(ra.(res), dec.(res), wra.(res), wdec.(res), - new_outliers) - # Update fit - fit = tryls(res, x0, params.newtoniter) - - return evalfit(NEOSolution(sol.tracklets, bwd, fwd, res, fit, J)) - end - - if max_drop > 1 - # Assemble points - points = Matrix{T}(undef, 2, max_drop + 1) - for i in eachindex(Qs) - points[1, i] = Qs[i] - points[2, i] = N_outliers[i] - end - # K-means clustering - cluster = kmeans(points, 2; init = [1, max_drop + 1]) - # Index of smallest cluster - i_0 = cluster.assignments[1] - # Find last fit of smallest cluster - i = findfirst(x -> x != i_0, cluster.assignments) - 1 - # Update outliers indexes - idxs = idxs[i:end] - end - - # Reset boolean mask - new_outliers[1:end] .= false - # Outliers - new_outliers[idxs] .= true - # Update residuals - res = OpticalResidual.(ra.(res), dec.(res), wra.(res), wdec.(res), - new_outliers) - # Update fit - fit = tryls(res, x0, params.newtoniter) - - return evalfit(NEOSolution(sol.tracklets, bwd, fwd, res, fit, J)) -end \ No newline at end of file diff --git a/src/propagation/parameters.jl b/src/propagation/parameters.jl index 55f1d961..1c9c21ad 100644 --- a/src/propagation/parameters.jl +++ b/src/propagation/parameters.jl @@ -37,15 +37,18 @@ Parameters for all orbit determination functions. ## Jet Transport Least Squares Parameters -- `jtlsiter::Int`: maximum number of iterations for jet transport least squares - (default: `5`). -- `newtoniter::Int`: number of iterations for differential corrections / Newton's method - (default: `5`). -- `jtlsorder::Int`: order of the jet transport perturbation (default: `5`). +- `lsiter::Int`: maximum number of iterations for `leastsquares` (default: `5`). +- `jtlsiter::Int`: maximum number of iterations for `jtls` (default: `5`). +- `jtlsorder::Int`: order of the jet transport perturbation in `jtls` (default: `5`). ## Outlier Rejection Parameters -- `max_per::T`: maximum allowed rejection percentage (default: `18.0`). +- `outrej::Bool`: whether to perform outlier rejection during least squares + iterations (default: `false`). +- `χ2_rec::T`: recovery threshold (default: `7.0`). +- `χ2_rej::T`: rejection threshold (default: `8.0`). +- `fudge::T`: rejection fudge term coefficient (default: `400.0`). +- `max_per::T`: maximum allowed rejection percentage (default: `10.0`). """ @kwdef struct NEOParameters{T <: Real} # Propagation Parameters @@ -73,11 +76,15 @@ Parameters for all orbit determination functions. tsaorder::Int = 6 tsaQmax::T = 1.5 # Jet Transport Least Squares Parameters + lsiter::Int = 5 jtlsiter::Int = 5 - newtoniter::Int = 5 jtlsorder::Int = 5 # Outlier Rejection Parameters - max_per::T = 18.0 + outrej::Bool = false + χ2_rec::T = 7.0 + χ2_rej::T = 8.0 + fudge::T = 400.0 + max_per::T = 10.0 end # Outer constructors diff --git a/test/aqua.jl b/test/aqua.jl index 24292333..6d69b5f9 100644 --- a/test/aqua.jl +++ b/test/aqua.jl @@ -24,7 +24,7 @@ using Aqua end @testset "Aqua tests (additional)" begin - Aqua.test_ambiguities(NEOs) + Aqua.test_ambiguities(NEOs, broken = true) Aqua.test_all( NEOs; ambiguities=false diff --git a/test/bplane.jl b/test/bplane.jl index dfb778bb..d241c28d 100644 --- a/test/bplane.jl +++ b/test/bplane.jl @@ -8,22 +8,17 @@ using Test using NEOs: propres -@testset "2018 LA" begin +@testset "B-plane" begin # Fetch optical astrometry radec = fetch_radec_mpc("2018 LA") # Parameters params = NEOParameters(coeffstol = Inf, bwdoffset = 0.007, fwdoffset = 0.007) # Orbit determination problem - od = ODProblem(newtonian!, radec[1:8]) + od = ODProblem(newtonian!, radec) # Initial Orbit Determination sol = orbitdetermination(od, params) - # Update parameters - params = NEOParameters(params; jtlsorder = 6) - # Refine orbit - NEOs.update!(od, radec) - sol = orbitdetermination(od, sol, params) # Radial velocity with respect to the Earth. function rvelea(t, fwd, params) @@ -33,7 +28,7 @@ using NEOs: propres return dot(rv[1:3], rv[4:6]) end - # Values by June 23, 2024 + # Values by Oct 12, 2024 # Time of close approach params = NEOParameters(params; fwdoffset = 0.3) diff --git a/test/orbitdetermination.jl b/test/orbitdetermination.jl index 4e5fe49a..928db3d0 100644 --- a/test/orbitdetermination.jl +++ b/test/orbitdetermination.jl @@ -7,7 +7,7 @@ using LinearAlgebra using Test using NEOs: NEOSolution, RadecMPC, reduce_tracklets, - indices, numberofdays + indices, numberofdays, nout function iodsubradec(radec::Vector{RadecMPC{T}}, N::Int = 3) where {T <: Real} tracklets = reduce_tracklets(radec) @@ -54,7 +54,7 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 9 - @test iszero(count(outlier.(sol.res))) + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success @test all( sigmas(sol) .< 9e-4 ) @@ -96,7 +96,7 @@ end @test isempty(sol1.t_fwd) && isempty(sol1.x_fwd) && isempty(sol1.g_fwd) # Vector of residuals @test length(sol1.res) == 43 - @test iszero(count(outlier.(sol1.res))) + @test iszero(nout(sol1.res)) # Least squares fit @test sol1.fit.success @test all( sigmas(sol1) .< 2e-4 ) @@ -149,7 +149,7 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 9 - @test iszero(count(outlier.(sol.res))) + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success @test all( sigmas(sol) .< 8e-5 ) @@ -317,7 +317,7 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 10 - @test iszero(count(outlier.(sol.res))) + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success @test all( sigmas(sol) .< 6e-3 ) @@ -345,14 +345,15 @@ end @test numberofdays(subradec) < 2.16 # Parameters - params = NEOParameters(bwdoffset = 0.007, fwdoffset = 0.007) + params = NEOParameters(bwdoffset = 0.007, fwdoffset = 0.007, + outrej = true, χ2_rec = 1.0, χ2_rej = 1.25, fudge = 0.0) # Orbit determination problem od = ODProblem(newtonian!, subradec) - # Initial Orbit Determination + # Initial Orbit Determination (with outlier rejection) sol = orbitdetermination(od, params) - # Values by Oct 1, 2024 + # Values by Oct 11, 2024 # Orbit solution @test isa(sol, NEOSolution{Float64, Float64}) @@ -371,12 +372,12 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 18 - @test count(outlier.(sol.res)) == 0 + @test nout(sol.res) == 2 # Least squares fit @test sol.fit.success - @test all( sigmas(sol) .< 3e-4 ) - @test all( snr(sol) .> 574) - @test nrms(sol) < 0.46 + @test all( sigmas(sol) .< 4e-3 ) + @test all( snr(sol) .> 50) + @test nrms(sol) < 0.22 # Jacobian @test size(sol.jacobian) == (6, 6) @test !isdiag(sol.jacobian) @@ -384,13 +385,12 @@ end # Compatibility with JPL JPL = [0.7673366466815864, 0.6484892781853565, 0.29323267343908294, -0.011023343781911974, 0.015392697071667377, 0.006528842022004942] - @test all(abs.(sol() - JPL) ./ sigmas(sol) .< 1.75) + @test all(abs.(sol() - JPL) ./ sigmas(sol) .< 0.07) # Add remaining observations NEOs.update!(od, radec) + # Refine orbit (with outlier rejection) sol1 = orbitdetermination(od, sol, params) - # Outlier rejection - sol1 = outlier_rejection(od, sol1, params) # Orbit solution @test isa(sol1, NEOSolution{Float64, Float64}) @@ -409,7 +409,7 @@ end @test isempty(sol1.t_fwd) && isempty(sol1.x_fwd) && isempty(sol1.g_fwd) # Vector of residuals @test length(sol1.res) == 21 - @test count(outlier.(sol1.res)) == 2 + @test nout(sol1.res) == 2 # Least squares fit @test sol1.fit.success @test all( sigmas(sol1) .< 3e-4 ) @@ -420,7 +420,7 @@ end @test isdiag(sol1.jacobian) @test maximum(sol1.jacobian) < 8e-7 # Compatibility with JPL - @test all(abs.(sol1() - JPL) ./ sigmas(sol1) .< 1.2e-3) + @test all(abs.(sol1() - JPL) ./ sigmas(sol1) .< 7e-4) # MPC Uncertainty Parameter @test uncertaintyparameter(od, sol1, params) == 8 end @@ -471,7 +471,7 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 7 - @test iszero(count(outlier.(sol.res))) + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success @test all( sigmas(sol) .< 3e-4 ) @@ -525,7 +525,7 @@ end @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals @test length(sol.res) == 18 - @test iszero(count(outlier.(sol.res))) + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success @test all( sigmas(sol) .< 2e-5 ) @@ -567,7 +567,7 @@ end @test isempty(sol1.t_fwd) && isempty(sol1.x_fwd) && isempty(sol1.g_fwd) # Vector of residuals @test length(sol1.res) == 97 - @test iszero(count(outlier.(sol1.res))) + @test iszero(nout(sol1.res)) # Least squares fit @test sol1.fit.success @test all( sigmas(sol1) .< 4e-7 ) @@ -598,7 +598,7 @@ end # Filter out incompatible observations filter!(radec) do r hascoord(r.observatory) && !issatellite(r.observatory) && - date(r) >= Date(2000) + date(r) > DateTime(2000, 1, 1, 12) end length(radec) < 3 && return false, radec # Find the first set of 3 tracklets with a < 15 days timespan @@ -614,18 +614,21 @@ end end # Fetch and filter optical astrometry - radec = fetch_radec_mpc("2011 UE256") - _, radec = radecfilter(radec) + radec = fetch_radec_mpc("2023 QR6") + flag, radec = radecfilter(radec) - @test length(radec) == 14 - @test numberofdays(radec) < 0.85 + @test flag + @test length(radec) == 6 + @test numberofdays(radec) < 6.22 # Parameters params = NEOParameters( - coeffstol = Inf, bwdoffset = 0.007, fwdoffset = 0.007, - gaussorder = 6, gaussQmax = 1.0, - adamiter = 500, adamQtol = 1e-5, tsaQmax = 2.0, - jtlsiter = 20, newtoniter = 10 + coeffstol = Inf, bwdoffset = 0.042, fwdoffset = 0.042, # Propagation + gaussorder = 6, gaussQmax = 2.0, # Gauss method + adamiter = 500, adamQtol = 1e-5, tsaQmax = 2.0, # ADAM + jtlsiter = 20, lsiter = 10, # Least squares + outrej = true, χ2_rec = 7.0, χ2_rej = 8.0, # Outlier rejection + fudge = 10.0, max_per = 34.0 ) # Orbit determination problem od = ODProblem(newtonian!, radec) @@ -633,7 +636,7 @@ end # Initial Orbit Determination sol = orbitdetermination(od, params; initcond = iodinitcond) - # Values by Oct 1, 2024 + # Values by Oct 25, 2024 # Orbit solution @test isa(sol, NEOSolution{Float64, Float64}) @@ -651,21 +654,21 @@ end @test all( norm.(sol.fwd.x, Inf) .< 2 ) @test isempty(sol.t_fwd) && isempty(sol.x_fwd) && isempty(sol.g_fwd) # Vector of residuals - @test length(sol.res) == 14 - @test iszero(count(outlier.(sol.res))) + @test length(sol.res) == 6 + @test iszero(nout(sol.res)) # Least squares fit @test sol.fit.success - @test all( sigmas(sol) .< 0.24 ) - @test all( snr(sol) .> 0.40) - @test nrms(sol) < 0.42 + @test all( sigmas(sol) .< 0.018 ) + @test all( snr(sol) .> 7.14) + @test nrms(sol) < 0.28 # Jacobian @test size(sol.jacobian) == (6, 6) - @test isdiag(sol.jacobian) - @test maximum(sol.jacobian) < 1e-5 + @test !isdiag(sol.jacobian) + @test maximum(sol.jacobian) < 0.003 # Compatibility with JPL - JPL = [1.5278518651683686, 0.9328047239405421, 0.3755795874791371, - -0.014356510695499359, 0.0002883092503924326, 0.002280143873309611] - @test all(abs.(sol() - JPL) ./ sigmas(sol) .< 1.33) + JPL = [0.8273620205094612, -0.8060976455411443, -0.650602108942513, + 0.016599531390362098, -0.0056141558047514955, 0.0028999599926801418] + @test all(abs.(sol() - JPL) ./ sigmas(sol) .< 0.53) end end \ No newline at end of file