use spacelab_autodiff::Scalar;
use crate::modifier::Observable;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum Component {
X,
Y,
Z,
Vx,
Vy,
Vz,
}
impl Component {
pub const POSITION: [Self; 3] = [Self::X, Self::Y, Self::Z];
pub const VELOCITY: [Self; 3] = [Self::Vx, Self::Vy, Self::Vz];
pub const ALL: [Self; 6] = [Self::X, Self::Y, Self::Z, Self::Vx, Self::Vy, Self::Vz];
#[must_use]
pub const fn index(self) -> usize {
match self {
Self::X => 0,
Self::Y => 1,
Self::Z => 2,
Self::Vx => 3,
Self::Vy => 4,
Self::Vz => 5,
}
}
#[must_use]
pub const fn is_velocity(self) -> bool {
self.index() >= 3
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub struct StateComponent {
pub component: Component,
}
impl StateComponent {
#[must_use]
pub const fn new(component: Component) -> Self {
Self { component }
}
#[must_use]
pub const fn position() -> [Self; 3] {
[
Self::new(Component::X),
Self::new(Component::Y),
Self::new(Component::Z),
]
}
#[must_use]
pub const fn position_velocity() -> [Self; 6] {
[
Self::new(Component::X),
Self::new(Component::Y),
Self::new(Component::Z),
Self::new(Component::Vx),
Self::new(Component::Vy),
Self::new(Component::Vz),
]
}
}
impl<const K: usize> Observable<K> for StateComponent {
fn value<S: Scalar>(&self, satellite: &[S; 6], _parameters: &[S; K]) -> S {
satellite[self.component.index()]
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::expect_used, clippy::unwrap_used, clippy::needless_range_loop)]
use spacelab_testkit::assert_within_ulps;
use super::*;
use crate::modifier::{Bias, EstimatedBias, Modified, partials};
const STATE: [f64; 6] = [
3_961_000.0,
2_614_000.0,
5_215_000.0,
-3_140.0,
6_190.0,
-700.0,
];
#[test]
fn every_component_reads_the_state_slot_it_names() {
let none: [f64; 0] = [];
for (i, observable) in StateComponent::position_velocity().into_iter().enumerate() {
assert_eq!(observable.component.index(), i);
assert_within_ulps(observable.value(&STATE, &none), STATE[i], 0, "a component");
}
for (i, component) in Component::ALL.into_iter().enumerate() {
assert_eq!(component.index(), i);
assert_eq!(component.is_velocity(), i >= 3);
}
assert_eq!(
Component::POSITION,
[Component::X, Component::Y, Component::Z]
);
assert_eq!(
Component::VELOCITY,
[Component::Vx, Component::Vy, Component::Vz]
);
assert_eq!(
StateComponent::position(),
[
StateComponent::new(Component::X),
StateComponent::new(Component::Y),
StateComponent::new(Component::Z)
]
);
}
#[test]
fn the_partial_is_the_unit_row() {
let none: [f64; 0] = [];
for (i, observable) in StateComponent::position_velocity().into_iter().enumerate() {
let p = partials(&observable, &STATE, &none);
assert_within_ulps(p.value, STATE[i], 0, "the value");
for j in 0..6 {
let want = if i == j { 1.0 } else { 0.0 };
assert_within_ulps(p.state[j], want, 0, "a partial");
}
}
}
#[test]
fn a_component_takes_the_modifier_chain() {
let observable = Modified::new(StateComponent::new(Component::Y), Bias { value: -12.5 })
.with(EstimatedBias::<1>::new(0).expect("the only parameter"));
let p = partials(&observable, &STATE, &[4.25]);
assert_within_ulps(
p.value,
STATE[1] - 12.5 + 4.25,
0,
"the corrected component",
);
assert_within_ulps(p.parameters[0], 1.0, 0, "∂component/∂bias");
assert_within_ulps(p.state[1], 1.0, 0, "the unit partial survives the chain");
}
}