nyx_space/md/opti/multipleshooting/
altitude_heuristic.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use super::ctrlnodes::Node;
20use super::multishoot::MultipleShooting;
21pub use super::CostFunction;
22use super::{
23    MultiShootAlmanacSnafu, MultiShootPhysicsSnafu, MultiShootTrajSnafu, MultipleShootingError,
24    TargetingSnafu,
25};
26use crate::errors::TargetingError;
27use crate::md::{prelude::*, PropSnafu};
28use crate::{Orbit, Spacecraft};
29use log::error;
30use snafu::ResultExt;
31
32impl<'a> MultipleShooting<'a, Node, 3, 3> {
33    /// Builds a multiple shooting structure assuming that the optimal trajectory is near a linear
34    /// heuristic in geodetic altitude and direction.
35    /// For example, if x0 has an altitude of 100 km and xf has an altitude
36    /// of 200 km, and 10 nodes are required over 10 minutes, then node 1 will be 110 km, node 2 220km, etc.
37    /// body_frame must be a body fixed frame
38    pub fn linear_altitude_heuristic(
39        x0: Spacecraft,
40        xf: Orbit,
41        node_count: usize,
42        body_frame: Frame,
43        prop: &'a Propagator<SpacecraftDynamics>,
44        almanac: Arc<Almanac>,
45    ) -> Result<Self, MultipleShootingError> {
46        if node_count < 3 {
47            error!("At least three nodes are needed for a multiple shooting optimization");
48            return Err(MultipleShootingError::TargetingError {
49                segment: 0_usize,
50                source: Box::new(TargetingError::UnderdeterminedProblem),
51            });
52        }
53
54        let delta_t = xf.epoch - x0.epoch();
55        let xf_bf = almanac
56            .transform_to(xf, body_frame, None)
57            .context(MultiShootAlmanacSnafu {
58                action: "converting node into the body frame",
59            })?;
60
61        let duration_increment = (xf.epoch - x0.epoch()) / (node_count as f64);
62
63        let (_, traj) = prop
64            .with(x0, almanac.clone())
65            .for_duration_with_traj(delta_t)
66            .context(PropSnafu)
67            .context(TargetingSnafu { segment: 0_usize })?;
68
69        // Build each node successively (includes xf)
70        let mut nodes = Vec::with_capacity(node_count + 1);
71        let mut prev_node_epoch = x0.epoch();
72
73        let inertial_frame = x0.orbit.frame;
74        for i in 0..node_count {
75            // Compute the position we want.
76            let this_epoch = prev_node_epoch + duration_increment;
77            let orbit_point = traj.at(this_epoch).context(MultiShootTrajSnafu)?.orbit;
78            // Convert this orbit into the body frame
79            let orbit_point_bf = almanac
80                .clone()
81                .transform_to(orbit_point, body_frame, None)
82                .context(MultiShootAlmanacSnafu {
83                    action: "converting node into the body frame",
84                })?;
85
86            // Note that the altitude here might be different, so we scale the altitude change by the current altitude
87            let desired_alt_i = (xf_bf.height_km().context(MultiShootPhysicsSnafu)?
88                - orbit_point_bf.height_km().context(MultiShootPhysicsSnafu)?)
89                / ((node_count - i) as f64).sqrt();
90            // Build the node in the body frame and convert that to the original frame
91            let node_bf = Orbit::try_latlongalt(
92                orbit_point_bf
93                    .latitude_deg()
94                    .context(MultiShootPhysicsSnafu)?,
95                orbit_point_bf.longitude_deg(),
96                orbit_point_bf.height_km().context(MultiShootPhysicsSnafu)? + desired_alt_i,
97                this_epoch,
98                body_frame,
99            )
100            .context(MultiShootPhysicsSnafu)?;
101
102            // Convert that back into the inertial frame
103            let this_node = almanac
104                .transform_to(node_bf, inertial_frame, None)
105                .context(MultiShootAlmanacSnafu {
106                    action: "converting node back into the inertial frame",
107                })?
108                .radius_km;
109
110            nodes.push(Node {
111                x: this_node[0],
112                y: this_node[1],
113                z: this_node[2],
114                vmag: 0.0,
115                frame: inertial_frame,
116                epoch: this_epoch,
117            });
118            prev_node_epoch = this_epoch;
119        }
120        Ok(Self {
121            prop,
122            targets: nodes,
123            x0,
124            xf,
125            current_iteration: 0,
126            max_iterations: 100,
127            improvement_threshold: 0.01,
128            variables: [
129                Vary::VelocityX.into(),
130                Vary::VelocityY.into(),
131                Vary::VelocityZ.into(),
132            ],
133            all_dvs: Vec::with_capacity(node_count),
134        })
135    }
136}
nyx_space/md/opti/multipleshooting/altitude_heuristic.rs

nyx_space/md/opti/multipleshooting/
altitude_heuristic.rs
