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 snafu::ResultExt;
20
21use super::ctrlnodes::Node;
22use super::multishoot::MultipleShooting;
23pub use super::CostFunction;
24use super::{
25    MultiShootAlmanacSnafu, MultiShootPhysicsSnafu, MultiShootTrajSnafu, MultipleShootingError,
26    TargetingSnafu,
27};
28use crate::errors::TargetingError;
29use crate::md::{prelude::*, PropSnafu};
30use crate::{Orbit, Spacecraft};
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        angular_velocity_deg_s: f64,
43        body_frame: Frame,
44        prop: &'a Propagator<SpacecraftDynamics>,
45        almanac: Arc<Almanac>,
46    ) -> Result<Self, MultipleShootingError> {
47        if node_count < 3 {
48            error!("At least three nodes are needed for a multiple shooting optimization");
49            return Err(MultipleShootingError::TargetingError {
50                segment: 0_usize,
51                source: TargetingError::UnderdeterminedProblem,
52            });
53        }
54
55        let delta_t = xf.epoch - x0.epoch();
56        let xf_bf = almanac
57            .transform_to(xf, body_frame, None)
58            .context(MultiShootAlmanacSnafu {
59                action: "converting node into the body frame",
60            })?;
61
62        let duration_increment = (xf.epoch - x0.epoch()) / (node_count as f64);
63
64        let (_, traj) = prop
65            .with(x0, almanac.clone())
66            .for_duration_with_traj(delta_t)
67            .context(PropSnafu)
68            .context(TargetingSnafu { segment: 0_usize })?;
69
70        // Build each node successively (includes xf)
71        let mut nodes = Vec::with_capacity(node_count + 1);
72        let mut prev_node_epoch = x0.epoch();
73
74        let inertial_frame = x0.orbit.frame;
75        for i in 0..node_count {
76            // Compute the position we want.
77            let this_epoch = prev_node_epoch + duration_increment;
78            let orbit_point = traj.at(this_epoch).context(MultiShootTrajSnafu)?.orbit;
79            // Convert this orbit into the body frame
80            let orbit_point_bf = almanac
81                .clone()
82                .transform_to(orbit_point, body_frame, None)
83                .context(MultiShootAlmanacSnafu {
84                    action: "converting node into the body frame",
85                })?;
86
87            // Note that the altitude here might be different, so we scale the altitude change by the current altitude
88            let desired_alt_i = (xf_bf.height_km().context(MultiShootPhysicsSnafu)?
89                - orbit_point_bf.height_km().context(MultiShootPhysicsSnafu)?)
90                / ((node_count - i) as f64).sqrt();
91            // Build the node in the body frame and convert that to the original frame
92            let node_bf = Orbit::try_latlongalt(
93                orbit_point_bf
94                    .latitude_deg()
95                    .context(MultiShootPhysicsSnafu)?,
96                orbit_point_bf.longitude_deg(),
97                orbit_point_bf.height_km().context(MultiShootPhysicsSnafu)? + desired_alt_i,
98                angular_velocity_deg_s,
99                this_epoch,
100                body_frame,
101            )
102            .context(MultiShootPhysicsSnafu)?;
103
104            // Convert that back into the inertial frame
105            let this_node = almanac
106                .transform_to(node_bf, inertial_frame, None)
107                .context(MultiShootAlmanacSnafu {
108                    action: "converting node back into the inertial frame",
109                })?
110                .radius_km;
111
112            nodes.push(Node {
113                x: this_node[0],
114                y: this_node[1],
115                z: this_node[2],
116                vmag: 0.0,
117                frame: inertial_frame,
118                epoch: this_epoch,
119            });
120            prev_node_epoch = this_epoch;
121        }
122        Ok(Self {
123            prop,
124            targets: nodes,
125            x0,
126            xf,
127            current_iteration: 0,
128            max_iterations: 100,
129            improvement_threshold: 0.01,
130            variables: [
131                Vary::VelocityX.into(),
132                Vary::VelocityY.into(),
133                Vary::VelocityZ.into(),
134            ],
135            all_dvs: Vec::with_capacity(node_count),
136        })
137    }
138}
nyx_space/md/opti/multipleshooting/altitude_heuristic.rs

nyx_space/md/opti/multipleshooting/
altitude_heuristic.rs
