Skip to content

Commit

Permalink
Compute the Cholesky decomposition of the R_k matrix for the correct …
Browse files Browse the repository at this point in the history 
…tracker noises. Add sequential OD struct. Add error logs for incorrect filter config
  • Loading branch information
@ChristopherRabotin
ChristopherRabotin committed Dec 8, 2024
1 parent 0937fe7 commit 23d243e
Show file tree
Hide file tree
Showing 5 changed files with 247 additions and 173 deletions.
49 changes: 26 additions & 23 deletions src/od/filter/kalman.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,7 +276,7 @@ where
nominal_state: T,
real_obs: &OVector<f64, M>,
computed_obs: &OVector<f64, M>,
measurement_covar: OMatrix<f64, M, M>,
r_k: OMatrix<f64, M, M>,
resid_rejection: Option<ResidRejectCrit>,
) -> Result<(Self::Estimate, Residual<M>), ODError> {
if !self.h_tilde_updated {
Expand All @@ -291,42 +291,45 @@ where

let h_tilde_t = &self.h_tilde.transpose();
let h_p_ht = &self.h_tilde * covar_bar * h_tilde_t;
// Account for state uncertainty in the measurement noise. Equation 4.10 of ODTK MathSpec.
let r_k = &h_p_ht + measurement_covar;

let s_k = &h_p_ht + &r_k;

// Compute observation deviation (usually marked as y_i)
let prefit = real_obs - computed_obs;

// Compute the prefit ratio for the automatic rejection.
// The measurement covariance is the square of the measurement itself.
// So we compute its Cholesky decomposition to return to the non squared values.
let r_k_chol_inv = r_k
.clone()
.cholesky()
.ok_or(ODError::SingularNoiseRk)?
.l()
.try_inverse()
.ok_or(ODError::SingularNoiseRk)?;

// Then we multiply the inverted r_k matrix with prefit, giving us the vector of
// ratios of the prefit over the r_k matrix diagonal.
let ratio_vec = r_k_chol_inv * &prefit;
// Compute the ratio as the dot product.
let ratio = ratio_vec.dot(&ratio_vec);
let r_k_chol = s_k.clone().cholesky().ok_or(ODError::SingularNoiseRk)?.l();

// Compute the ratio as the average of each component of the prefit over the square root of the measurement
// matrix r_k. Refer to ODTK MathSpec equation 4.10.
let ratio = s_k
.diagonal()
.iter()
.copied()
.enumerate()
.map(|(idx, r)| prefit[idx] / r.sqrt())
.sum::<f64>()
/ (M::USIZE as f64);

if let Some(resid_reject) = resid_rejection {
if ratio > resid_reject.num_sigmas {
if ratio.abs() > resid_reject.num_sigmas {
// Reject this whole measurement and perform only a time update
let pred_est = self.time_update(nominal_state)?;
return Ok((
pred_est,
Residual::rejected(epoch, prefit, ratio, r_k.diagonal()),
Residual::rejected(epoch, prefit, ratio, r_k_chol.diagonal()),
));
}
}

// Compute the Kalman gain but first adding the measurement noise to H⋅P⋅H^T
let mut innovation_covar = h_p_ht + &r_k;
// Compute the innovation matrix (S_k) but using the previously computed s_k.
// This differs from the typical Kalman definition, but it allows constant inflating of the covariance.
// In turn, this allows the filter to not be overly optimistic. In verification tests, using the nominal
// Kalman formulation shows an error roughly 7 times larger with a smaller than expected covariance, despite
// no difference in the truth and sim.
let mut innovation_covar = h_p_ht + &s_k;
if !innovation_covar.try_inverse_mut() {
return Err(ODError::SingularKalmanGain);
}
Expand All @@ -339,23 +342,23 @@ where
let postfit = &prefit - (&self.h_tilde * state_hat);
(
state_hat,
Residual::accepted(epoch, prefit, postfit, ratio, r_k.diagonal()),
Residual::accepted(epoch, prefit, postfit, ratio, r_k_chol.diagonal()),
)
} else {
// Must do a time update first
let state_bar = stm * self.prev_estimate.state_deviation;
let postfit = &prefit - (&self.h_tilde * state_bar);
(
state_bar + &gain * &postfit,
Residual::accepted(epoch, prefit, postfit, ratio, r_k.diagonal()),
Residual::accepted(epoch, prefit, postfit, ratio, r_k_chol.diagonal()),
)
};

// Compute covariance (Joseph update)
let first_term = OMatrix::<f64, <T as State>::Size, <T as State>::Size>::identity()
- &gain * &self.h_tilde;
let covar =
first_term * covar_bar * first_term.transpose() + &gain * &r_k * &gain.transpose();
first_term * covar_bar * first_term.transpose() + &gain * &s_k * &gain.transpose();

// And wrap up
let estimate = KfEstimate {
Expand Down
95 changes: 11 additions & 84 deletions src/od/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,16 @@ pub type SpacecraftODProcess<'a> = self::process::ODProcess<
GroundStation,
>;

/// A helper type for spacecraft orbit determination sequentially processing measurements
pub type SpacecraftODProcessSeq<'a> = self::process::ODProcess<
'a,
crate::md::prelude::SpacecraftDynamics,
nalgebra::Const<1>,
nalgebra::Const<3>,
filter::kalman::KF<crate::Spacecraft, nalgebra::Const<3>, nalgebra::Const<1>>,
GroundStation,
>;

#[allow(unused_imports)]
pub mod prelude {
pub use super::estimate::*;
Expand All @@ -83,89 +93,6 @@ pub mod prelude {
pub use crate::time::{Duration, Epoch, TimeUnits, Unit};
}

// /// A trait defining a measurement that can be used in the orbit determination process.
// pub trait Measurement: Copy + TimeTagged {
// /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2).
// type MeasurementSize: DimName;

// /// Returns the fields for this kind of measurement.
// /// The metadata must include a `unit` field with the unit.
// fn fields() -> Vec<Field>;

// /// Initializes a new measurement from the provided data.
// fn from_observation(epoch: Epoch, obs: OVector<f64, Self::MeasurementSize>) -> Self
// where
// DefaultAllocator: Allocator<Self::MeasurementSize>;

// /// Returns the measurement/observation as a vector.
// fn observation(&self) -> OVector<f64, Self::MeasurementSize>
// where
// DefaultAllocator: Allocator<Self::MeasurementSize>;
// }

// /// The Estimate trait defines the interface that is the opposite of a `SolveFor`.
// /// For example, `impl EstimateFrom<Spacecraft> for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`.
// ///
// /// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and
// /// the ground station bias information. Then, the `impl EstimateFrom<SpacecraftAndBias> for OrbitAndBias` will be added, where `OrbitAndBias` is the
// /// new State that includes the orbit and the bias of one ground station.
// pub trait EstimateFrom<O: State, M: Measurement>
// where
// Self: State,
// DefaultAllocator: Allocator<<O as State>::Size>
// + Allocator<<O as State>::VecLength>
// + Allocator<<O as State>::Size, <O as State>::Size>
// + Allocator<Self::Size>
// + Allocator<Self::VecLength>
// + Allocator<Self::Size, Self::Size>,
// {
// /// From the state extract the state to be estimated
// fn extract(from: O) -> Self;

// /// Returns the measurement sensitivity (often referred to as H tilde).
// ///
// /// # Limitations
// /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero.
// /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use.
// /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator.
// fn sensitivity(
// msr: &M,
// receiver: Self,
// transmitter: Orbit,
// ) -> OMatrix<f64, M::MeasurementSize, Self::Size>
// where
// DefaultAllocator: Allocator<M::MeasurementSize, Self::Size>;
// }

// /// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state.
// /// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame.
// impl<O> EstimateFrom<O, O> for O
// where
// O: State + Measurement,
// Self: State,
// DefaultAllocator: Allocator<<O as State>::Size>
// + Allocator<<O as State>::VecLength>
// + Allocator<<O as State>::Size, <O as State>::Size>
// + Allocator<Self::Size>
// + Allocator<Self::VecLength>
// + Allocator<Self::Size, Self::Size>,
// {
// fn extract(from: O) -> Self {
// from
// }

// fn sensitivity(
// _full_state_msr: &O,
// _receiver: Self,
// _transmitter: Orbit,
// ) -> OMatrix<f64, <O as Measurement>::MeasurementSize, Self::Size>
// where
// DefaultAllocator: Allocator<<O as Measurement>::MeasurementSize, Self::Size>,
// {
// OMatrix::<f64, O::MeasurementSize, Self::Size>::identity()
// }
// }

#[derive(Debug, PartialEq, Snafu)]
#[snafu(visibility(pub(crate)))]
pub enum ODError {
Expand All @@ -187,7 +114,7 @@ pub enum ODError {
SensitivityNotUpdated,
#[snafu(display("Kalman gain is singular"))]
SingularKalmanGain,
#[snafu(display("Noise matrix is singular"))]
#[snafu(display("noise matrix is singular"))]
SingularNoiseRk,
#[snafu(display("{kind} noise not configured"))]
NoiseNotConfigured { kind: String },
Expand Down
12 changes: 12 additions & 0 deletions src/od/process/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -424,6 +424,18 @@ where
StepSizeSnafu { step: max_step }
);

// Check proper configuration.
if MsrSize::USIZE > arc.unique_types().len() {
error!("Filter misconfigured: expect high rejection count!");
error!(
"Arc only contains {} measurement types, but filter configured for {}.",
arc.unique_types().len(),
MsrSize::USIZE
);
error!("Filter should be configured for these numbers to match.");
error!("Consider running subsequent arcs if ground stations provide different measurements.")
}

// Start by propagating the estimator.
let num_msrs = measurements.len();

Expand Down
6 changes: 3 additions & 3 deletions src/od/process/rejectcrit.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,10 @@ impl ResidRejectCrit {
}

impl Default for ResidRejectCrit {
/// By default, a measurement is rejected if its prefit residual is greater the 4-sigma value of the measurement noise at that time step.
/// This corresponds to [1 chance in in 15,787](https://en.wikipedia.org/wiki/68%E2%80%9395%E2%80%9399.7_rule).
/// By default, a measurement is rejected if its prefit residual is greater the 3-sigma value of the measurement noise at that time step.
/// This corresponds to [1 chance in in 370](https://en.wikipedia.org/wiki/68%E2%80%9395%E2%80%9399.7_rule).
fn default() -> Self {
Self { num_sigmas: 4.0 }
Self { num_sigmas: 3.0 }
}
}

Expand Down
Loading

0 comments on commit 23d243e

Please sign in to comment.