diff --git a/Manifest.toml b/Manifest.toml index cf3ac48..b304107 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -11,9 +11,9 @@ version = "0.8.10" [[Compat]] deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] -git-tree-sha1 = "84aa74986c5b9b898b0d1acaf3258741ee64754f" +git-tree-sha1 = "ed2c4abadf84c53d9e58510b5fc48912c2336fbb" uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" -version = "2.1.0" +version = "2.2.0" [[Dates]] deps = ["Printf"] @@ -92,9 +92,9 @@ uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" [[StaticArrays]] deps = ["LinearAlgebra", "Random", "Statistics"] -git-tree-sha1 = "db23bbf50064c582b6f2b9b043c8e7e98ea8c0c6" +git-tree-sha1 = "1085ffbf5fd48fdba64ef8e902ca429c4e1212d3" uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "0.11.0" +version = "0.11.1" [[Statistics]] deps = ["LinearAlgebra", "SparseArrays"] diff --git a/src/relative_orbits.jl b/src/relative_orbits.jl new file mode 100644 index 0000000..430c274 --- /dev/null +++ b/src/relative_orbits.jl @@ -0,0 +1,242 @@ +############## +# RTN | LVLH # +############## + +export rRTNtoECI +export rECItoRTN + + +""" +Compute the radial, along-track, cross-track (RTN) to Earth-centered inertial +rotation matrix. If applied to a position vector in the RTN frame, it will +transform that vector to into the equivalent position vector in the ECI frame. + +The RTN frame is also commonly refered to as the local-vertical, local-horizontal (LVLH) frame. + +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite + +Returns: +- `R_rtn2eci::SMatrix{3,3}`: Rotation matrix transforming _from_ the RTN frame _to_ the ECI frame +""" +function rRTNtoECI(x::AbstractVector{<:Real}) + r = x[idx1t3] + v = x[idx4t6] + n = cross(r, v) + + R = normalize(r) + N = normalize(n) + T = cross(N, R) + + R_rtn2eci = hcat(R, T, N) + + return R_rtn2eci +end + + +""" +Compute the Earth-centered inertial to radial, along-track, cross-track (RTN) +rotation matrix. If applied to a position vector in the ECI frame, it will +transform that vector into the equivalent position vector in the RTN frame. + +The RTN frame is also commonly refered to as the local-vertical, local-horizontal (LVLH) frame. + +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite + +Returns: +- `R_eci2rtn::SMatrix{3,3}`: Rotation matrix transforming _from_ the ECI frame _to_ the RTN frame +""" +function rECItoRTN(x::AbstractVector{<:Real}) + r = x[idx1t3] + v = x[idx4t6] + n = cross(r, v) + + R = normalize(r) + N = normalize(n) + T = cross(N, R) + + R_eci2rtn = hcat(R, T, N)' + + return R_eci2rtn +end + + +export sECItoRTN +""" +Compute the radial, along-track, cross-track (RTN) coordinates of a target satellite in the primary satellites RTN frame. + +The RTN frame is also commonly refered to as the local-vertical, local-horizontal (LVLH) frame. + +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xt::AbstractVector{<:Real}`: Inertial state (position and velocity) of the target satellite + +Returns: +- `xrtn::AbstractVector{<:Real}`: Position and velocity of the target relative of the observing satellite in the RTN. +""" +function sECItoRTN(x::AbstractVector{<:Real}, xt::AbstractVector{<:Real}) + if length(xt) >= 6 + return sECItoRTN(x, xt[idx1t6]) + else + return sECItoRTN(x, xt[idx1t3]) + end +end + +""" +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xt::SVector{3,<:Real}`: Inertial state (position) of the target satellite + +Returns: +- `xrtn::SVector{3,<:Real}`: Position of the target relative of the observing satellite in the RTN. +""" +function sECItoRTN(x::AbstractVector{<:Real}, xt::SVector{3,<:Real}) + # Create RTN rotation matrix + R_eci2rtn = rECItoRTN(x) + + # Transform Position + r = x[idx1t3] + rho = xt[idx1t3] - r + r_rtn = R_eci2rtn*rho + + return r_rtn +end + +""" +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xt::SVector{6,<:Real}`: Inertial state (position and velocity) of the target satellite + +Returns: +- `xrtn::SVector{6,<:Real}`: Position and velocity of the target relative of the observing satellite in the RTN. +""" +function sECItoRTN(x::AbstractVector{<:Real}, xt::SVector{6,<:Real}) + # Create RTN rotation matrix + R_eci2rtn = rECItoRTN(x) + + # Transform Position + r = x[idx1t3] + rho = xt[idx1t3] - r + r_rtn = R_eci2rtn*rho + + v = x[idx4t6] + f_dot = norm(cross(r, v)) / norm(r)^2 + omega = SVector(0, 0, f_dot) + rho_dot = xt[idx4t6] - v + v_rtn = R_eci2rtn*rho_dot - cross(omega, r_rtn) + + return vcat(r_rtn, v_rtn) +end + + +export sRTNtoECI +""" +Compute the Earth-center + +The RTN frame is also commonly refered to as the local-vertical, local-horizontal (LVLH) frame. + +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xrtn::AbstractVector{<:Real}`: Inertial state (position and velocity) of the target satellite + +Returns: +- `xt::AbstractVector{<:Real}`: Position and velocity of the target relative of the observing satellite in the RTN. +""" +function sRTNtoECI(x::AbstractVector{<:Real}, xrtn::AbstractVector{<:Real}) + if length(xrtn) >= 6 + return sRTNtoECI(x, xrtn[idx1t6]) + else + return sRTNtoECI(x, xrtn[idx1t3]) + end +end + +""" +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xrtn::SVector{3,<:Real}`: Inertial state (position) of the target satellite + +Returns: +- `xt::SVector{3,<:Real}`: Position of the target relative of the observing satellite in the RTN. +""" +function sRTNtoECI(x::AbstractVector{<:Real}, xrtn::SVector{3,<:Real}) + # Create RTN rotation matrix + R_rtn2eci = rRTNtoECI(x) + + # Transform position + r = x[idx1t3] + r_t = R_rtn2eci*xrtn + r + + return r_t +end + +""" +Arguments: +- `x::AbstractVector{<:Real}`: Inertial state (position and velocity) of primary (observing) satellite +- `xrtn::SVector{6,<:Real}`: Inertial state (position and velocity) of the target satellite + +Returns: +- `xt::SVector{6,<:Real}`: Position and velocity of the target relative of the observing satellite in the RTN. +""" +function sRTNtoECI(x::AbstractVector{<:Real}, xrtn::SVector{6,<:Real}) + # Create RTN rotation matrix + R_rtn2eci = rRTNtoECI(x) + + # Transform position + r = x[idx1t3] + r_rtn = xrtn[idx1t3] + r_t = R_rtn2eci*r_rtn + r + + # Transform velocity + v = x[idx4t6] + v_rtn = xrtn[idx4t6] + f_dot = norm(cross(r, v)) / norm(r)^2 + omega = SVector(0, 0, f_dot) + v_t = R_rtn2eci*(v_rtn + cross(omega, r_rtn)) + v + + return vcat(r_t, v_t) +end + +####### +# ROE # +####### + +""" +Convert Keplerian relative orbital element state to ROE relative state +""" +function sOSCtoROE(oe_c::SVector{6,<:Real}, oe_d::SVector{6,<:Real}) +end + +""" +Convert ROE relative state and chief relative orbital elements and return +osculating orbital elements +""" +function sROEtoOSC(oe_c::SVector{6,<:Real}, roe_d::SVector{6,<:Real}) +end + +""" +Convert inertial states to relative orbital element +""" +function sECItoROE(x_c::SVector{6,<:Real}, x_d::SVector{6,<:Real}) +end + +""" +Convert ROE relative state and chief absolute inertial state and return +osculating orbital elements +""" +function sROEtoECI(x_c::SVector{6,<:Real}, roe_d::SVector{6,<:Real}) +end + +""" +Convert RTN state and chief absolute inertial state and return relative +orbital element state +""" +function sRTNtoROE(x_c::SVector{6,<:Real}, rtn_d::SVector{6,<:Real}) +end + +""" +Covert relative orbital element state and chief inertial state and return +RTN state. +""" +function sROEtoRTN(x_c::SVector{6,<:Real}, roe_d::SVector{6,<:Real}) +end \ No newline at end of file diff --git a/test/test_coordinates.jl b/test/test_coordinates.jl index eadf671..3633d8f 100644 --- a/test/test_coordinates.jl +++ b/test/test_coordinates.jl @@ -315,12 +315,15 @@ let azel_enz = sENZtoAZEL(enz, use_degrees=true) azel_sez = sSEZtoAZEL(sez, use_degrees=true) - @test azel_enz[1] == azel_sez[1] - @test azel_enz[2] == azel_sez[2] - @test azel_enz[3] == azel_sez[3] - @test azel_enz[4] == azel_sez[4] - @test azel_enz[5] == azel_sez[5] - @test azel_enz[6] == azel_sez[6] + + tol = 1e-7 + @test isapprox(azel_enz[1], azel_sez[1], atol=tol) + @test isapprox(azel_enz[2], azel_sez[2], atol=tol) + @test isapprox(azel_enz[3], azel_sez[3], atol=tol) + @test isapprox(azel_enz[4], azel_sez[4], atol=tol) + @test isapprox(azel_enz[5], azel_sez[5], atol=tol) + @test isapprox(azel_enz[6], azel_sez[6], atol=tol) + end let diff --git a/test/test_relative_orbits.jl b/test/test_relative_orbits.jl new file mode 100644 index 0000000..be719a6 --- /dev/null +++ b/test/test_relative_orbits.jl @@ -0,0 +1,53 @@ +let + epc = Epoch(2018,1,1,12,0,0) + + a = R_EARTH + 500e3 + oe = [a, 0, 0, 0, 0, sqrt(GM_EARTH/a)] + eci = sOSCtoCART(oe, use_degrees=true) + + r_rtn = rRTNtoECI(eci) + r_rtn_t = rECItoRTN(eci) + + r = r_rtn * r_rtn_t + + tol = 1e-8 + @test isapprox(r[1, 1], 1.0, atol=tol) + @test isapprox(r[1, 2], 0.0, atol=tol) + @test isapprox(r[1, 3], 0.0, atol=tol) + + @test isapprox(r[2, 1], 0.0, atol=tol) + @test isapprox(r[2, 2], 1.0, atol=tol) + @test isapprox(r[2, 3], 0.0, atol=tol) + + @test isapprox(r[3, 1], 0.0, atol=tol) + @test isapprox(r[3, 2], 0.0, atol=tol) + @test isapprox(r[3, 3], 1.0, atol=tol) +end + +let + epc = Epoch(2018,1,1,12,0,0) + + oe = [R_EARTH + 500e3, 0, 0, 0, 0, 0] + eci = sOSCtoCART(oe, use_degrees=true) + + xt = deepcopy(eci) + [100, 0, 0, 0, 0, 0] + + x_rtn = sECItoRTN(eci, xt) + + tol = 1e-8 + @test isapprox(x_rtn[1], 100.0, atol=tol) + @test isapprox(x_rtn[2], 0.0, atol=tol) + @test isapprox(x_rtn[3], 0.0, atol=tol) + @test isapprox(x_rtn[4], 0.0, atol=tol) + @test isapprox(x_rtn[5], 0.0, atol=0.5) + @test isapprox(x_rtn[6], 0.0, atol=tol) + + xt2 = sRTNtoECI(eci, x_rtn) + + @test isapprox(xt[1], xt2[1], atol=tol) + @test isapprox(xt[2], xt2[2], atol=tol) + @test isapprox(xt[3], xt2[3], atol=tol) + @test isapprox(xt[4], xt2[4], atol=tol) + @test isapprox(xt[5], xt2[5], atol=tol) + @test isapprox(xt[6], xt2[6], atol=tol) +end \ No newline at end of file