From 12c2a47f9433a4a1081372c2efee332ba0695774 Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Tue, 13 Dec 2022 17:45:41 +0100 Subject: [PATCH 1/7] Calc velocity and clock drift --- src/PositionVelocityTime.jl | 27 ++++++++--- src/sat_position.jl | 93 ++++++++++++++++++++++++++++++++++++- src/sat_time.jl | 10 ++++ src/user_position.jl | 37 ++++++++++++--- test/pvt.jl | 22 ++++++++- test/runtests.jl | 1 + 6 files changed, 174 insertions(+), 16 deletions(-) diff --git a/src/PositionVelocityTime.jl b/src/PositionVelocityTime.jl index f6f99f3..0ee0c40 100755 --- a/src/PositionVelocityTime.jl +++ b/src/PositionVelocityTime.jl @@ -9,7 +9,8 @@ using CoordinateTransformations, AstroTime, LsqFit, StaticArrays, - Tracking + Tracking, + Unitful using Unitful: s, Hz @@ -21,12 +22,14 @@ export calc_pvt, get_LLA, get_num_used_sats, calc_satellite_position, + calc_satellite_position_and_velocity, get_sat_enu, get_gdop, get_pdop, get_hdop, get_vdop, - get_tdop + get_tdop, + get_frequency_offset """ Struct of decoder, code- and carrierphase of satellite @@ -35,6 +38,7 @@ Struct of decoder, code- and carrierphase of satellite decoder::GNSSDecoder.GNSSDecoderState system::AbstractGNSS code_phase::CP + carrier_doppler::typeof(1.0Hz) carrier_phase::CP = 0.0 end @@ -46,6 +50,7 @@ function SatelliteState( decoder, get_system(tracking_results), get_code_phase(tracking_results), + get_carrier_doppler(tracking_results), get_carrier_phase(tracking_results), ) end @@ -67,8 +72,10 @@ positions. """ @with_kw struct PVTSolution position::ECEF = ECEF(0, 0, 0) + velocity::ECEF = ECEF(0, 0, 0) time_correction::Float64 = 0 time::Union{TAIEpoch{Float64},Nothing} = nothing + relative_clock_drift::Float64 = 0 dop::Union{DOP,Nothing} = nothing used_sats::Vector{Int64} = [] sat_positions::Vector{ECEF} = [] @@ -137,14 +144,18 @@ function calc_pvt( prev_ξ = [prev_pvt.position; prev_pvt.time_correction] healthy_prns = map(state -> state.decoder.prn, healthy_states) times = map(state -> calc_corrected_time(state), healthy_states) - sat_positions = map( - (state, time) -> calc_satellite_position(state.decoder, time), + sat_positions_and_velocities = map( + (state, time) -> calc_satellite_position_and_velocity(state.decoder, time), healthy_states, times, ) + sat_positions = map(get_sat_position, sat_positions_and_velocities) pseudo_ranges, reference_time = calc_pseudo_ranges(times) ξ = user_position(sat_positions, pseudo_ranges, prev_ξ) + user_velocity_and_clock_drift = calc_user_velocity_and_clock_drift(sat_positions_and_velocities, ξ, states) position = ECEF(ξ[1], ξ[2], ξ[3]) + velocity = ECEF(user_velocity_and_clock_drift[1], user_velocity_and_clock_drift[2], user_velocity_and_clock_drift[3]) + relative_clock_drift = user_velocity_and_clock_drift[4] / SPEEDOFLIGHT time_correction = ξ[4] corrected_reference_time = reference_time + time_correction / SPEEDOFLIGHT @@ -155,9 +166,13 @@ function calc_pvt( corrected_reference_time - floor(Int, corrected_reference_time), ) - dop = calc_DOP(H(reduce(hcat, sat_positions), ξ)) + dop = calc_DOP(calc_H(reduce(hcat, sat_positions), ξ)) - PVTSolution(position, time_correction, time, dop, healthy_prns, sat_positions) + PVTSolution(position, velocity, time_correction, time, relative_clock_drift, dop, healthy_prns, sat_positions) +end + +function get_frequency_offset(system::AbstractGNSS, pvt::PVTSolution) + pvt.relative_clock_drift * get_center_frequency(system) end function get_system_start_time( diff --git a/src/sat_position.jl b/src/sat_position.jl index 9fb8fa5..34c754b 100755 --- a/src/sat_position.jl +++ b/src/sat_position.jl @@ -27,15 +27,28 @@ $SIGNATURES `t`: time at satellite in system time """ function calc_satellite_position(decoder::GNSSDecoder.GNSSDecoderState, t) + pos_and_vel = calc_satellite_position_and_velocity(decoder, t) + pos_and_vel.position +end + +function calc_satellite_position_and_velocity(decoder::GNSSDecoder.GNSSDecoderState, t) data = decoder.data constants = decoder.constants semi_major_axis = data.sqrt_A^2 time_from_ephemeris_reference_epoch = correct_week_crossovers(t - data.t_0e) + computed_mean_motion = sqrt(decoder.constants.μ) / data.sqrt_A^3 + corrected_mean_motion = computed_mean_motion + data.Δn eccentric_anomaly = calc_eccentric_anomaly(decoder, t) + eccentric_anomaly_dot = corrected_mean_motion / (1.0 - data.e * cos(eccentric_anomaly)) β = data.e / (1 + sqrt(1 - data.e^2)) true_anomaly = eccentric_anomaly + 2 * atan(β * sin(eccentric_anomaly) / (1 - β * cos(eccentric_anomaly))) + true_anomaly_dot = + sin(eccentric_anomaly) * + eccentric_anomaly_dot * + (1.0 + data.e * cos(true_anomaly)) / + (sin(true_anomaly) * (1.0 - data.e * cos(eccentric_anomaly))) argument_of_latitude = true_anomaly + data.ω argrument_of_latitude_correction = data.C_us * sin(2 * argument_of_latitude) + @@ -51,12 +64,50 @@ function calc_satellite_position(decoder::GNSSDecoder.GNSSDecoderState, t) semi_major_axis * (1 - data.e * cos(eccentric_anomaly)) + radius_correction corrected_inclination = data.i_0 + inclination_correction + data.i_dot * time_from_ephemeris_reference_epoch + + corrected_argument_of_latitude_dot = + true_anomaly_dot + + 2 * + ( + data.C_us * cos(2 * corrected_argument_of_latitude) - + data.C_uc * sin(2 * corrected_argument_of_latitude) + ) * + true_anomaly_dot + corrected_radius_dot = + semi_major_axis * data.e * sin(eccentric_anomaly) * corrected_mean_motion / + (1.0 - data.e * cos(eccentric_anomaly)) + + 2 * + ( + data.C_rs * cos(2 * corrected_argument_of_latitude) - + data.C_rc * sin(2 * corrected_argument_of_latitude) + ) * + true_anomaly_dot + corrected_inclination_dot = + data.i_dot + + ( + data.C_is * cos(2 * corrected_argument_of_latitude) - + data.C_ic * sin(2 * corrected_argument_of_latitude) + ) * + 2 * + true_anomaly_dot + x_position_in_orbital_plane = corrected_radius * cos(corrected_argument_of_latitude) y_position_in_orbital_plane = corrected_radius * sin(corrected_argument_of_latitude) + + x_position_in_orbital_plane_dot = + corrected_radius_dot * cos(corrected_argument_of_latitude) - + y_position_in_orbital_plane * corrected_argument_of_latitude_dot + y_position_in_orbital_plane_dot = + corrected_radius_dot * sin(corrected_argument_of_latitude) + + x_position_in_orbital_plane * corrected_argument_of_latitude_dot + corrected_longitude_of_ascending_node = data.Ω_0 + (data.Ω_dot - constants.Ω_dot_e) * time_from_ephemeris_reference_epoch - constants.Ω_dot_e * data.t_0e - SVector( + + corrected_longitude_of_ascending_node_dot = data.Ω_dot - constants.Ω_dot_e + + position = SVector( x_position_in_orbital_plane * cos(corrected_longitude_of_ascending_node) - y_position_in_orbital_plane * cos(corrected_inclination) * @@ -67,6 +118,40 @@ function calc_satellite_position(decoder::GNSSDecoder.GNSSDecoderState, t) cos(corrected_longitude_of_ascending_node), y_position_in_orbital_plane * sin(corrected_inclination), ) + + velocity = SVector( + ( + x_position_in_orbital_plane_dot - + y_position_in_orbital_plane * + cos(corrected_inclination) * + corrected_longitude_of_ascending_node_dot + ) * cos(corrected_longitude_of_ascending_node) - + ( + x_position_in_orbital_plane * corrected_longitude_of_ascending_node_dot + + y_position_in_orbital_plane_dot * cos(corrected_inclination) - + y_position_in_orbital_plane * + sin(corrected_inclination) * + corrected_inclination_dot + ) * sin(corrected_longitude_of_ascending_node), + ( + x_position_in_orbital_plane_dot - + y_position_in_orbital_plane * + cos(corrected_inclination) * + corrected_longitude_of_ascending_node_dot + ) * sin(corrected_longitude_of_ascending_node) + + ( + x_position_in_orbital_plane * corrected_longitude_of_ascending_node_dot + + y_position_in_orbital_plane_dot * cos(corrected_inclination) - + y_position_in_orbital_plane * + sin(corrected_inclination) * + corrected_inclination_dot + ) * cos(corrected_longitude_of_ascending_node), + y_position_in_orbital_plane_dot * sin(corrected_inclination) + + y_position_in_orbital_plane * + cos(corrected_inclination) * + corrected_inclination_dot, + ) + (position = position, velocity = velocity) end """ @@ -77,8 +162,12 @@ $SIGNATURES `state`: Satellite state (SatelliteState) """ function calc_satellite_position(state::SatelliteState) + pos_and_vel = calc_satellite_position_and_velocity(state) + pos_and_vel.position +end +function calc_satellite_position_and_velocity(state::SatelliteState) t = calc_corrected_time(state) - calc_satellite_position(state.decoder, t) + calc_satellite_position_and_velocity(state.decoder, t) end """ diff --git a/src/sat_time.jl b/src/sat_time.jl index 7b38980..de2cfd5 100644 --- a/src/sat_time.jl +++ b/src/sat_time.jl @@ -30,6 +30,16 @@ function correct_clock(decoder::GNSSDecoder.GNSSDecoderState, t) t - correct_by_group_delay(decoder, Δt) end +function calc_satellite_clock_drift(decoder::GNSSDecoder.GNSSDecoderState, t) + decoder.data.a_f1 + + decoder.data.a_f2 * t * 2 +end + +function calc_satellite_clock_drift(state::SatelliteState) + approximated_time = calc_uncorrected_time(state) + calc_satellite_clock_drift(state.decoder, approximated_time) +end + function correct_by_group_delay( decoder::GNSSDecoder.GNSSDecoderState{<:GNSSDecoder.GPSL1Data}, t, diff --git a/src/user_position.jl b/src/user_position.jl index d481b10..92ac06c 100755 --- a/src/user_position.jl +++ b/src/user_position.jl @@ -6,7 +6,7 @@ $SIGNATURES `ξ`: Combination of estimated user position and time correction `sat_positions`: Satellite Positions """ -function ρ_hat(sat_positions, ξ) +function calc_ρ_hat(sat_positions, ξ) rₙ = ξ[1:3] tc = ξ[4] @@ -35,7 +35,7 @@ $SIGNATURES `ξ`: Combination of estimated user position and time correction `sat_pos`: Single satellite positions """ -function e(sat_pos, ξ) +function calc_e(sat_pos, ξ) rₙ = ξ[1:3] travel_time = norm(sat_pos - rₙ) / SPEEDOFLIGHT rotated_sat_pos = rotate_by_earth_rotation(sat_pos, travel_time) @@ -50,8 +50,8 @@ $SIGNATURES `ξ`: Combination of estimated user position and time correction `sat_positions`: Matrix of satellite positions """ -function H(sat_positions, ξ) - mapreduce(sat_pos -> [transpose(e(sat_pos, ξ)) 1], vcat, eachcol(sat_positions)) +function calc_H(sat_positions, ξ) + mapreduce(sat_pos -> [transpose(calc_e(sat_pos, ξ)) 1], vcat, eachcol(sat_positions)) end """ @@ -83,9 +83,34 @@ Calculates the user position by least squares method. The algorithm is based on function user_position(sat_positions, ρ, prev_ξ = zeros(4)) sat_positions_mat = reduce(hcat, sat_positions) - ξ_fit_ols = curve_fit(ρ_hat, H, sat_positions_mat, ρ, collect(prev_ξ)) + ξ_fit_ols = curve_fit(calc_ρ_hat, calc_H, sat_positions_mat, ρ, collect(prev_ξ)) # wt = 1 ./ (ξ_fit_ols.resid .^ 2) # ξ_fit_wls = curve_fit(ρ_hat, H, sat_positions_mat, ρ, wt, collect(prev_ξ)) return ξ_fit_ols.param -end \ No newline at end of file +end + +""" +Computes user velocity + +$SIGNATURES + +Calculates the user velocity and clock drift +""" +function calc_user_velocity_and_clock_drift(sat_positions_and_velocities, ξ, states) + sat_positions = map(get_sat_position, sat_positions_and_velocities) + sat_positions_mat = reduce(hcat, sat_positions) + sat_clock_drifts = map(calc_satellite_clock_drift, states) + sat_dopplers = map(x -> upreferred(x.carrier_doppler / Hz), states) + H = collect(calc_H(sat_positions_mat, ξ)) + center_frequency = get_center_frequency(first(states).system) + λ = SPEEDOFLIGHT / upreferred(center_frequency / Hz) + y = + sat_dopplers * λ - + sat_clock_drifts * SPEEDOFLIGHT - + map(x -> dot(calc_e(get_sat_position(x), ξ), get_sat_velocity(x)), sat_positions_and_velocities) + H \ y +end + +get_sat_position(x) = x.position +get_sat_velocity(x) = x.velocity \ No newline at end of file diff --git a/test/pvt.jl b/test/pvt.jl index 4e1c127..532f848 100644 --- a/test/pvt.jl +++ b/test/pvt.jl @@ -1,6 +1,6 @@ BitIntegers.@define_integers 288 BitIntegers.@define_integers 320 -@testset "PVT Galileo E1B" begin +@testset "PVT Galileo E1B with frequency offset of $freq_offset" for freq_offset in (0.0Hz, 500Hz, -1000Hz) galileo_e1b = GalileoE1B() states = [ SatelliteState(; @@ -94,6 +94,7 @@ BitIntegers.@define_integers 320 false, ), system = galileo_e1b, + carrier_doppler = 1617.3312825078192Hz + freq_offset, code_phase = 2216.5793761534956, carrier_phase = -1.461823180908076, ), @@ -188,6 +189,7 @@ BitIntegers.@define_integers 320 true, ), system = galileo_e1b, + carrier_doppler = 4055.9318556579988Hz + freq_offset, code_phase = 48.75149191469789, carrier_phase = -2.3877702498883373, ), @@ -282,6 +284,7 @@ BitIntegers.@define_integers 320 false, ), system = galileo_e1b, + carrier_doppler = 209.7024609093128Hz + freq_offset, code_phase = 69.54389556899758, carrier_phase = -1.4138935647217596, ), @@ -376,6 +379,7 @@ BitIntegers.@define_integers 320 true, ), system = galileo_e1b, + carrier_doppler = -974.6289079820336Hz + freq_offset, code_phase = 1701.8894620076721, carrier_phase = -1.721713905186919, ), @@ -470,6 +474,7 @@ BitIntegers.@define_integers 320 true, ), system = galileo_e1b, + carrier_doppler = 4089.415808665647Hz + freq_offset, code_phase = 4015.495436832823, carrier_phase = -2.8554107708683047, ), @@ -479,9 +484,11 @@ BitIntegers.@define_integers 320 @test get_LLA(pvt) ≈ LLA(; lat = 50.778851672464015, lon = 6.065568885758519, alt = 289.4069805158367) @test pvt.time ≈ TAIEpoch(2021, 5, 31, 12, 53, 14.1183385390904732) + @test pvt.velocity ≈ ECEF(0.0, 0.0, 0.0) atol = 9 + @test get_frequency_offset(galileo_e1b, pvt) ≈ (1675.63Hz + freq_offset) atol = 0.01Hz end -@testset "PVT GPS L1" begin +@testset "PVT GPS L1 with frequency offset of $freq_offset" for freq_offset in (0.0Hz, 500Hz, -1000Hz) gpsl1 = GPSL1() states = [ SatelliteState(; @@ -582,6 +589,7 @@ end false, ), system = gpsl1, + carrier_doppler = 2669.8440799388595Hz + freq_offset, code_phase = 4876.431542382193, carrier_phase = 0.09402551301430394, ), @@ -683,6 +691,7 @@ end false, ), system = gpsl1, + carrier_doppler = 4704.3549972665805Hz + freq_offset, code_phase = 8455.107739656896, carrier_phase = 2.7614518715603946, ), @@ -784,6 +793,7 @@ end true, ), system = gpsl1, + carrier_doppler = 4603.179391134832Hz + freq_offset, code_phase = 10510.15670303955, carrier_phase = -0.7447786034769108, ), @@ -885,6 +895,7 @@ end false, ), system = gpsl1, + carrier_doppler = 3144.174219887768Hz + freq_offset, code_phase = 3618.5099300503684, carrier_phase = -0.22375187424152987, ), @@ -986,6 +997,7 @@ end false, ), system = gpsl1, + carrier_doppler = 595.7926881306387Hz + freq_offset, code_phase = 17600.66651489137, carrier_phase = 2.59152430602131, ), @@ -1087,6 +1099,7 @@ end true, ), system = gpsl1, + carrier_doppler = -794.0022484221436Hz + freq_offset, code_phase = 14504.587373634655, carrier_phase = -0.7416765461612318, ), @@ -1188,6 +1201,7 @@ end true, ), system = gpsl1, + carrier_doppler = 2826.783251528247Hz + freq_offset, code_phase = 14394.465193705755, carrier_phase = -0.7597910878699417, ), @@ -1289,6 +1303,7 @@ end true, ), system = gpsl1, + carrier_doppler = -1083.2317687377372Hz + freq_offset, code_phase = 13905.233170289455, carrier_phase = -2.1444956909602526, ), @@ -1390,6 +1405,7 @@ end false, ), system = gpsl1, + carrier_doppler = 3393.7445379085816Hz + freq_offset, code_phase = 16861.02713837273, carrier_phase = -0.9185401811870872, ), @@ -1399,4 +1415,6 @@ end @test get_LLA(pvt) ≈ LLA(; lat = 50.77885249310784, lon = 6.0656199911189175, alt = 291.95658091689086) @test pvt.time ≈ TAIEpoch(2021, 5, 31, 12, 53, 14.1491024351271335) + @test pvt.velocity ≈ ECEF(0.0, 0.0, 0.0) atol = 2.5 + @test get_frequency_offset(gpsl1, pvt) ≈ (1632.59Hz + freq_offset) atol = 0.01Hz end \ No newline at end of file diff --git a/test/runtests.jl b/test/runtests.jl index 0dd6925..181c33a 100755 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,5 +1,6 @@ using Test, PositionVelocityTime, GNSSDecoder, AstroTime, BitIntegers, GNSSSignals, Geodesy +using Unitful: Hz include("sat_time.jl") include("pvt.jl") \ No newline at end of file From 35d661c7ebf55d51d796d372320fb57464961099 Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 14 Dec 2022 11:04:53 +0100 Subject: [PATCH 2/7] Calc frequency offset for any given base frequency --- src/PositionVelocityTime.jl | 4 ++-- test/pvt.jl | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/PositionVelocityTime.jl b/src/PositionVelocityTime.jl index 0ee0c40..4c7bac3 100755 --- a/src/PositionVelocityTime.jl +++ b/src/PositionVelocityTime.jl @@ -171,8 +171,8 @@ function calc_pvt( PVTSolution(position, velocity, time_correction, time, relative_clock_drift, dop, healthy_prns, sat_positions) end -function get_frequency_offset(system::AbstractGNSS, pvt::PVTSolution) - pvt.relative_clock_drift * get_center_frequency(system) +function get_frequency_offset(pvt::PVTSolution, base_frequency) + pvt.relative_clock_drift * base_frequency end function get_system_start_time( diff --git a/test/pvt.jl b/test/pvt.jl index 532f848..2a18edb 100644 --- a/test/pvt.jl +++ b/test/pvt.jl @@ -485,7 +485,7 @@ BitIntegers.@define_integers 320 LLA(; lat = 50.778851672464015, lon = 6.065568885758519, alt = 289.4069805158367) @test pvt.time ≈ TAIEpoch(2021, 5, 31, 12, 53, 14.1183385390904732) @test pvt.velocity ≈ ECEF(0.0, 0.0, 0.0) atol = 9 - @test get_frequency_offset(galileo_e1b, pvt) ≈ (1675.63Hz + freq_offset) atol = 0.01Hz + @test get_frequency_offset(pvt, get_center_frequency(galileo_e1b)) ≈ (1675.63Hz + freq_offset) atol = 0.01Hz end @testset "PVT GPS L1 with frequency offset of $freq_offset" for freq_offset in (0.0Hz, 500Hz, -1000Hz) @@ -1416,5 +1416,5 @@ end LLA(; lat = 50.77885249310784, lon = 6.0656199911189175, alt = 291.95658091689086) @test pvt.time ≈ TAIEpoch(2021, 5, 31, 12, 53, 14.1491024351271335) @test pvt.velocity ≈ ECEF(0.0, 0.0, 0.0) atol = 2.5 - @test get_frequency_offset(gpsl1, pvt) ≈ (1632.59Hz + freq_offset) atol = 0.01Hz + @test get_frequency_offset(pvt, get_center_frequency(gpsl1)) ≈ (1632.59Hz + freq_offset) atol = 0.01Hz end \ No newline at end of file From 57344350579d88686e44f732307eeb212a8d76a1 Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 14 Dec 2022 18:05:25 +0100 Subject: [PATCH 3/7] More reliable PVT --- src/PositionVelocityTime.jl | 3 +++ src/user_position.jl | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/src/PositionVelocityTime.jl b/src/PositionVelocityTime.jl index 4c7bac3..2846449 100755 --- a/src/PositionVelocityTime.jl +++ b/src/PositionVelocityTime.jl @@ -167,6 +167,9 @@ function calc_pvt( ) dop = calc_DOP(calc_H(reduce(hcat, sat_positions), ξ)) + if dop.GDOP < 0 + return PVTSolution() + end PVTSolution(position, velocity, time_correction, time, relative_clock_drift, dop, healthy_prns, sat_positions) end diff --git a/src/user_position.jl b/src/user_position.jl index 92ac06c..f32f7f1 100755 --- a/src/user_position.jl +++ b/src/user_position.jl @@ -62,6 +62,16 @@ $SIGNATURES """ function calc_DOP(H_GEO) D = inv(Symmetric(collect(H_GEO' * H_GEO))) + if D[4, 4] < 0 || + D[3, 3] < 0 || + D[1, 1] + D[2, 2] < 0 || + D[1, 1] + D[2, 2] + D[3, 3] < 0 || + sum(diag(D)) < 0 + # Something has gone wrong + # This could probably be detected somewhere else + # more efficiently. + return DOP(-1, -1, -1, -1, -1) + end TDOP = sqrt(D[4, 4]) # temporal dop VDOP = sqrt(D[3, 3]) # vertical dop HDOP = sqrt(D[1, 1] + D[2, 2]) # horizontal dop From 93d7a529e8253ab7e10e14834ff24f2eaa5e285b Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 21 Dec 2022 13:11:30 +0000 Subject: [PATCH 4/7] Add RAIM --- Project.toml | 7 ++-- src/PositionVelocityTime.jl | 65 +++++++++++++++++++++++++++++++++---- src/user_position.jl | 4 +-- 3 files changed, 64 insertions(+), 12 deletions(-) diff --git a/Project.toml b/Project.toml index 9a65cf5..a6addca 100755 --- a/Project.toml +++ b/Project.toml @@ -14,21 +14,22 @@ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" LsqFit = "2fda8390-95c7-5789-9bda-21331edee243" Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" +Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" Tracking = "10b2438b-ffd4-5096-aa58-44041d5c8f3b" Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" [compat] AstroTime = "0.7" BitIntegers = "0.2.6" -Tracking = "0.15" -LsqFit = "0.12, 0.13" CoordinateTransformations = "0.6" DocStringExtensions = "0.6, 0.7, 0.8, 0.9" GNSSDecoder = "0.1.0" GNSSSignals = "0.16" -StaticArrays = "1" Geodesy = "0.5, 1.0" +LsqFit = "0.12, 0.13" Parameters = "0.11, 0.12" +StaticArrays = "1" +Tracking = "0.15" Unitful = "1" julia = "1.6" diff --git a/src/PositionVelocityTime.jl b/src/PositionVelocityTime.jl index 2846449..d09d13d 100755 --- a/src/PositionVelocityTime.jl +++ b/src/PositionVelocityTime.jl @@ -10,7 +10,8 @@ using CoordinateTransformations, LsqFit, StaticArrays, Tracking, - Unitful + Unitful, + Statistics using Unitful: s, Hz @@ -151,11 +152,52 @@ function calc_pvt( ) sat_positions = map(get_sat_position, sat_positions_and_velocities) pseudo_ranges, reference_time = calc_pseudo_ranges(times) - ξ = user_position(sat_positions, pseudo_ranges, prev_ξ) - user_velocity_and_clock_drift = calc_user_velocity_and_clock_drift(sat_positions_and_velocities, ξ, states) + ξ, rmse = user_position(sat_positions, pseudo_ranges, prev_ξ) + if rmse > 1e-3 + # Try to exclude a satellite to get better RMSE + num_sats = length(sat_positions) + if num_sats > 4 + # Let's try to use the satellites from previous PVT +# available_prns_from_prev_pvt = map(prn -> prn in prev_pvt.used_sats, healthy_prns) +# filtered_sat_positions = sat_positions[available_prns_from_prev_pvt] +# filtered_pseudo_ranges = pseudo_ranges[available_prns_from_prev_pvt] +# temp_ξ, temp_rmse = user_position(filtered_sat_positions, filtered_pseudo_ranges, prev_ξ) +# if temp_rmse < 1e-3 +# ξ = temp_ξ +# rmse = temp_rmse +# healthy_states = healthy_states[available_prns_from_prev_pvt] +# sat_positions_and_velocities = sat_positions_and_velocities[available_prns_from_prev_pvt] +# healthy_prns = healthy_prns[available_prns_from_prev_pvt] +# else + for i = 1:num_sats + filtered_sat_positions = sat_positions[1:num_sats .!= i] + filtered_pseudo_ranges = pseudo_ranges[1:num_sats .!= i] + temp_ξ, temp_rmse = user_position(filtered_sat_positions, filtered_pseudo_ranges, prev_ξ) + if temp_rmse < 1e-3 + ξ = temp_ξ + rmse = temp_rmse + healthy_states = healthy_states[1:num_sats .!= i] + sat_positions_and_velocities = sat_positions_and_velocities[1:num_sats .!= i] + healthy_prns = healthy_prns[1:num_sats .!= i] + sat_positions = filtered_sat_positions + break + end + end +# end + end + if rmse > 1e-3 + return prev_pvt + end + end + user_velocity_and_clock_drift = + calc_user_velocity_and_clock_drift(sat_positions_and_velocities, ξ, healthy_states) position = ECEF(ξ[1], ξ[2], ξ[3]) - velocity = ECEF(user_velocity_and_clock_drift[1], user_velocity_and_clock_drift[2], user_velocity_and_clock_drift[3]) - relative_clock_drift = user_velocity_and_clock_drift[4] / SPEEDOFLIGHT + velocity = ECEF( + user_velocity_and_clock_drift[1], + user_velocity_and_clock_drift[2], + user_velocity_and_clock_drift[3], + ) + relative_clock_drift = user_velocity_and_clock_drift[4] / SPEEDOFLIGHT time_correction = ξ[4] corrected_reference_time = reference_time + time_correction / SPEEDOFLIGHT @@ -168,10 +210,19 @@ function calc_pvt( dop = calc_DOP(calc_H(reduce(hcat, sat_positions), ξ)) if dop.GDOP < 0 - return PVTSolution() + return prev_pvt end - PVTSolution(position, velocity, time_correction, time, relative_clock_drift, dop, healthy_prns, sat_positions) + PVTSolution( + position, + velocity, + time_correction, + time, + relative_clock_drift, + dop, + healthy_prns, + sat_positions, + ) end function get_frequency_offset(pvt::PVTSolution, base_frequency) diff --git a/src/user_position.jl b/src/user_position.jl index f32f7f1..61387f8 100755 --- a/src/user_position.jl +++ b/src/user_position.jl @@ -96,8 +96,8 @@ function user_position(sat_positions, ρ, prev_ξ = zeros(4)) ξ_fit_ols = curve_fit(calc_ρ_hat, calc_H, sat_positions_mat, ρ, collect(prev_ξ)) # wt = 1 ./ (ξ_fit_ols.resid .^ 2) # ξ_fit_wls = curve_fit(ρ_hat, H, sat_positions_mat, ρ, wt, collect(prev_ξ)) - - return ξ_fit_ols.param + rmse = sqrt(mean(ξ_fit_ols.resid .^ 2)) + return ξ_fit_ols.param, rmse end """ From e36b3a6b3074b7872d53f7756b39dd1532313bf5 Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 28 Dec 2022 13:42:10 +0000 Subject: [PATCH 5/7] Save dictionary of satellites and their positions in PVTSolution --- src/PositionVelocityTime.jl | 55 ++++++++++--------------------------- src/sat_time.jl | 2 +- 2 files changed, 15 insertions(+), 42 deletions(-) diff --git a/src/PositionVelocityTime.jl b/src/PositionVelocityTime.jl index d09d13d..b1635b3 100755 --- a/src/PositionVelocityTime.jl +++ b/src/PositionVelocityTime.jl @@ -67,6 +67,11 @@ struct DOP TDOP::Float64 end +struct SatInfo + position::ECEF + time::Float64 +end + """ PVT solution including DOP, used satellites and satellite positions. @@ -78,8 +83,7 @@ positions. time::Union{TAIEpoch{Float64},Nothing} = nothing relative_clock_drift::Float64 = 0 dop::Union{DOP,Nothing} = nothing - used_sats::Vector{Int64} = [] - sat_positions::Vector{ECEF} = [] + sats::Dict{Int, SatInfo} = Dict{Int, SatInfo}() end function get_num_used_sats(pvt_solution::PVTSolution) @@ -138,6 +142,7 @@ function calc_pvt( throw(ArgumentError("You'll need at least 4 satellites to calculate PVT")) all(state -> state.system == states[1].system, states) || ArgumentError("For now all satellites need to be base on the same GNSS") + system = first(states).system healthy_states = filter(x -> is_sat_healthy(x.decoder), states) if length(healthy_states) < 4 return prev_pvt @@ -152,43 +157,7 @@ function calc_pvt( ) sat_positions = map(get_sat_position, sat_positions_and_velocities) pseudo_ranges, reference_time = calc_pseudo_ranges(times) - ξ, rmse = user_position(sat_positions, pseudo_ranges, prev_ξ) - if rmse > 1e-3 - # Try to exclude a satellite to get better RMSE - num_sats = length(sat_positions) - if num_sats > 4 - # Let's try to use the satellites from previous PVT -# available_prns_from_prev_pvt = map(prn -> prn in prev_pvt.used_sats, healthy_prns) -# filtered_sat_positions = sat_positions[available_prns_from_prev_pvt] -# filtered_pseudo_ranges = pseudo_ranges[available_prns_from_prev_pvt] -# temp_ξ, temp_rmse = user_position(filtered_sat_positions, filtered_pseudo_ranges, prev_ξ) -# if temp_rmse < 1e-3 -# ξ = temp_ξ -# rmse = temp_rmse -# healthy_states = healthy_states[available_prns_from_prev_pvt] -# sat_positions_and_velocities = sat_positions_and_velocities[available_prns_from_prev_pvt] -# healthy_prns = healthy_prns[available_prns_from_prev_pvt] -# else - for i = 1:num_sats - filtered_sat_positions = sat_positions[1:num_sats .!= i] - filtered_pseudo_ranges = pseudo_ranges[1:num_sats .!= i] - temp_ξ, temp_rmse = user_position(filtered_sat_positions, filtered_pseudo_ranges, prev_ξ) - if temp_rmse < 1e-3 - ξ = temp_ξ - rmse = temp_rmse - healthy_states = healthy_states[1:num_sats .!= i] - sat_positions_and_velocities = sat_positions_and_velocities[1:num_sats .!= i] - healthy_prns = healthy_prns[1:num_sats .!= i] - sat_positions = filtered_sat_positions - break - end - end -# end - end - if rmse > 1e-3 - return prev_pvt - end - end + ξ, rmse = user_position(sat_positions, pseudo_ranges) user_velocity_and_clock_drift = calc_user_velocity_and_clock_drift(sat_positions_and_velocities, ξ, healthy_states) position = ECEF(ξ[1], ξ[2], ξ[3]) @@ -208,6 +177,11 @@ function calc_pvt( corrected_reference_time - floor(Int, corrected_reference_time), ) + sat_infos = SatInfo.( + sat_positions, + times + ) + dop = calc_DOP(calc_H(reduce(hcat, sat_positions), ξ)) if dop.GDOP < 0 return prev_pvt @@ -220,8 +194,7 @@ function calc_pvt( time, relative_clock_drift, dop, - healthy_prns, - sat_positions, + Dict(healthy_prns .=> sat_infos) ) end diff --git a/src/sat_time.jl b/src/sat_time.jl index de2cfd5..5590571 100644 --- a/src/sat_time.jl +++ b/src/sat_time.jl @@ -7,7 +7,7 @@ function calc_uncorrected_time(state::SatelliteState) system = state.system t_tow = state.decoder.data.TOW t_bits = - (state.decoder.num_bits_after_valid_syncro_sequence) / + state.decoder.num_bits_after_valid_syncro_sequence / GNSSSignals.get_data_frequency(system) * Hz t_code_phase = state.code_phase / GNSSSignals.get_code_frequency(system) * Hz t_carrier_phase = state.carrier_phase / GNSSSignals.get_center_frequency(system) * Hz From e5e8bc54f0365d5c26853d5401ec026044fdc2ff Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 28 Dec 2022 13:44:51 +0000 Subject: [PATCH 6/7] Bump version --- Project.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Project.toml b/Project.toml index a6addca..ce911ec 100755 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "PositionVelocityTime" uuid = "52ec0b5e-ee36-11ea-211c-3d844fc5682d" authors = ["Soeren Schoenbrod ", "Michael Niestroj ", "Erik Deinzer "] -version = "0.1.0" +version = "0.1.1" [deps] AstroTime = "c61b5328-d09d-5e37-a9a8-0eb41c39009c" @@ -23,7 +23,7 @@ AstroTime = "0.7" BitIntegers = "0.2.6" CoordinateTransformations = "0.6" DocStringExtensions = "0.6, 0.7, 0.8, 0.9" -GNSSDecoder = "0.1.0" +GNSSDecoder = "0.1.1" GNSSSignals = "0.16" Geodesy = "0.5, 1.0" LsqFit = "0.12, 0.13" From cd8dea7260d688a21eb3764da3242e717d75e05e Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Wed, 28 Dec 2022 13:46:51 +0000 Subject: [PATCH 7/7] Bump version to 0.2 - breaking change --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index ce911ec..6f28657 100755 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "PositionVelocityTime" uuid = "52ec0b5e-ee36-11ea-211c-3d844fc5682d" authors = ["Soeren Schoenbrod ", "Michael Niestroj ", "Erik Deinzer "] -version = "0.1.1" +version = "0.2.0" [deps] AstroTime = "c61b5328-d09d-5e37-a9a8-0eb41c39009c"