use crate::dimensions::allocator::Allocator;
use crate::dimensions::{DefaultAllocator, DimName, MatrixMN, VectorN, U3};
pub use super::estimate::{Estimate, KfEstimate};
pub use super::residual::Residual;
use super::{CovarFormat, EpochFormat, EstimableState, Filter, FilterError};
#[derive(Debug, Clone)]
pub struct KF<S, A, M, T>
where
S: DimName,
A: DimName,
M: DimName,
T: EstimableState<S>,
DefaultAllocator: Allocator<f64, M>
+ Allocator<f64, S>
+ Allocator<f64, M, M>
+ Allocator<f64, M, S>
+ Allocator<f64, S, S>
+ Allocator<f64, A, A>
+ Allocator<f64, S, A>
+ Allocator<f64, A, S>,
{
pub prev_estimate: KfEstimate<S, T>,
pub measurement_noise: MatrixMN<f64, M, M>,
pub process_noise: Option<MatrixMN<f64, A, A>>,
pub process_noise_dt: Option<f64>,
pub ekf: bool,
h_tilde: MatrixMN<f64, M, S>,
stm: MatrixMN<f64, S, S>,
stm_updated: bool,
h_tilde_updated: bool,
epoch_fmt: EpochFormat,
covar_fmt: CovarFormat,
}
impl<S, A, M, T> KF<S, A, M, T>
where
S: DimName,
A: DimName,
M: DimName,
T: EstimableState<S>,
DefaultAllocator: Allocator<f64, M>
+ Allocator<f64, S>
+ Allocator<f64, M, M>
+ Allocator<f64, M, S>
+ Allocator<f64, S, M>
+ Allocator<f64, S, S>
+ Allocator<f64, A, A>
+ Allocator<f64, S, A>
+ Allocator<f64, A, S>,
{
pub fn new(
initial_estimate: KfEstimate<S, T>,
process_noise: MatrixMN<f64, A, A>,
measurement_noise: MatrixMN<f64, M, M>,
process_noise_dt: Option<f64>,
) -> Self {
let epoch_fmt = initial_estimate.epoch_fmt;
let covar_fmt = initial_estimate.covar_fmt;
Self {
prev_estimate: initial_estimate,
measurement_noise,
process_noise: Some(process_noise),
process_noise_dt,
ekf: false,
h_tilde: MatrixMN::<f64, M, S>::zeros(),
stm: MatrixMN::<f64, S, S>::identity(),
stm_updated: false,
h_tilde_updated: false,
epoch_fmt,
covar_fmt,
}
}
}
impl<S, M, T> KF<S, U3, M, T>
where
S: DimName,
M: DimName,
T: EstimableState<S>,
DefaultAllocator: Allocator<f64, M>
+ Allocator<f64, S>
+ Allocator<f64, M, M>
+ Allocator<f64, M, S>
+ Allocator<f64, S, M>
+ Allocator<f64, S, S>
+ Allocator<f64, U3, U3>
+ Allocator<f64, S, U3>
+ Allocator<f64, U3, S>,
{
pub fn no_snc(
initial_estimate: KfEstimate<S, T>,
measurement_noise: MatrixMN<f64, M, M>,
) -> Self {
let epoch_fmt = initial_estimate.epoch_fmt;
let covar_fmt = initial_estimate.covar_fmt;
Self {
prev_estimate: initial_estimate,
measurement_noise,
process_noise: None,
process_noise_dt: None,
ekf: false,
h_tilde: MatrixMN::<f64, M, S>::zeros(),
stm: MatrixMN::<f64, S, S>::identity(),
stm_updated: false,
h_tilde_updated: false,
epoch_fmt,
covar_fmt,
}
}
}
impl<S, A, M, T> Filter<S, A, M, T> for KF<S, A, M, T>
where
S: DimName,
A: DimName,
M: DimName,
T: EstimableState<S>,
DefaultAllocator: Allocator<f64, M>
+ Allocator<f64, S>
+ Allocator<f64, M, M>
+ Allocator<f64, M, S>
+ Allocator<f64, S, M>
+ Allocator<f64, S, S>
+ Allocator<f64, A, A>
+ Allocator<f64, S, A>
+ Allocator<f64, A, S>,
{
type Estimate = KfEstimate<S, T>;
fn previous_estimate(&self) -> &Self::Estimate {
&self.prev_estimate
}
fn set_previous_estimate(&mut self, est: &Self::Estimate) {
self.prev_estimate = est.clone();
}
fn update_stm(&mut self, new_stm: MatrixMN<f64, S, S>) {
self.stm = new_stm;
self.stm_updated = true;
}
fn update_h_tilde(&mut self, h_tilde: MatrixMN<f64, M, S>) {
self.h_tilde = h_tilde;
self.h_tilde_updated = true;
}
fn time_update(&mut self, nominal_state: T) -> Result<Self::Estimate, FilterError> {
if !self.stm_updated {
return Err(FilterError::StateTransitionMatrixNotUpdated);
}
let covar_bar = &self.stm * &self.prev_estimate.covar * &self.stm.transpose();
let state_bar = if self.ekf {
VectorN::<f64, S>::zeros()
} else {
&self.stm * &self.prev_estimate.state_deviation
};
let estimate = KfEstimate {
nominal_state,
state_deviation: state_bar,
covar: covar_bar,
stm: self.stm.clone(),
predicted: true,
epoch_fmt: self.epoch_fmt,
covar_fmt: self.covar_fmt,
};
self.stm_updated = false;
self.prev_estimate = estimate.clone();
Ok(estimate)
}
fn measurement_update(
&mut self,
nominal_state: T,
real_obs: VectorN<f64, M>,
computed_obs: VectorN<f64, M>,
) -> Result<(Self::Estimate, Residual<M>), FilterError> {
if !self.stm_updated {
return Err(FilterError::StateTransitionMatrixNotUpdated);
}
if !self.h_tilde_updated {
return Err(FilterError::SensitivityNotUpdated);
}
let mut covar_bar = &self.stm * &self.prev_estimate.covar * &self.stm.transpose();
if let Some(pcr_dt) = self.process_noise_dt {
let delta_t = nominal_state.epoch() - self.prev_estimate.epoch();
if delta_t <= pcr_dt {
let mut gamma = MatrixMN::<f64, S, A>::zeros();
assert_eq!(
A::dim() % 3,
0,
"SNC can only be applied to accelerations multiple of 3"
);
for blk in 0..A::dim() / 3 {
for i in 0..3 {
let idx_i = i + A::dim() * blk;
let idx_j = i + 3 * blk;
let idx_k = i + 3 + A::dim() * blk;
gamma[(idx_i, idx_j)] = delta_t.powi(2) / 2.0;
gamma[(idx_k, idx_j)] = delta_t;
}
}
covar_bar += &gamma * self.process_noise.as_ref().unwrap() * &gamma.transpose();
}
}
let h_tilde_t = &self.h_tilde.transpose();
let mut invertible_part = &self.h_tilde * &covar_bar * h_tilde_t + &self.measurement_noise;
if !invertible_part.try_inverse_mut() {
return Err(FilterError::GainSingular);
}
let gain = &covar_bar * h_tilde_t * invertible_part;
let prefit = &real_obs - computed_obs;
let (state_hat, res) = if self.ekf {
let state_hat = &gain * &prefit;
let postfit = &prefit - (&self.h_tilde * &state_hat);
(
state_hat,
Residual::new(nominal_state.epoch(), prefit, postfit),
)
} else {
let state_bar = &self.stm * &self.prev_estimate.state_deviation;
let postfit = &prefit - (&self.h_tilde * &state_bar);
(
state_bar + &gain * &postfit,
Residual::new(nominal_state.epoch(), prefit, postfit),
)
};
let first_term = MatrixMN::<f64, S, S>::identity() - &gain * &self.h_tilde;
let covar = &first_term * covar_bar * &first_term.transpose()
+ &gain * &self.measurement_noise * &gain.transpose();
let estimate = KfEstimate {
nominal_state,
state_deviation: state_hat,
covar,
stm: self.stm.clone(),
predicted: false,
epoch_fmt: self.epoch_fmt,
covar_fmt: self.covar_fmt,
};
self.stm_updated = false;
self.h_tilde_updated = false;
self.prev_estimate = estimate.clone();
Ok((estimate, res))
}
fn is_extended(&self) -> bool {
self.ekf
}
fn set_extended(&mut self, status: bool) {
self.ekf = status;
}
fn set_process_noise(&mut self, prc: MatrixMN<f64, A, A>) {
self.process_noise = Some(prc);
}
}