gnss_rtk/
solver.rs

1use log::{debug, error, info};
2
3use anise::{
4    math::Vector3,
5    prelude::{Almanac, Frame},
6};
7
8use crate::{
9    bancroft::Bancroft,
10    candidate::Candidate,
11    cfg::Config,
12    ephemeris::EphemerisSource,
13    navigation::{apriori::Apriori, solutions::PVTSolution, state::State, Navigation},
14    orbit::OrbitSource,
15    pool::Pool,
16    prelude::{EnvironmentalBias, Epoch, Error, Rc, SpacebornBias, UserParameters},
17    rtk::{NullRTK, RTKBase},
18    time::AbsoluteTime,
19};
20
21/// [Solver] to resolve [PVTSolution]s.
22///
23/// ## Generics:
24/// - EPH: [EphemerisSource] custom data source. Curreuntly unused: limited to [ORB] only!
25/// - ORB: [OrbitSource], custom Orbit data source.
26/// - EB: [EnvironmentalBias] implementation using either intenral or custom external model.
27/// - SB: [SpacebornBias] external information provider.
28/// - TIM: [AbsoluteTime] source for correct absolute time management.
29pub struct Solver<
30    EPH: EphemerisSource,
31    ORB: OrbitSource,
32    EB: EnvironmentalBias,
33    SB: SpacebornBias,
34    TIM: AbsoluteTime,
35> {
36    /// Solver [Config]uration
37    pub cfg: Config,
38
39    /// [Frame]
40    earth_cef: Frame,
41
42    /// Rover pool
43    rover_pool: Pool<EPH, ORB, EB, SB>,
44
45    /// Base pool
46    base_pool: Pool<EPH, ORB, EB, SB>,
47
48    /// PPP prefit
49
50    /// [Navigation] solver
51    navigation: Navigation,
52
53    /// [AbsoluteTime] implementation
54    absolute_time: TIM,
55
56    /// Possible initial position
57    initial_ecef_m: Option<Vector3>,
58}
59
60impl<
61        EPH: EphemerisSource,
62        ORB: OrbitSource,
63        EB: EnvironmentalBias,
64        SB: SpacebornBias,
65        TIM: AbsoluteTime,
66    > Solver<EPH, ORB, EB, SB, TIM>
67{
68    /// Creates a new [Solver] for either direct or differential navigation,
69    /// with possible apriori knowledge.
70    ///
71    /// ## Input
72    /// - almanac: provided valid [Almanac]
73    /// - earth_cef: [Frame] that must be an ECEF
74    /// - cfg: solver [Config]uration
75    /// - eph_source: custom [EphemerisSource], unavailable right now,
76    /// tie to null.
77    /// - orbit_source: custom [OrbitSource] implementation,
78    /// wrapped in a Rc<RefCell<>> which allows the solver
79    /// and the orbital provider to live in the same thread.
80    /// - absolute_time: external [AbsoluteTime] implementation.
81    /// - bias: [Bias] model implementation
82    /// - state_ecef_m: provide initial state as ECEF 3D coordinates,
83    /// otherwise we will have to figure them.
84    pub fn new(
85        almanac: Almanac,
86        earth_cef: Frame,
87        cfg: Config,
88        eph_source: Rc<EPH>,
89        orbit_source: Rc<ORB>,
90        spaceborn_biases: Rc<SB>,
91        environmental_biases: Rc<EB>,
92        absolute_time: TIM,
93        state_ecef_m: Option<(f64, f64, f64)>,
94    ) -> Self {
95        let initial_ecef_m = match state_ecef_m {
96            Some((x0, y0, z0)) => Some(Vector3::new(x0, y0, z0)),
97            _ => None,
98        };
99
100        let navigation = Navigation::new(&cfg, earth_cef);
101
102        let rover_pool = Pool::allocate(
103            almanac.clone(),
104            cfg.clone(),
105            earth_cef,
106            eph_source.clone(),
107            orbit_source.clone(),
108            environmental_biases.clone(),
109            spaceborn_biases.clone(),
110        );
111
112        let base_pool = Pool::allocate(
113            almanac,
114            cfg.clone(),
115            earth_cef,
116            eph_source,
117            orbit_source,
118            environmental_biases,
119            spaceborn_biases,
120        );
121
122        Self {
123            earth_cef,
124            navigation,
125            rover_pool,
126            base_pool,
127            absolute_time,
128            initial_ecef_m,
129            cfg: cfg.clone(),
130        }
131    }
132
133    /// Creates a new [Solver] to operate without a priori knowledge
134    /// and perform a complete survey from scratch. Refer to [Self::new]
135    /// for more information.
136    pub fn new_survey(
137        almanac: Almanac,
138        earth_cef: Frame,
139        cfg: Config,
140        eph_source: Rc<EPH>,
141        orbit_source: Rc<ORB>,
142        spaceborn_biases: Rc<SB>,
143        environmental_biases: Rc<EB>,
144        absolute_time: TIM,
145    ) -> Self {
146        Self::new(
147            almanac,
148            earth_cef,
149            cfg,
150            eph_source,
151            orbit_source,
152            spaceborn_biases,
153            environmental_biases,
154            absolute_time,
155            None,
156        )
157    }
158
159    /// [PVTSolution] solving attempt using PPP technique (no reference).
160    /// Unlike RTK resolution attempt, PPP solving will resolve both
161    /// the position and the local clock state, but it is
162    /// a less accurate and much more complex process.
163    ///
164    /// ## Input
165    /// - epoch: [Epoch] of measurement
166    /// - params: [UserParameters]
167    /// - candidates: proposed [Candidate]s (= measurements)
168    /// - rtk_base: possible [RTKBase] we will connect to
169    ///
170    /// ## Output
171    /// - success: [PVTSolution]
172    /// - failure: [Error]
173    pub fn ppp(
174        &mut self,
175        epoch: Epoch,
176        params: UserParameters,
177        candidates: &[Candidate],
178    ) -> Result<PVTSolution, Error> {
179        let null_rtk = NullRTK {};
180        let solution = self.solve(epoch, params, candidates, &null_rtk, false)?;
181        Ok(solution)
182    }
183
184    /// [PVTSolution] solving attempt using RTK technique and a single remote
185    /// reference site that implements [RTKBase].
186    /// You may catch RTK solving issues (for example, network loss)
187    /// and retry using [Self::ppp], because this object is compatible
188    /// with both. For this function to converge, the remote
189    /// site and rover proposals must agree in their content (enough matches)
190    /// and signals must fulfill the navigation [Method] being used.
191    ///
192    /// NB: unlike PPP solving, RTK solving will only resolve the spatial
193    /// state, the clock state can only remain undetermined. If you are
194    /// interested by both, you will either have to restrict to [Self::ppp_run],
195    /// or do a PPP (time only) run after the RTK run.
196    ///
197    /// ## Input
198    /// - epoch: [Epoch] of measurement from the rover reported by the rover.
199    /// The remote site must match the sampling instant closely, reducing
200    /// the time-difference (RTK aging).
201    /// - profile: rover [UserProfile]
202    /// - candidates: rover measurements, wrapped as [Candidate]s.
203    /// - rtk_base: remote reference site that implements [RTKBase]reference.
204    ///
205    /// ## Output
206    /// - [PVTSolution].
207    pub fn rtk<RTK: RTKBase>(
208        &mut self,
209        epoch: Epoch,
210        params: UserParameters,
211        candidates: &[Candidate],
212        rtk_base: &RTK,
213    ) -> Result<PVTSolution, Error> {
214        self.solve(epoch, params, candidates, rtk_base, true)
215    }
216
217    /// [PVTSolution] solving attempt.
218    fn solve<RTK: RTKBase>(
219        &mut self,
220        epoch: Epoch,
221        params: UserParameters,
222        pool: &[Candidate],
223        rtk_base: &RTK,
224        uses_rtk: bool,
225    ) -> Result<PVTSolution, Error> {
226        let rtk_base_name = rtk_base.name();
227        let min_required = self.min_sv_required(uses_rtk);
228
229        if pool.len() < min_required {
230            // no need to proceed further
231            return Err(Error::NotEnoughCandidates);
232        }
233
234        self.rover_pool.new_epoch(pool);
235
236        if uses_rtk {
237            let observations = rtk_base.observe(epoch);
238            info!("{epoch} - using remote {rtk_base_name} reference");
239            self.base_pool.new_epoch(&observations);
240        }
241
242        self.rover_pool.pre_fit("rover", &self.absolute_time);
243
244        if uses_rtk {
245            self.base_pool.pre_fit(&rtk_base_name, &self.absolute_time);
246        }
247
248        if self.rover_pool.len() < min_required {
249            return Err(Error::NotEnoughPreFitCandidates);
250        }
251
252        let epoch = if epoch.time_scale == self.cfg.timescale {
253            epoch
254        } else {
255            let corrected = self
256                .absolute_time
257                .epoch_correction(epoch, self.cfg.timescale);
258
259            debug!(
260                "{} - |{}-{}| corrected sampling: {}",
261                epoch, epoch.time_scale, self.cfg.timescale, corrected
262            );
263
264            corrected
265        };
266
267        self.rover_pool.orbital_states_fit("rover");
268
269        if uses_rtk {
270            self.base_pool.orbital_states_fit(&rtk_base_name);
271        }
272
273        // current state
274        let state = if self.navigation.is_initialized() {
275            self.navigation.state.with_epoch(epoch)
276        } else {
277            match self.initial_ecef_m {
278                Some(x0_y0_z0_m) => {
279                    let apriori = Apriori::from_ecef_m(x0_y0_z0_m, epoch, self.earth_cef);
280
281                    let state = State::from_apriori(&apriori).unwrap_or_else(|e| {
282                        panic!("Solver initial preset is physically incorrect: {e}");
283                    });
284
285                    debug!("{epoch} - initial state: {state}");
286                    state
287                },
288                None => {
289                    let solver = Bancroft::new(self.rover_pool.candidates())?;
290                    let solution = solver.resolve()?;
291                    let x0_y0_z0_m = Vector3::new(solution[0], solution[1], solution[2]);
292
293                    let apriori = Apriori::from_ecef_m(x0_y0_z0_m, epoch, self.earth_cef);
294
295                    let state = State::from_apriori(&apriori).unwrap_or_else(|e| {
296                        panic!("Solver failed to initialize itself. Physical error: {e}");
297                    });
298
299                    debug!("{epoch} - initial state: {state}");
300                    state
301                },
302            }
303        };
304
305        // rover post-fit
306        self.rover_pool.post_fit("rover", &state).map_err(|e| {
307            error!("{epoch} rover postfit error {e}");
308            Error::PostfitPrenav
309        })?;
310
311        let double_differences = if uses_rtk {
312            // base post-fit
313            self.base_pool
314                .post_fit(&rtk_base_name, &state)
315                .map_err(|e| {
316                    error!("{epoch} {rtk_base_name} postfit error: {e}");
317                    Error::PostfitPrenav
318                })?;
319
320            // special RTK post-fit
321            match self.rover_pool.rtk_post_fit(&mut self.base_pool) {
322                Ok(double_differences) => Some(double_differences),
323                Err(e) => {
324                    error!("{epoch} - rtk post-fit error: {e}");
325                    return Err(e);
326                },
327            }
328        } else {
329            None
330        };
331
332        let pool_size = self.rover_pool.len();
333
334        if pool_size < min_required {
335            return Err(Error::NotEnoughPostFitCandidates);
336        }
337
338        // Solving attempt
339        match self.navigation.solve(
340            epoch,
341            params,
342            &state,
343            self.rover_pool.candidates(),
344            pool_size,
345            uses_rtk,
346            rtk_base,
347            &self.rover_pool.pivot_position_ecef_m,
348            &double_differences,
349        ) {
350            Ok(_) => {
351                info!("{epoch} - sucess");
352            },
353            Err(e) => {
354                error!("{epoch} - iteration failure: {e}");
355                return Err(e);
356            },
357        }
358
359        // Form P.V.T solution
360        let solution = PVTSolution::new(
361            epoch,
362            uses_rtk,
363            &self.navigation.state,
364            &self.navigation.dop,
365            &self.navigation.sv,
366        );
367
368        // Special "open loop" option
369        if self.cfg.solver.open_loop {
370            self.navigation.state = state;
371        }
372
373        Ok(solution)
374    }
375
376    /// Reset this [Solver].
377    pub fn reset(&mut self) {
378        self.navigation.reset();
379    }
380
381    /// Returns minimal requirement for current preset
382    fn min_sv_required(&self, uses_rtk: bool) -> usize {
383        let mut min_sv = 4;
384
385        if self.navigation.is_initialized() && self.cfg.fixed_altitude.is_some() {
386            min_sv -= 1;
387        }
388
389        if uses_rtk {
390            min_sv += 1;
391        }
392
393        min_sv
394    }
395}
gnss_rtk/solver.rs

gnss_rtk/
solver.rs
