Merge pull request JuliaGNSS#7 from JuliaGNSS/ss/calc-velocity-and-cl…

…ock-drift

Calc velocity and clock drift

Project.toml

@@ -1,7 +1,7 @@
 name = "PositionVelocityTime"
 uuid = "52ec0b5e-ee36-11ea-211c-3d844fc5682d"
 authors = ["Soeren Schoenbrod <[email protected]>", "Michael Niestroj <[email protected]>", "Erik Deinzer <[email protected]>"]
-version = "0.1.0"
+version = "0.2.0"
 
 [deps]
 AstroTime = "c61b5328-d09d-5e37-a9a8-0eb41c39009c"
@@ -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"
+GNSSDecoder = "0.1.1"
 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"
 

src/PositionVelocityTime.jl

@@ -9,7 +9,9 @@ using CoordinateTransformations,
     AstroTime,
     LsqFit,
     StaticArrays,
-    Tracking
+    Tracking,
+    Unitful,
+    Statistics
 
 using Unitful: s, Hz
 
@@ -21,12 +23,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 +39,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 +51,7 @@ function SatelliteState(
         decoder,
         get_system(tracking_results),
         get_code_phase(tracking_results),
+        get_carrier_doppler(tracking_results),
         get_carrier_phase(tracking_results),
     )
 end
@@ -61,17 +67,23 @@ struct DOP
     TDOP::Float64
 end
 
+struct SatInfo
+    position::ECEF
+    time::Float64
+end
+
 """
 PVT solution including DOP, used satellites and satellite
 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} = []
+    sats::Dict{Int, SatInfo} = Dict{Int, SatInfo}()
 end
 
 function get_num_used_sats(pvt_solution::PVTSolution)
@@ -130,21 +142,31 @@ 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
     end
     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_ξ)
+    ξ, 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])
+    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 +177,29 @@ function calc_pvt(
         corrected_reference_time - floor(Int, corrected_reference_time),
     )
 
-    dop = calc_DOP(H(reduce(hcat, sat_positions), ξ))
+    sat_infos = SatInfo.(
+        sat_positions,
+        times
+    )
+
+    dop = calc_DOP(calc_H(reduce(hcat, sat_positions), ξ))
+    if dop.GDOP < 0
+        return prev_pvt
+    end
+
+    PVTSolution(
+        position,
+        velocity,
+        time_correction,
+        time,
+        relative_clock_drift,
+        dop,
+        Dict(healthy_prns .=> sat_infos)
+    )
+end
 
-    PVTSolution(position, time_correction, time, dop, healthy_prns, sat_positions)
+function get_frequency_offset(pvt::PVTSolution, base_frequency)
+    pvt.relative_clock_drift * base_frequency
 end
 
 function get_system_start_time(

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
 
 """

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
@@ -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,