diff options
| -rw-r--r-- | src/lib.rs | 41 |
1 files changed, 29 insertions, 12 deletions
@@ -81,10 +81,29 @@ where impl<T, D, SM, SV> Filter<T, D, SM, SV> where + T: Scalar, + D: Dim, + SM: Storage<T, D, D>, + SV: Storage<T, D>, + DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>, +{ + /// Clone this `Filter` using `DefaultAllocator`. + /// + /// Useful to get types to match up (e.g. if you need a `DefaultAllocFilter`). + pub fn clone_owned(&self) -> DefaultAllocFilter<T, D> { + Filter { + x_k: self.x_k.clone_owned(), + p_k: self.p_k.clone_owned(), + } + } +} + +impl<T, D, SM, SV> Filter<T, D, SM, SV> +where T: Scalar + RealField, - D: Dim + DimName, + D: Dim, SM: Storage<T, D, D>, - SV: Storage<T, D, D> + Storage<T, D>, + SV: Storage<T, D>, DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>, { /// Predict the next state. @@ -129,22 +148,20 @@ where SH: Storage<T, D, D>, SR: Storage<T, D, D>, { - use num_traits::Zero; - let z = z.borrow(); let h = h.borrow(); let r = r.borrow(); let h_t = &h.transpose(); - let k = &self.p_k * - h_t * - (h * &self.p_k * h_t + r).try_inverse().unwrap_or_else(DefaultAllocMatrix::zero); - - Filter { - x_k: &self.x_k + &k * (z - h * &self.x_k), - p_k: &self.p_k - &k * h * &self.p_k, - } + (h * &self.p_k * h_t + r) + .try_inverse() + .map(|part_k| &self.p_k * h_t * part_k) + .map(|k| Filter { + x_k: &self.x_k + &k * (z - h * &self.x_k), + p_k: &self.p_k - &k * h * &self.p_k, + }) + .unwrap_or_else(|| self.clone_owned()) } } |