text:
mutable struct UKF
    # Measurement times
    ts::Vector{Float64}
    imuΔt::Float64
    gpsΔt::Float64

    # Measurement residuals
    rs::Matrix{Float64}

    # State estimates and covariance matrix diags
    txp::Vector{Float64}    # Epochs associated with entries in xhats and Ps
    ixp::Int                # Index associated with latest entry
    xhats::Matrix{Float64}  # Matrix of state estimates
    es::Matrix{Float64}     # Matrix of state estimation errors
    Ps::Matrix{Float64}     # Matrix of covariance matrix diagonals

    # Parameters
    α::Float64
    β::Float64
    κ::Float64
    λ::Float64
    γ::Float64

    # Augmented state vector size
    L::Int

    # Preallocate matricies
    χ::Matrix{Float64}
    χd::Matrix{Float64}
    Pa::SparseMatrixCSC{Float64, Int64}
    C::SuiteSparse.CHOLMOD.Factor{Float64}
    P⁺::Matrix{Float64}
    P⁻::Matrix{Float64}
    Py::Matrix{Float64}
    Pyt::Matrix{Float64}
    Pxy::Matrix{Float64}
    Q::Diagonal{Float64, Vector{Float64}}
    expmeas::Matrix{Float64}
    expmeasd::Matrix{Float64}
    yhat::Vector{Float64}

    # Simulators
    gpsSim::GPSSim
    imuSim::IMUSim
    scSim::SpacecraftSim

    # Measurement covariance terms
    σ2GPS::Float64
    σ2Acc::Float64

    # Step Index
    k::Int64

    # Update flag
    updated::Bool

    # Finished flag
    finished::Bool

    # Luna perturbation flag
    lunaPerts::Bool

end

function UKF(xhat0, P0, Q, σ2GPS, σ2Acc, ts, gpsΔt, gpsSim, imuSim, scSim;
    α = 1.0, β = 2.0, κ = 1.0, lunaPerts = false)

    # Instantiate preallocated matricies
    ixp     = 1
    rs      = zeros(length(ts), 35)
    txp     = zeros(2*length(ts) + 1); txp[1] = scSim.t0
    xhats   = zeros(2*length(ts) + 1, 7); xhats[1, :] .= xhat0
    es      = zeros(2*length(ts) + 1, 7);
    Ps      = zeros(2*length(ts) + 1, 7); Ps[1, :] .= diag(P0)
    χ       = zeros(14,29)
    χd      = zeros(7,29)
    P⁻      = zeros(7,7)
    P⁺      = deepcopy(P0)
    Py      = zeros(35, 35)
    Pyt     = zeros(35, 35)
    Pxy     = zeros(7, 35)
    expmeas = zeros(35, 29)
    expmeasd= zeros(35, 29)
    yhat    = zeros(35)
    L       = 14

    # Construct sparse augmented covariance
    Pa      = sparse(zeros(14,14))
    for c in 1:7
        for r in 1:7
            Pa[r,c]     = P⁺[r,c]
            if c == r
                Pa[7 + r,7 + c]     = Q[r,c]
            end
        end
    end
    C       = cholesky(Pa; perm = 1:14)

    # Compute IMU Δt from ts vector
    imuΔt   = ts[2] - ts[1]

    # Compute initial estimates error
    (ytrue, u) = GetStateAndControl(scSim, scSim.t0)
    @views es[1,:] .= xhat0 .- ytrue[1:7]

    # Compute gamma
    λ       = α^2*(L + κ) - L
    γ       = sqrt(L + λ)

    # Instantiate UKF
    UKF(ts,imuΔt,gpsΔt,rs,txp,ixp,xhats,es,Ps,α,β,κ,λ,γ,L,χ,χd,Pa,C,P⁺,P⁻,Py,Pyt,Pxy,Q,expmeas,expmeasd,yhat,
        gpsSim,imuSim,scSim,σ2GPS,σ2Acc,0,true,false,lunaPerts)
end

function propagate!(ukf::UKF)
    if ukf.updated == false
        throw(ErrorException("Cannot propagate filter before updating."))
    else
        ukf.k += 1
        ukf.updated = false
    end

    @inbounds begin
    # Get time
    if ukf.k == 1
        t0  = ukf.scSim.t0
        tf  = ukf.ts[ukf.k]
    else
        t0  = ukf.ts[ukf.k - 1]
        tf  = ukf.ts[ukf.k]
    end

    # Generate vector of times to save at
    saveat = range(t0; length = 2, stop = tf)

    # Compute sigma points
    cholesky!(ukf.C, ukf.Pa)
    L       = sparse(ukf.C.L)
    ukf.χ  .= 0.0
    for i in 1:ukf.L
        @views ukf.χ[1:7,2*i - 1]   .= ukf.xhats[ukf.ixp, :] .+ ukf.γ.*L[1:7,i]
        @views ukf.χ[8:14,2*i - 1]  .= ukf.γ.*L[8:14,i]
        @views ukf.χ[1:7,2*i]       .= ukf.xhats[ukf.ixp, :] .- ukf.γ.*L[1:7,i]
        @views ukf.χ[8:14,2*i]      .= -ukf.γ.*L[8:14,i]
    end
    ukf.χ[1:7,2*ukf.L + 1] .= ukf.xhats[ukf.ixp, :]

    # Propagate sigma points
    if ukf.lunaPerts == false
        #@views sols = [rkProp(ukfNoLunarPertEOM, ukf.χ[:,i], saveat, ukf) for i in 1:2*ukf.L + 1]
        @views sols = [solve(
            ODEProblem{false}((u,p,t)->ukfNoLunarPertEOM(u,p,t,ukf.χ[8:14,i]),
            SVector{7}(ukf.χ[1:7,i]), (t0, tf), ukf), Vern7();
            reltol=1e-10, abstol=1e-10, saveat=saveat) for i in 1:2*ukf.L + 1]
    else
        #@views sols = [rkProp(ukfWithLunarPertEOM, ukf.χ[:,i], saveat, ukf) for i in 1:2*ukf.L + 1]
        @views sols = [solve(
            ODEProblem{false}((u,p,t)->ukfWithLunarPertEOM(u,p,t,ukf.χ[8:14,i]),
            SVector{7}(ukf.χ[1:7,i]), (t0, tf), ukf), Vern7();
            reltol=1e-10, abstol=1e-10, saveat=saveat) for i in 1:2*ukf.L + 1]
    end

    # Update storage matricies
    ukf.ixp += 1

    # Time
    ukf.txp[ukf.ixp] = tf

    # Fill matrix of sigma point states
    for j in 1:2*ukf.L + 1
        for k in 1:7
            ukf.χ[k,j] = sols[j].u[end][k]
        end
    end

    # Compute weights
    w0m     = ukf.λ / (ukf.L + ukf.λ)
    w0c     = w0m + (1 - ukf.α^2 + ukf.β)
    wi      = 1.0 / (2.0*(ukf.L + ukf.λ))

    # State estimate
    ukf.xhats[ukf.ixp,:] .*= 0.0
    for j in 1:2*ukf.L
        @views ukf.xhats[ukf.ixp,:] .+= wi.*ukf.χ[1:7,j]
    end
    @views ukf.xhats[ukf.ixp,:] .+= w0m.*ukf.χ[1:7,2*ukf.L + 1]

    # Error
    (ytrue, u) = GetStateAndControl(ukf.scSim, tf)
    @views ukf.es[ukf.ixp,:] .= ukf.xhats[ukf.ixp,:] .- ytrue[1:7]

    # Covariance
    ukf.P⁻ .*= 0.0
    for j in 1:2*ukf.L
        @views ukf.χd[:,j] .= ukf.χ[1:7,j] .- ukf.xhats[ukf.ixp,:]
        @views mul!(ukf.P⁻, ukf.χd[:,j], transpose(ukf.χd[:,j]), wi, 1.0) # Using P⁺ for storage of data
    end
    @views ukf.χd[:,2*ukf.L + 1] .= ukf.χ[1:7,2*ukf.L + 1] .- ukf.xhats[ukf.ixp,:]
    @views mul!(ukf.P⁻, ukf.χd[:,2*ukf.L + 1], transpose(ukf.χd[:,2*ukf.L + 1]), w0c, 1.0)

    @views ukf.Ps[ukf.ixp,:] .= diag(ukf.P⁻)
    end
    nothing
end

function updateGPS!(ukf::UKF)
    if ukf.updated == true
        throw(ErrorException("Cannot update again before propagating."))
    else
        ukf.updated = true
    end

    # Get current time
    t   = ukf.ts[ukf.k]

    # Get true state
    (ytrue, u) = GetStateAndControl(ukf.scSim, t)

    # Generate GPS measurements
    @views measGPS = gpsMeasurement(ukf.gpsSim, ytrue[1:3], t)

    # Get satelite ids
    @views sats = measGPS[2:2:end]
    numSats = length(sats)

    # Compute expected measurements
    ukf.expmeas .*= 0.0
    for i in 1:2*ukf.L + 1
        # Get expected gps measurement
        @views (expMeasGPS, ps) = gpsMeasurement(ukf.gpsSim, ukf.χ[1:3,i], sats, t; type = :expected)
        for j in 1:numSats
            ukf.expmeas[j,i] = expMeasGPS[3 + 2*(j - 1)]
        end
    end

    # Compute weights
    w0m     = ukf.λ / (ukf.L + ukf.λ)
    w0c     = w0m + (1 - ukf.α^2 + ukf.β)
    wi      = 1.0 / (2.0*(ukf.L + ukf.λ))

    # Compute mean observation
    ukf.yhat .= 0.0
    for i in 1:2*ukf.L
        @views ukf.yhat[1:numSats] .+= wi.*ukf.expmeas[1:numSats,i]
    end
    @views ukf.yhat[1:numSats] .+= w0m.*ukf.expmeas[1:numSats,2*ukf.L + 1]

    # Compute residuals with measurement rejection
    accMeas     = Vector{Bool}(undef, 35); accMeas .= false
    rejectTol   = 10.0
    numRejected = 0
    for i in 1:numSats
        r = measGPS[3 + 2*(i - 1)] - ukf.yhat[i]
        if abs(r) < rejectTol
            accMeas[i] = true
            ukf.rs[ukf.k, i - numRejected] = r
        else
            numRejected += 1
        end
    end

    # Compute output covariance
    ukf.Py      .= 0.0
    numMeas  = numSats - numRejected
    for j in 1:2*ukf.L
        @views ukf.expmeasd[1:numMeas,j] .= ukf.expmeas[accMeas,j] .- ukf.yhat[accMeas]
        @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,j],
            transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views ukf.expmeasd[1:numMeas,2*ukf.L + 1] .= ukf.expmeas[accMeas,2*ukf.L + 1] .- ukf.yhat[accMeas]
    @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,2*ukf.L + 1],
        transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Update output covariance to innovations covariance (assuming linearly appearing measurement noise)
    for i in 1:numMeas; ukf.Py[i,i] += ukf.σ2GPS; end

    # Compute cross correlation matrix
    ukf.Pxy .= 0.0
    for j in 1:2*ukf.L
        @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,j], transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,2*ukf.L + 1], transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Compute gain
    @views Pyfact = factorize(ukf.Py[1:numMeas,1:numMeas])
    @views rdiv!(ukf.Pxy[1:7,1:numMeas], Pyfact)

    # Update storage matricies
    # Time
    ukf.ixp += 1
    ukf.txp[ukf.ixp] = t

    # Compute state update and save
    @views mul!(ukf.xhats[ukf.ixp, :], ukf.Pxy[1:7,1:numMeas], ukf.rs[ukf.k, 1:numMeas])
    ukf.xhats[ukf.ixp,:] .+= ukf.xhats[ukf.ixp - 1, :]

    # Compute covariance update
    ukf.P⁺ .= ukf.P⁻
    @views mul!(ukf.Pyt[1:7,1:numMeas], ukf.Pxy[1:7,1:numMeas], ukf.Py[1:numMeas,1:numMeas])
    @views mul!(ukf.P⁺, ukf.Pyt[1:7,1:numMeas], transpose(ukf.Pxy[1:7,1:numMeas]), -1.0, 1.0)

    # Compute error
    (ytrue,u) = GetStateAndControl(ukf.scSim, t)
    @views ukf.es[ukf.ixp,:] .= ukf.xhats[ukf.ixp,:] .- ytrue[1:7]

    # Set covariance diag
    ukf.Ps[ukf.ixp,:] .= diag(ukf.P⁺)

    # Force covariance symmetry
    for r in 1:6
        for c in r+1:7
            ukf.P⁺[r,c] = ukf.P⁺[c,r]
        end
    end

    # Update augmented covariance
    for r in 1:7
        for c in 1:7
            ukf.Pa[r,c] = ukf.P⁺[r,c]
        end
    end
end

function updateIMU!(ukf::UKF)
    if ukf.updated == true
        throw(ErrorException("Cannot update again before propagating."))
    else
        ukf.updated = true
    end

    @inbounds begin
    # Get current time
    t   = ukf.ts[ukf.k]

    # Get true state
    (ytrue, u)  = GetStateAndControl(ukf.scSim, t)

    # Generate accelerometer measurements
    (dy, _us)   = GetStateAndControl(ukf.scSim, t; derivs = true)
    @views measAcc = ComputeMeasurement(ukf.imuSim, dy[4:6])

    # Compute expected measurements
    ukf.yhat    .= 0.0
    ukf.expmeas .= 0.0
    for i in 1:2*ukf.L + 1
        if ukf.lunaPerts == false
            dy = ukfNoLunarPertEOM(ukf.χ[:,i], ukf, t, ukf.yhat)
        else
            dy = ukfWithLunarPertEOM(ukf.χ[:,i], ukf, t, ukf.yhat)
        end
        @views expMeasAcc = ComputeMeasurement(ukf.imuSim, dy[4:6]; type = :expected)
        for j in 1:3
            ukf.expmeas[j,i] = expMeasAcc[j]
        end
    end

    # Number of measurements
    numMeas = 3

    # Compute weights
    w0m     = ukf.λ / (ukf.L + ukf.λ)
    w0c     = w0m + (1 - ukf.α^2 + ukf.β)
    wi      = 1.0 / (2.0*(ukf.L + ukf.λ))

    # Compute mean observation
    ukf.yhat .= 0.0
    for i in 1:2*ukf.L
        @views ukf.yhat[1:numMeas] .+= wi.*ukf.expmeas[1:numMeas,i]
    end
    @views ukf.yhat[1:numMeas] .+= w0m.*ukf.expmeas[1:numMeas,2*ukf.L + 1]

    # Compute residuals
    for i in 1:3
        ukf.rs[ukf.k, i] = measAcc[i] - ukf.yhat[i]
    end

    # Compute output covariance
    ukf.Py      .= 0.0
    for j in 1:2*ukf.L
        @views ukf.expmeasd[1:numMeas,j] .= ukf.expmeas[1:numMeas,j] .- ukf.yhat[1:numMeas]
        @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,j],
            transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views ukf.expmeasd[1:numMeas,2*ukf.L + 1] .= ukf.expmeas[1:numMeas,2*ukf.L + 1] .- ukf.yhat[1:numMeas]
    @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,2*ukf.L + 1],
        transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Update output covariance to innovations covariance (assuming linearly appearing measurement noise)
    for i in 1:numMeas; ukf.Py[i,i] += ukf.σ2Acc; end

    # Compute cross correlation matrix
    ukf.Pxy .= 0.0
    for j in 1:2*ukf.L
        @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,j], transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,2*ukf.L + 1], transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Compute gain
    @views Pyfact = factorize(ukf.Py[1:numMeas,1:numMeas])
    @views rdiv!(ukf.Pxy[1:7,1:numMeas], Pyfact)

    # Update storage matricies
    # Time
    ukf.ixp += 1
    ukf.txp[ukf.ixp] = t

    # Compute state update and save
    @views mul!(ukf.xhats[ukf.ixp, :], ukf.Pxy[1:7,1:numMeas], ukf.rs[ukf.k, 1:numMeas])
    ukf.xhats[ukf.ixp,:] .+= ukf.xhats[ukf.ixp - 1, :]

    # Compute covariance update
    ukf.P⁺ .= ukf.P⁻
    @views mul!(ukf.Pyt[1:7,1:numMeas], ukf.Pxy[1:7,1:numMeas], ukf.Py[1:numMeas,1:numMeas])
    @views mul!(ukf.P⁺, ukf.Pyt[1:7,1:numMeas], transpose(ukf.Pxy[1:7,1:numMeas]), -1.0, 1.0)

    # Compute error
    (ytrue,u) = GetStateAndControl(ukf.scSim, t)
    @views ukf.es[ukf.ixp,:] .= ukf.xhats[ukf.ixp,:] .- ytrue[1:7]

    # Set covariance diag
    ukf.Ps[ukf.ixp,:] .= diag(ukf.P⁺)

    # Force covariance symmetry
    for r in 1:6
        for c in r+1:7
            ukf.P⁺[r,c] = ukf.P⁺[c,r]
        end
    end

    # Update augmented covariance
    for r in 1:7
        for c in 1:7
            ukf.Pa[r,c] = ukf.P⁺[r,c]
        end
    end

    end
    nothing
end

function updateGPSIMU!(ukf::UKF)
    if ukf.updated == true
        throw(ErrorException("Cannot update again before propagating."))
    else
        ukf.updated = true
    end

    @inbounds begin
    # Get current time
    t   = ukf.ts[ukf.k]

    # Get true state
    (ytrue, u) = GetStateAndControl(ukf.scSim, t)

    ## === GPS ===

    # Generate GPS measurements
    @views measGPS = gpsMeasurement(ukf.gpsSim, ytrue[1:3], t)

    # Get satelite ids
    @views sats = measGPS[2:2:end]
    numSats = length(sats)

    # Compute expected measurements
    ukf.expmeas .= 0.0
    for i in 1:2*ukf.L + 1
        # Get expected gps measurement
        @views (expMeasGPS, ps) = gpsMeasurement(ukf.gpsSim, ukf.χ[1:3,i], sats, t; type = :expected)
        for j in 1:numSats
            ukf.expmeas[j,i] = expMeasGPS[3 + 2*(j - 1)]
        end
    end

    ## === IMU ===

    # Generate accelerometer measurement
    (dy, _us) = GetStateAndControl(ukf.scSim, t; derivs = true)
    @views measAcc = ComputeMeasurement(ukf.imuSim, dy[4:6])

    # Compute expected measurements
    ukf.yhat .= 0.0
    for i in 1:2*ukf.L + 1
        if ukf.lunaPerts == false
            dy = ukfNoLunarPertEOM(ukf.χ[:,i], ukf, t, ukf.yhat)
        else
            dy = ukfWithLunarPertEOM(ukf.χ[:,i], ukf, t, ukf.yhat)
        end
        @views expMeasAcc = ComputeMeasurement(ukf.imuSim, dy[4:6]; type = :expected)
        for j in 1:3
            ukf.expmeas[numSats + j,i] = expMeasAcc[j]
        end
    end

    # Compute weights
    w0m     = ukf.λ / (ukf.L + ukf.λ)
    w0c     = w0m + (1 - ukf.α^2 + ukf.β)
    wi      = 1.0 / (2.0*(ukf.L + ukf.λ))

    # Compute mean observation
    ukf.yhat .= 0.0
    for i in 1:2*ukf.L
        @views ukf.yhat[1:numSats + 3] .+= wi.*ukf.expmeas[1:numSats + 3,i]
    end
    @views ukf.yhat[1:numSats + 3] .+= w0m.*ukf.expmeas[1:numSats + 3,2*ukf.L + 1]

    # Compute residuals with GPS pseudorange measurement rejection
    accMeas     = Vector{Bool}(undef, 35); accMeas .= false
    rejectTol   = 1.0
    numRejected = 0
    for i in 1:numSats
        r = measGPS[3 + 2*(i - 1)] - ukf.yhat[i]
        if abs(r) < rejectTol
            accMeas[i] = true
            ukf.rs[ukf.k, i - numRejected] = r
        else
            numRejected += 1
        end
    end
    j = 0
    for i in numSats - numRejected + 1:numSats - numRejected + 3
        j += 1
        ukf.rs[ukf.k, i]    = measAcc[j] - ukf.yhat[i + numRejected]
        accMeas[i + numRejected]    = true
    end

    # Compute output covariance
    ukf.Py      .= 0.0
    numMeas  = numSats - numRejected + 3
    for j in 1:2*ukf.L
        @views ukf.expmeasd[1:numMeas,j] .= ukf.expmeas[accMeas,j] .- ukf.yhat[accMeas]
        @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,j],
            transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views ukf.expmeasd[1:numMeas,2*ukf.L + 1] .= ukf.expmeas[accMeas,2*ukf.L + 1] .- ukf.yhat[accMeas]
    @views mul!(ukf.Py[1:numMeas,1:numMeas], ukf.expmeasd[1:numMeas,2*ukf.L + 1],
        transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Update output covariance to innovations covariance (assuming linearly appearing measurement noise)
    for i in 1:numSats - numRejected; ukf.Py[i,i] += ukf.σ2GPS; end
    for i in numSats - numRejected + 1:numSats - numRejected + 3; ukf.Py[i,i] += ukf.σ2Acc; end

    # Compute cross correlation matrix
    ukf.Pxy .= 0.0
    for j in 1:2*ukf.L
        @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,j], transpose(ukf.expmeasd[1:numMeas,j]), wi, 1.0)
    end
    @views mul!(ukf.Pxy[1:7,1:numMeas], ukf.χd[:,2*ukf.L + 1], transpose(ukf.expmeasd[1:numMeas,2*ukf.L + 1]), w0c, 1.0)

    # Compute gain
    @views Pyfact = factorize(ukf.Py[1:numMeas,1:numMeas])
    @views rdiv!(ukf.Pxy[1:7,1:numMeas], Pyfact)

    # Update storage matricies
    # Time
    ukf.ixp += 1
    ukf.txp[ukf.ixp] = t

    # Compute state update and save
    @views mul!(ukf.xhats[ukf.ixp, :], ukf.Pxy[1:7,1:numMeas], ukf.rs[ukf.k, 1:numMeas])
    ukf.xhats[ukf.ixp,:] .+= ukf.xhats[ukf.ixp - 1, :]

    # Compute covariance update
    ukf.P⁺ .= ukf.P⁻
    @views mul!(ukf.Pyt[1:7,1:numMeas], ukf.Pxy[1:7,1:numMeas], ukf.Py[1:numMeas,1:numMeas])
    @views mul!(ukf.P⁺, ukf.Pyt[1:7,1:numMeas], transpose(ukf.Pxy[1:7,1:numMeas]), -1.0, 1.0)

    # Compute error
    (ytrue,u) = GetStateAndControl(ukf.scSim, t)
    @views ukf.es[ukf.ixp,:] .= ukf.xhats[ukf.ixp,:] .- ytrue[1:7]

    # Set covariance diag
    ukf.Ps[ukf.ixp,:] .= diag(ukf.P⁺)

    # Force covariance symmetry
    for r in 1:6
        for c in r+1:7
            ukf.P⁺[r,c] = ukf.P⁺[c,r]
        end
    end

    # Update augmented covariance
    for r in 1:7
        for c in 1:7
            ukf.Pa[r,c] = ukf.P⁺[r,c]
        end
    end

    end
    nothing
end

function runFilter!(ukf::UKF)
    for i in 1:length(ukf.ts)
        propagate!(ukf)
        if i*ukf.imuΔt % ukf.gpsΔt == 0
            updateGPSIMU!(ukf)
        else
            updateIMU!(ukf)
        end
    end
end

function rkProp(f, x0, ts, ukf::UKF)
    h   = ts[2] - ts[1]
    χw  = view(x0, 8:14)
    xin = @SVector [x0[1], x0[2], x0[3], x0[4], x0[5], x0[6], x0[7]]
    for i in 1:length(ts) - 1
        xi1 = @SVector [xin[1], xin[2], xin[3], xin[4], xin[5], xin[6], xin[7]]
        k1  = f(xi1, ukf, ts[i], χw)
        xi2 = @SVector [xi1[1] + 0.5*h*k1[1],
                        xi1[2] + 0.5*h*k1[2],
                        xi1[3] + 0.5*h*k1[3],
                        xi1[4] + 0.5*h*k1[4],
                        xi1[5] + 0.5*h*k1[5],
                        xi1[6] + 0.5*h*k1[6],
                        xi1[7] + 0.5*h*k1[7]]
        k2  = f(xi2, ukf, ts[i] + 0.5*h, χw)
        xi3 = @SVector [xi1[1] + 0.5*h*k2[1],
                        xi1[2] + 0.5*h*k2[2],
                        xi1[3] + 0.5*h*k2[3],
                        xi1[4] + 0.5*h*k2[4],
                        xi1[5] + 0.5*h*k2[5],
                        xi1[6] + 0.5*h*k2[6],
                        xi1[7] + 0.5*h*k2[7]]
        k3  = f(xi3, ukf, ts[i] + 0.5*h, χw)
        xi4 = @SVector [xi1[1] + h*k3[1],
                        xi1[2] + h*k3[2],
                        xi1[3] + h*k3[3],
                        xi1[4] + h*k3[4],
                        xi1[5] + h*k3[5],
                        xi1[6] + h*k3[6],
                        xi1[7] + h*k3[7]]
        k4  = f(xi4, ukf, ts[i] + h, χw)
        xin = @SVector [xi1[1] + h*(k1[1] + 2.0*k2[1] + 2.0*k3[1] + k4[1])/6.0,
                        xi1[2] + h*(k1[2] + 2.0*k2[2] + 2.0*k3[2] + k4[2])/6.0,
                        xi1[3] + h*(k1[3] + 2.0*k2[3] + 2.0*k3[3] + k4[3])/6.0,
                        xi1[4] + h*(k1[4] + 2.0*k2[4] + 2.0*k3[4] + k4[4])/6.0,
                        xi1[5] + h*(k1[5] + 2.0*k2[5] + 2.0*k3[5] + k4[5])/6.0,
                        xi1[6] + h*(k1[6] + 2.0*k2[6] + 2.0*k3[6] + k4[6])/6.0,
                        xi1[7] + h*(k1[7] + 2.0*k2[7] + 2.0*k3[7] + k4[7])/6.0]
    end
    return xin
end

function ukfNoLunarPertEOM(y, ukf::UKF, t, χw)
    # Earth gravitational parameter
    μ = 3.986004418e5 # [km^3/s^2]

    # Thrust magnitued
    T = 10e-3 # [kN]

    # Get control
    u = GetControl(ukf.scSim, t)

    # States
    @views r    = norm(y[1:3])
    nμr3        = -μ/r^3
    at          = T*u[1] / y[7]
    dy          = @SVector [y[4] + χw[1], y[5] + χw[2], y[6] + χw[3],
                            nμr3*y[1] + at*u[2] + χw[4],
                            nμr3*y[2] + at*u[3] + χw[5],
                            nμr3*y[3] + at*u[4] + χw[6],
                            -u[1]*T / ukf.scSim.ps.sp.c + χw[7]]

    return dy
end

function ukfWithLunarPertEOM(y, ukf::UKF, t, χw)
    # Earth gravitational parameter
    μ = 5.9742e24*6.673e-20

    # Luna gravitational parameter
    μl = 7.3483e22*6.673e-20

    # Thrust magnitued
    T = 10e-3 # [kN]

    # Get control
    (u, rLuna) = GetControlAndLunaPos(ukf.scSim, t)

    # Luna gravitational perturbation
    rls         = @SVector [rLuna[1] - y[1], rLuna[2] - y[2], rLuna[3] - y[3]]
    μldRls3     = μl / (norm(rls)^3)
    μldRluna3   = μl / (norm(rLuna)^3)
    al          = @SVector [rls[1]*μldRls3 - rLuna[1]*μldRluna3,
                            rls[2]*μldRls3 - rLuna[2]*μldRluna3,
                            rls[3]*μldRls3 - rLuna[3]*μldRluna3]
    # States
    @views r    = norm(y[1:3])
    nμr3        = -μ/r^3
    at          = T*u[1] / y[7]
    dy          = @SVector [y[4] + χw[1], y[5] + χw[2], y[6] + χw[3],
                            nμr3*y[1] + al[1] + at*u[2] + χw[4],
                            nμr3*y[2] + al[2] + at*u[3] + χw[5],
                            nμr3*y[3] + al[3] + at*u[4] + χw[6],
                            -u[1]*T / ukf.scSim.ps.sp.c + χw[7]]

    return dy
end

function plot(ukf::UKF, xtrue, n)
    ts   = ukf.txp[1:n]
    xhat = ukf.xhats[1:n, :]
    xt   = xtrue[1:n, :]
    es   = ukf.es[1:n, :]
    σ311 = 3*sqrt.(ukf.Ps[1:n,1])
    σ322 = 3*sqrt.(ukf.Ps[1:n,2])
    σ333 = 3*sqrt.(ukf.Ps[1:n,3])
    σ344 = 3*sqrt.(ukf.Ps[1:n,4])
    σ355 = 3*sqrt.(ukf.Ps[1:n,5])
    σ366 = 3*sqrt.(ukf.Ps[1:n,6])
    σ377 = 3*sqrt.(ukf.Ps[1:n,7])

    mat"""
    ts      = $ts;
    ts      = (ts - $(ukf.scSim.t0))/86400;
    xhat    = $xhat;
    xt      = $xt;
    es      = $es;
    s311    = $σ311;
    s322    = $σ322;
    s333    = $σ333;
    s344    = $σ344;
    s355    = $σ355;
    s366    = $σ366;
    s377    = $σ377;

    figure()
    subplot(3,1,1)
    plot(ts, es(:,1), 'k')
    hold on
    plot(ts, s311, 'r')
    plot(ts, -s311, 'r')
    legend('Estimation Error', 'UKF \$3\\sigma\$', 'Interpreter', 'latex')
    grid on

    subplot(3,1,2)
    plot(ts, es(:,1), 'k')
    hold on
    plot(ts, s322, 'r')
    plot(ts, -s322, 'r')
    grid on

    subplot(3,1,3)
    plot(ts, es(:,3), 'k')
    hold on
    plot(ts, s333, 'r')
    plot(ts, -s333, 'r')
    xlabel("Time, days")
    grid on

    figure()
    subplot(3,1,1)
    plot(ts, es(:,4), 'k')
    hold on
    plot(ts, s344, 'r')
    plot(ts, -s344, 'r')

    subplot(3,1,2)
    plot(ts, es(:,5), 'k')
    hold on
    plot(ts, s355, 'r')
    plot(ts, -s355, 'r')

    subplot(3,1,3)
    plot(ts, es(:,6), 'k')
    hold on
    plot(ts, s366, 'r')
    plot(ts, -s366, 'r')

    figure()
    plot(ts, es(:,7), 'k')
    hold on
    plot(ts, s377, 'r')
    plot(ts, -s377, 'r')

    figure()
    plot3(xhat(:,1),xhat(:,2), xhat(:,3), 'r')
    hold on
    plot3(xt(:,1),xt(:,2),xt(:,3), 'b')
    grid on
    axis equal
    """
end

function plotMC(ukf::UKF, em, ev, n)
    ts      = ukf.txp[1:n]
    xhat    = ukf.xhats[1:n, :]
    es      = ukf.es[1:n, :]
    σ311    = 3*sqrt.(ukf.Ps[1:n,1])
    σ311mc  = 3*sqrt.(ev[:,1])
    σ322    = 3*sqrt.(ukf.Ps[1:n,2])
    σ322mc  = 3*sqrt.(ev[:,2])
    σ333    = 3*sqrt.(ukf.Ps[1:n,3])
    σ333mc  = 3*sqrt.(ev[:,3])
    σ344    = 3*sqrt.(ukf.Ps[1:n,4])
    σ344mc  = 3*sqrt.(ev[:,4])
    σ355    = 3*sqrt.(ukf.Ps[1:n,5])
    σ355mc  = 3*sqrt.(ev[:,5])
    σ366    = 3*sqrt.(ukf.Ps[1:n,6])
    σ366mc  = 3*sqrt.(ev[:,6])
    σ377    = 3*sqrt.(ukf.Ps[1:n,7])
    σ377mc  = 3*sqrt.(ev[:,7])

    mat"""
    ts      = $ts;
    ts      = (ts - $(ukf.scSim.t0))/86400;
    xhat    = $xhat;
    es      = $es;
    s311    = $σ311;
    s311mc  = $σ311mc;
    s322    = $σ322;
    s322mc  = $σ322mc;
    s333    = $σ333;
    s333mc  = $σ333mc;
    s344    = $σ344;
    s344mc  = $σ344mc;
    s355    = $σ355;
    s355mc  = $σ355mc;
    s366    = $σ366;
    s366mc  = $σ366mc;
    s377    = $σ377;
    s377mc  = $σ377mc;
    em      = $em;

    figure()
    subplot(3,1,1)
    plot(ts, es(:,1).*1000, 'k')
    hold on
    plot(ts, s311.*1000, 'r')
    plot(ts, -s311.*1000, 'r')
    plot(ts, s311mc.*1000, '--r')
    plot(ts, -s311mc.*1000, '--r')
    legend('Estimation Error', 'UKF \$3\\sigma\$', 'MC \$3\\sigma\$','Interpreter', 'latex')
    ylabel('\$e_{r_x}\$, m', 'Interpreter', 'latex')

    ylim([-1000, 1000])
    grid on

    subplot(3,1,2)
    plot(ts, es(:,1).*1000, 'k')
    hold on
    plot(ts, s322.*1000, 'r')
    plot(ts, -s322.*1000, 'r')
    plot(ts, s322mc.*1000, '--r')
    plot(ts, -s322mc.*1000, '--r')
    ylabel('\$e_{r_y}\$, m', 'Interpreter', 'latex')

    ylim([-1000, 1000])
    grid on

    subplot(3,1,3)
    plot(ts, es(:,3).*1000, 'k')
    hold on
    plot(ts, s333.*1000, 'r')
    plot(ts, -s333.*1000, 'r')
    plot(ts, s333mc.*1000, '--r')
    plot(ts, -s333mc.*1000, '--r')
    xlabel('Time, days')
    ylabel('\$e_{r_z}\$, m', 'Interpreter', 'latex')

    ylim([-1000, 1000])
    grid on

    figure()
    subplot(3,1,1)
    plot(ts, es(:,4).*1000, 'k')
    hold on
    plot(ts, s344.*1000, 'r')
    plot(ts, -s344.*1000, 'r')
    plot(ts, s344mc.*1000, '--r')
    plot(ts, -s344mc.*1000, '--r')
    legend('Estimation Error', 'UKF \$3\\sigma\$', 'MC \$3\\sigma\$','Interpreter', 'latex')
    ylabel('\$e_{v_x}\$, m/s', 'Interpreter', 'latex')

    ylim([-2, 2])
    grid on

    subplot(3,1,2)
    plot(ts, es(:,5).*1000, 'k')
    hold on
    plot(ts, s355.*1000, 'r')
    plot(ts, -s355.*1000, 'r')
    plot(ts, s355mc.*1000, '--r')
    plot(ts, -s355mc.*1000, '--r')
    ylabel('\$e_{v_y}\$, m/s', 'Interpreter', 'latex')

    ylim([-2, 2])
    grid on

    subplot(3,1,3)
    plot(ts, es(:,6).*1000, 'k')
    hold on
    plot(ts, s366.*1000, 'r')
    plot(ts, -s366.*1000, 'r')
    plot(ts, s366mc.*1000, '--r')
    plot(ts, -s366mc.*1000, '--r')
    xlabel('Time, days')
    ylabel('\$e_{v_z}\$, m/s', 'Interpreter', 'latex')

    ylim([-2, 2])
    grid on

    figure()
    plot(ts, es(:,7), 'k')
    hold on
    plot(ts, s377, 'r')
    plot(ts, -s377, 'r')
    plot(ts, s377mc, '--r')
    plot(ts, -s377mc, '--r')
    xlabel('Time, days')
    ylabel('\$e_m\$, kg', 'Interpreter', 'latex')
    legend('Estimation Error', 'UKF \$3\\sigma\$', 'MC \$3\\sigma\$','Interpreter', 'latex')

    grid on

    figure()
    histogram(em(:,1).*1000)
    hold on
    xline(mean(em(:,1))*1000, '--k', 'LineWidth', 3)
    xlim([-10,10])
    xlabel('\$e_{r_x}\$, m', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,2).*1000)
    hold on
    xline(mean(em(:,2))*1000, '--k', 'LineWidth', 3)
    xlim([-10,10])
    xlabel('\$e_{r_y}\$, m', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,3).*1000)
    hold on
    xline(mean(em(:,3))*1000, '--k', 'LineWidth', 3)
    xlim([-10,10])
    xlabel('\$e_{r_z}\$, m', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,4).*1000)
    hold on
    xline(mean(em(:,4))*1000, '--k', 'LineWidth', 3)
    xlim([-0.02,0.02])
    xlabel('\$e_{v_x}\$, m/s', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,5).*1000)
    hold on
    xline(mean(em(:,5))*1000, '--k', 'LineWidth', 3)
    xlim([-0.02,0.02])
    xlabel('\$e_{v_y}\$, m/s', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,6).*1000)
    hold on
    xline(mean(em(:,6))*1000, '--k', 'LineWidth', 3)
    xlim([-0.02,0.02])
    xlabel('\$e_{v_z}\$, m/s', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    figure()
    histogram(em(:,7))
    hold on
    xline(mean(em(:,7)), '--k', 'LineWidth', 3)
    %xlim([-0.02,0.02])
    xlabel('\$e_m\$, m/s', 'Interpreter', 'latex')
    ylabel('# Occurances')
    grid on

    """
end

function saveMCmat(ukf::UKF, em, ev, n)
    ts      = ukf.txp[1:n]
    xhat    = ukf.xhats[1:n, :]
    es      = ukf.es[1:n, :]
    σ311    = 3*sqrt.(ukf.Ps[1:n,1])
    σ311mc  = 3*sqrt.(ev[:,1])
    σ322    = 3*sqrt.(ukf.Ps[1:n,2])
    σ322mc  = 3*sqrt.(ev[:,2])
    σ333    = 3*sqrt.(ukf.Ps[1:n,3])
    σ333mc  = 3*sqrt.(ev[:,3])
    σ344    = 3*sqrt.(ukf.Ps[1:n,4])
    σ344mc  = 3*sqrt.(ev[:,4])
    σ355    = 3*sqrt.(ukf.Ps[1:n,5])
    σ355mc  = 3*sqrt.(ev[:,5])
    σ366    = 3*sqrt.(ukf.Ps[1:n,6])
    σ366mc  = 3*sqrt.(ev[:,6])
    σ377    = 3*sqrt.(ukf.Ps[1:n,7])
    σ377mc  = 3*sqrt.(ev[:,7])

    fname   = datadir("MATLABdata", "ukf.mat")

    mat"""
    ts      = $ts;
    ts      = (ts - $(ukf.scSim.t0))/86400;
    xhat    = $xhat;
    es      = $es;
    s311    = $σ311;
    s311mc  = $σ311mc;
    s322    = $σ322;
    s322mc  = $σ322mc;
    s333    = $σ333;
    s333mc  = $σ333mc;
    s344    = $σ344;
    s344mc  = $σ344mc;
    s355    = $σ355;
    s355mc  = $σ355mc;
    s366    = $σ366;
    s366mc  = $σ366mc;
    s377    = $σ377;
    s377mc  = $σ377mc;
    em      = $em;

    save($fname)
    """

    nothing
end

max_stars_repo_name: GrantHecht/OptimalEstimationProject


text:
using Documenter, Underscores

DocMeta.setdocmeta!(Underscores, :DocTestSetup, :(using Underscores); recursive=true)
makedocs(;
    modules=[Underscores],
    format=Documenter.HTML(),
    pages=[
        "Home" => "index.md",
    ],
    repo="https://github.com/c42f/Underscores.jl/blob/{commit}{path}#L{line}",
    sitename="Underscores.jl",
    authors="Chris Foster <chris42f@gmail.com>",
    doctest=true
)

deploydocs(;
    repo="github.com/c42f/Underscores.jl",
    push_preview=true
)

ext: jl


text:
include("./src/GBDTsfuzzy.jl")
include("./MyGrammar.jl")

using .GBDTsfuzzy
using .MyGrammar
using ArgParse
using MultivariateTimeSeries
using Printf
import TikzPictures
import JLD
using Random; Random.seed!(1)

s = ArgParseSettings()
@add_arg_table s begin
    "data"
        help = "path to data"
        required = true
        arg_type = String
    "--fuzzy"
        help = "run in fuzzy more"
        action = :store_true
    "depth"
        help = "maximum depth of the tree"
        arg_type = Int
        required = true
    "--output_dir"
        help = "output directory"
        arg_type = String
        default = "."
    "--name"
        help = "name of the model"
        arg_type = String
        default = "model"
    "--seed"
        help = "random seed"
        arg_type = Int
        default = 0
end
args = parse_args(ARGS, s)

Random.seed!(args["seed"])

mode = "normal"
if args["fuzzy"]
    mode = "fuzzy"
end

println("Reading data")
X, y = read_data_labeled(args["data"])
println("Finished reading")

g = grammar
if mode == "fuzzy"
    g = grammar_fuzzy
end

for xid in names(X)
    if startswith(string(xid), "p_")
        add_xid(g, xid)
    end
end

println("Grammar defined")

const v = Dict{Symbol,Vector{Float64}}()
mins, maxes = minimum(X), maximum(X)
for (i,xid) in enumerate(names(X))
    v[xid] = collect(range(mins[i],stop=maxes[i],length=3))
end;

println("Constants are set")

p = MonteCarlo(2000, 5)

println("MonteCarlo")

println("Learning decision tree")
#if mode == "normal"
#    model = induce_tree(g, :b, p, X, y, args["depth"], afsoon_loss);
#else
model = induce_tree(g, :b, p, X, y, args["depth"], afsoon_loss_fuzzy);
#end
println("Finished learning")
println("Show")
show(model)
println(v)
println("Done show")

println("Storing model")
JLD.save(joinpath(args["output_dir"], string(args["name"], ".jld")), "model", model, "v", v, "mode", mode)
println("Done saving")

try
    println("Display")
    t = display(model; edgelabels=false) #suppress edge labels for clarity (left branch is true, right branch is false)
    cd(args["output_dir"])
    TikzPictures.save(TikzPictures.PDF(args["name"]), t)
    println("Done display")
catch y
    println("Display failed")
    println(y)
end


text:
module HelicopterSciML

function get_standard_model()
    P0 = (rc = 0.015, h = 0.005, m = 0.5, Jbc = 0.015, Js = 0.005, g = 9.81, K_th_th = 0.055, K_th_psi = 0.005, K_psi_th = 0.15, K_psi_psi = 0.2, D_th = 0.01, D_psi = 0.08)
    Ps_best = [1.307946352764753, 0.2015053005215372, 1.4051491906882028, 0.2189022428782021, 0.2525307240269974, 1.0, 1.6851193774035353, 0.3183582622802048, 0.200000888476103, 0.21474380697773823, 1.1382680514356591, 0.6049221351393168]
    x0_best = [-0.66698, 0.81165, 1.307946352764753, 0.2015053005215372]
    tspan = (0.0, 130.99)
end

end


text:
"""
    add_index!(table::CleanTable)
Adds in-place a column being the row index for the `CleanTable` table.
"""
function add_index!(table::CleanTable)
    columns = cols(table)
    indexes = collect(1:length(columns[1]))

    insert!(columns, 1, indexes)
    insert!(names(table), 1, :row_index)

    return table
end

"""
    add_index(table)
Creates a `CleanTable` with copied columns and adds to it a new column being the row index
for the table passed.
"""
function add_index(table)
    return add_index!(CleanTable(table))
end

max_stars_repo_name: TheRoniOne/Cleaner

max_issues_repo_name: TheRoniOne/Cleaner

max_forks_repo_name: TheRoniOne/Cleaner


text:
export phase_estimation_circuit, phase_estimation_analysis

"""
    phase_estimation_circuit(unitarygate::GeneralMatrixBlock, n_reg, n_b) -> ChainBlock

Phase estimation circuit. Input arguments are

* `unitarygate`: the input unitary matrix.
* `n_reg`: the number of bits to store phases,
* `n_b`: the number of bits to store vector.

References
----------------------
[Wiki](https://en.wikipedia.org/wiki/Quantum_phase_estimation_algorithm)
"""
function phase_estimation_circuit(unitarygate::GeneralMatrixBlock, n_reg::Int, n_b::Int)
    nbit = n_b + n_reg
    # Apply Hadamard Gate.
    hs = repeat(nbit, H, 1:n_reg)

    # Construct a control circuit.
    control_circuit = chain(nbit)
    for i = 1:n_reg
        push!(control_circuit, control(nbit, (i,), (n_reg+1:nbit...,)=>unitarygate))
        if i != n_reg
            unitarygate = matblock(mat(unitarygate) * mat(unitarygate))
        end
    end

    # Inverse QFT Block.
    iqft = subroutine(nbit, qft_circuit(n_reg)',[1:n_reg...,])
    chain(hs, control_circuit, iqft)
end

"""
    phase_estimation_analysis(eigenvectors::Matrix, reg::ArrayReg) -> Tuple

Analyse phase estimation result using state projection.
It returns a tuple of (most probable configuration, the overlap matrix, the relative probability for this configuration)
`eigenvectors` is the eigen vectors of the unitary gate matrix, while `reg` is the result of phase estimation.
"""
function phase_estimation_analysis(eigenvectors::AbstractMatrix, reg::ArrayReg)
    overlap = eigenvectors'*state(reg)
    amp_relative = Float64[]
    bs = Int[]

    for b in basis(overlap)
        mc = argmax(view(overlap, b+1, :) .|> abs)-1
        push!(amp_relative, abs2(overlap[b+1, mc+1])/sum(overlap[b+1, :] .|> abs2))
        push!(bs, mc)
    end
    bs, overlap, amp_relative
end


text:
# Autogenerated wrapper script for rr_jll for x86_64-linux-gnu-cxx11
export rr

using CompilerSupportLibraries_jll
JLLWrappers.@generate_wrapper_header("rr")
JLLWrappers.@declare_executable_product(rr)
function __init__()
    JLLWrappers.@generate_init_header(CompilerSupportLibraries_jll)
    JLLWrappers.@init_executable_product(
        rr,
        "bin/rr",
    )

    JLLWrappers.@generate_init_footer()
end  # __init__()


text:
ENV["JULIA_DEBUG"] = "Intcode"
using Intcode, Test, Logging

include("test_data.jl")

@testset "Run test for Intcode" begin
    res = all(
        map(TestData.test_cases_data) do (i,o)
            ic = IntcodeMachine(i)
            @info "\nTesting $i => $o..."
            run_intcode!(ic)
            res = ic.tape == o
            if res
                @info "PASSED"
            else
                @error "[FAILED] $ic" ic.tape
            end
            res
        end
    )
    @test res
end


text:
using DelimitedFiles
using Plots

include("exact.jl")

f = 999.99999999999989
U = 0.75
c = 343.0
ρ = 1.205
a = 0.005

linedata = readdlm("linedata_2.csv", ',')

x = Float64.(linedata[2:end, 3])
y = Float64.(linedata[2:end, 4])
z = Float64.(linedata[2:end, 5])

r = sqrt.(x.^2 + y.^2 + z.^2)

femSolution = Float64.(linedata[2:end, 1]) + im .* Float64.(linedata[2:end, 2])

exactSolution = sphericalWave.(U, a, f, 0, r, ρ, c)

err = femSolution - exactSolution

realplt = plot(
    r,
    real.(err),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[Pa]",
    title="Real Part of the Error"
    )

imagplt = plot(
    r,
    imag.(err),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[Pa]",
    title="Imaginary Part of the Error"
)

absplt = plot(
    r,
    abs.(err),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[Pa]",
    title="Absolute Value of the Error"
)

angplt = plot(
    r,
    angle.(err),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[rad]",
    title="Phase of the Error"
)

dBplt = plot(
    r,
    20.0 * log10.(abs.(femSolution) ./ abs.(exactSolution)),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[dB re Exact]",
    title="Error with respect Exact Solution (Magnitude)"
)

angRplt = plot(
    r,
    angle.(femSolution ./ exactSolution),
    legend=false,
    framestyle=:box,
    xlabel="Distance from Source [m]",
    ylabel="[rad re Exact]",
    title="Error with respect Exact Solution (Phase)"
)

l = @layout [grid(2,2) grid(2, 1)]

errorPlot = plot(realplt, imagplt, absplt, angplt, dBplt, angRplt, layout=l)

display(errorPlot)

readline()

max_stars_repo_name: CrocoDuckoDucks/Acoustics-Models

max_issues_repo_name: CrocoDuckoDucks/Acoustics-Models

max_forks_repo_name: CrocoDuckoDucks/Acoustics-Models


text:
module ReadDataFile

export Reader, read_str, read_bool, read_int, read_flt, read_int_array, read_flt_array, close_reader

mutable struct Reader

    fname::String
    istream::IOStream
    line::String
    lineno::Int

    # a constructor: opens a file, sets lineno
    function Reader(fname::String)
        istream = open(fname, "r")
        r = new(fname,istream, "", 0)
        return r
    end
end

finalizer(r::Reader, close_reader) = close_reader(r)

function readln(r::Reader)
    skip_rexp = r"^\s*(?:#|$)"
    rexp = r"^\s*(.*?)\s*(?:(#|=:)\s*(.*?)\s*$|$)"

    while true
        r.line = readline(r.istream)
        r.lineno += 1
        # skip empty lines and comments starting from #
        if match(skip_rexp, r.line) == nothing
            break
        end
    end
    m = match(rexp, r.line)
    r.line = m.captures[1]
end

function read_str(r::Reader)
    readln(r)
    return r.line
end

function read_bool(r::Reader)
    readln(r)
    if r.line != "T" && r.line != "F"
        local fname = r.fname, lineno = r.lineno
        error("$fname($lineno): error parsing boolean")
    end
    return r.line == "T"
end

function read_int(r::Reader)
    readln(r)
    return parse(Int,r.line)
end

function read_flt(r::Reader)
    readln(r)
    return parse(Float64, r.line)
end

function read_raw(r::Reader)
    readln(r)
    return split(r.line)
end

function read_array(r::Reader, parser, len)
    buf = read_raw(r)
    buflen = length(buf)
    @assert(buflen == len)
    return [parser(buf[i]) for i = 1:buflen]
end

read_int_array(r::Reader, len::Int) = read_array(r, x -> parse(Int, x), len)
read_flt_array(r::Reader, len::Int) = read_array(r, x -> parse(Float64, x), len)

function close_reader(r::Reader)
    println("closing Reader $(r.fname)")
    Base.close(r.istream)
end

# ```jlcon
# julia> include("ReadDataFile.jl")
#
# julia> using .ReadDataFile
#
# julia> r = Reader("claw.data")
# -I- claw.data 6 blank lines were skipped
# Reader("claw.data",IOStream(<file claw.data>),"1",6,r"^\s*(.*?)\s*(?:#\s*(.*?)\s*$|$)")
#
# julia> readint(r)
# 1
#
# julia> readflt(r)
# -1.0
# ```

end # ReadDataFile


text:
ambient = Dict{Symbol,Union{Missing, Number, String}}([
    :release_pressure => 101325.0,   # Pa
    :release_temperature => 298.15,# K
    :ambient_density => 1.225,     # kg/m^3
    :ambient_pressure => 101325.0,   # Pa
    :ambient_temperature => 298.15,# K
])

default_windspeeds = Dict{String, Number}([
# these are pretty arbitrary, it would be better to have a reference for these
    "A" => 3.0,
    "B" => 5.0,
    "C" => 6.0,
    "D" => 6.0,
    "E" => 5.0,
    "F" => 3.0,
])

"""
    scenario_builder(P, T, conditions; <keyword arguments>)

Builds a scenario given a release pressure, temperature, and set of ambient
conditions.

# Arguments
- P::Number, the release pressure in Pa
- T::Number, the release temperature in K
- conditions::Scenario=ambient, a scenario or dictionary with the ambient conditions
- model=:jet, the model, defaults to a jet
- phase=:liquid, the phase of the release, defaults to liquid
- stability::String, the Pasquill atmospheric stability
- windspeed::Number, the windspeed, defaults to a look-up table for stability
- kwargs... each model requires additional keyword arguments
"""
function scenario_builder(P::Number, T::Number, conditions::AbstractDict=ambient; stability::String, windspeed::Union{Missing,Number}=missing,
                          model=:jet, phase=:liquid, kwargs...)

    d = Dict{Symbol, Any}([ key => get(Missing, conditions, key) for key in fieldnames(Scenario)])

    if ismissing(windspeed)
        windspeed = default_windspeeds[stability]
    end

    d[:windspeed] = windspeed
    d[:pasquill_gifford] = stability

    if model == :jet && phase == :liquid
        required_params = [:liquid_density, :hole_diameter]
        if all(key -> key ∈ keys(kwargs), required_params)
            ρⱼ = kwargs[:liquid_density]
            Dⱼ = kwargs[:hole_diameter]
            cd = if (:discharge_coeff ∈ keys(kwargs)) kwargs[:discharge_coeff] else 0.61 end
            Pₐ = d[:ambient_pressure]

            m, uⱼ = liquid_jet(P, Pₐ, ρⱼ, Dⱼ; cd=cd)

            d[:mass_emission_rate] = m
            d[:jet_diameter] = Dⱼ
            d[:jet_velocity] = uⱼ
            d[:jet_density] = ρⱼ
            d[:release_pressure] = P
            d[:release_temperature] = T
            d[:release_height] = if (:release_height ∈ keys(kwargs)) kwargs[:release_height] else 0.0 end
        else
            missing_params = [ String(i) for i in filter(key -> !(key ∈ keys(kwargs)), required_params)]
            error_string = "Missing parameters: " * join(missing_params, ", ")
            error(error_string)
        end
    else
        error("Not implemented")
    end

    return Scenario(d)

end

function scenario_builder(P::Number, T::Number, conditions::Scenario; stability::String, windspeed::Union{Missing,Number}=missing,
                          model=:jet, phase=:liquid, kwargs...)

    d = Dict([ key => getproperty(conditions, key) for key in fieldnames(Scenario)])
    return scenario_builder(P, T; stability=stability, conditions=d, windspeed=windspeed,
                            model=model, phase=phase, kwargs...)
end

"""
    liquid_jet(P₁, P₂, ρ, D; cd=0.61)

Returns the mass emission rate (kg/s) and jet velocity (m/s) for a liquid jetting
through a circular hole of diameter D, with discharge coefficient cd.

# Arguments
- P₁::Number, the internal pressure in Pa
- P₂::Number, the external pressure in Pa
- ρ::Number, the liquid density in kg/m^3
- D::Number, the diameter of the hole in m, assumed circular
- cd::Number=0.61, the discharge coefficient
"""
function liquid_jet(P₁, P₂, ρ, D; cd=0.61)

    u = cd * √( 2*( (P₁ - P₂) / ρ))
    A = (π/4)*D^2
    m = ρ*A*u

    return m, u
end


text:
import StatsBase
n = 300000

mydata = rand(n)

intIdx = StatsBase.sample(1:n, fld(n, 2), replace=false)
sort!(intIdx)
complIdx = Vector{eltype(intIdx)}(undef, 0)
for i = 1:n
    found = searchsorted(intIdx, i)
    if length(found) < 1
        push!(complIdx, i)
    end
end

function sumlr(dat, l, r)
    sl = sr = zero(eltype(dat))

    for i in l
        sl += dat[i]
    end
    for i in r
        sr += dat[i]
    end
    sl, sr
end

u32a = convert(Vector{UInt32}, intIdx)
u32b = convert(Vector{UInt32}, complIdx)
@benchmark sumlr(mydata, u32a, u32b)
@benchmark sumlr(mydata, intIdx, complIdx)


text:
using BenchmarkTools
β = 0.42

@btime magnetisation(ctm(isingpart(β), 64, maxit = 100, verbose=false, tol = -1.)[2])


text:
function kink_equation(cur_reactor::AbstractReactor)
  cur_G_T = K_KF(cur_reactor)

  cur_equation = Equation(1, 1, -1, cur_G_T)

  cur_equation
end


text:
using Documenter, Maker

# Build documentation.
# ====================

makedocs(
    # options
    modules = [Maker],
    clean   = false
)

# Deploy built documentation from Travis.
# =======================================

# Needs to install an additional dep, mkdocs-material, so provide a custom `deps`.
custom_deps() = run(`pip install --user pygments mkdocs mkdocs-material`)

deploydocs(
    # options
    deps = custom_deps,
    julia   = "release",
    repo = "github.com/tshort/Maker.jl.git"
)


text:
## Creates a simple Euler--Maruyama discretization of a diffusion in which the
## drift coefficient corresponds to the Lorenz96 model and the diffusion
## coefficient is constant.

module Lorenz96

using SequentialMonteCarlo
using RNGPool
using StaticArrays
import SMCExamples.Particles.MVFloat64Particle
using Random

struct LorenzTheta
  σ::Float64
  F::Float64
  δ::Float64
  Δ::Float64
  steps::Int64
end

struct LorenzScratch{d}
  t1::MVector{d, Float64}
  t2::MVector{d, Float64}
end
LorenzScratch{d}() where d = LorenzScratch{d}(MVector{d, Float64}(undef),
  MVector{d, Float64}(undef))

@inline function lorenz(F::Float64, x::MVector{d, Float64},
  z::MVector{d, Float64}) where d
  @inbounds z[1] = (x[2] - x[d-1]) * x[d]
  @inbounds z[2] = (x[3] - x[d]) * x[1]
  for i = 3:d-1
    @inbounds z[i] = (x[i+1] - x[i-2]) * x[i-1]
  end
  @inbounds z[d] = (x[1] - x[d-2]) * x[d-1]
  z .+= F
end

function makeLorenzModel(theta::LorenzTheta,
  ys::Vector{SVector{d, Float64}}) where d
  n::Int64 = length(ys)
  σ::Float64 = theta.σ
  F::Float64 = theta.F
  invδ²over2 = 0.5 / (theta.δ * theta.δ)
  logncG = - 0.5 * d * log(2 * π * theta.δ * theta.δ)
  steps::Float64 = theta.steps
  Δ::Float64 = theta.Δ
  h::Float64 = Δ/steps
  sqrth::Float64 = sqrt(Δ/steps)

  @inline function lG(p::Int64, particle::MVFloat64Particle{d}, ::LorenzScratch{d})
    r::Float64 = 0.0
    for i = 1:d
      @inbounds v::Float64 = particle.x[i] - ys[p][i]
      r += logncG - v * invδ²over2 * v
    end
    return r
  end
  ## Euler--Maruyama
  @inline function M!(newParticle::MVFloat64Particle{d}, rng::RNG, p::Int64,
    particle::MVFloat64Particle{d}, scratch::LorenzScratch{d})
    if p == 1
      randn!(rng, newParticle.x)
      newParticle.x .*= σ
    else
      scratch.t1 .= particle.x
      for i = 1:steps
        lorenz(F, scratch.t1, scratch.t2)
        scratch.t2 .*= h
        randn!(rng, scratch.t1)
        scratch.t1 .*= σ * sqrth
        scratch.t1 .+= scratch.t2
      end
      newParticle.x .= scratch.t1
    end
  end
  return SMCModel(M!, lG, length(ys), MVFloat64Particle{d}, LorenzScratch{d})
end

function simulateLorenzModel(theta::LorenzTheta, d::Int64, n::Int64)
  model = makeLorenzModel(theta, Vector{SVector{d, Float64}}(undef, 0))
  ys = Vector{SVector{d, Float64}}(undef, n)
  xParticle = MVFloat64Particle{d}()
  xScratch = LorenzScratch{d}()
  rng = getRNG()
  for p in 1:n
    model.M!(xParticle, rng, p, xParticle, xScratch)
    ys[p] = xParticle.x + theta.δ * randn(rng, d)
  end
  return ys
end

function defaultLorenzModel(d::Int64, n::Int64)
  theta = LorenzTheta(0.5, 8.0, 0.7, 0.05, 20)
  ys = simulateLorenzModel(theta, d, n)
  model = makeLorenzModel(theta, ys)
  return model, theta, ys
end

end


text:
abstract type Order end

struct GoToAbsPosition{T<:AbstractVector{<:Number}} <: Order
    position::T
    GoToAbsPosition(position::T) where T<:AbstractVector{<:Number} = length(position) != 2 ? error("Position must be a 2d vector.") : new{T}(position)
end

struct RotateBodyBy <: Order
    angle::Number
end

struct RotateHeadBy <: Order
    angle::Number
end

struct SetVelocity{T<:Number} <: Order
    x::T
    y::T
end
Base.eltype(::SetVelocity{T}) where T = T

struct NoOrder <: Order
end


text:
using Images: data, Image, load
ENV["PYTHON"] = "/usr/bin/python3"
using PyCall
using PartsBasedDetector: make_estimator, estimate

filename = "dress.jpg"
url = "http://images0.chictopia.com/photos/districtofchic/9466442894/" *
      "black-skinny-jeans-dl1961-jeans-sky-blue-pixie-market-shirt_400.jpg"

filename_png = replace(filename, ".jpg", ".png")
run(`wget --continue $url -O $filename`)
run(`convert $filename $filename_png`)

estimator = make_estimator()
candidates = estimate(estimator, filename, n_candidates = 3)
parts = candidates[1].parts
x, y = parts[:, 1], parts[:, 2]

@pyimport matplotlib.pyplot as plt
image = plt.imread(filename_png)
plt.imshow(image)
plt.plot(x, y, color="r", marker="o", ls="")
plt.show()

max_stars_repo_name: IshitaTakeshi/VirtualFitting

max_issues_repo_name: IshitaTakeshi/VirtualFitting

max_forks_repo_name: IshitaTakeshi/VirtualFitting


text:
using LocalitySensitive
using BenchmarkTools
using TextAnalysis
using SparseArrays

benchmark_documents = readlines("resources/benchmark_data.csv")

@info "Benchmark fingerprinting"
function fingerprint_benchmark(docs, sh::SimHash)
    for d in docs
       fingerprint(sh, d)
    end
end
sh = SimHash()
els = [[(s,1) for s in shingle(d)] for d in benchmark_documents]
@benchmark fingerprint_benchmark(els, sh)

names_doc = [StringDocument(doc) for doc in benchmark_documents]
crps = Corpus(names_doc)
update_lexicon!(crps)
document_matrix = DocumentTermMatrix(crps)
tfidf = SparseMatrixCSC(tf_idf(document_matrix)')

@benchmark fingerprint_all(sh, tfidf)

fps = fingerprint_all(sh, tfidf)

@info "Push to index"
function pushall(fingerp)
    sh = SimHashIndex(0.85)
    for f in fingerp
        push!(sh, f)
    end
    sh
end

# @benchmark pushall(fp)
shind = pushall(fps)
estimate_cosine(fps[24], fps[25])
find_similar(shind, fps[24])
benchmark_documents[find_similar(shind, fps[24])]
benchmark_documents[find_similar(shind, fps[1])]
benchmark_documents[1]

function simall(fingerp, sh::SimHashIndex)
    for f in fingerp
        find_similar(sh, f)
    end
end
using Profile
Profile.clear()
Profile.init(n=10000000, delay=0.00001)
@benchmark simall(fp, sh)
@profile simall(fp[1:3000], sh)
Profile.print()
using ProfileView
ProfileView.view()
pairs = similar_pairs(sh)