Update UKF covariance calculations and improve input validation

.gitignore

@@ -4,6 +4,7 @@
 /strapdown_py/.pytest_cache
 /strapdown_py/dist
 /core/*.csv
+/core/db
 
 # pixi environments
 .pixi
@@ -22,3 +23,4 @@ __pycache__
 .python-version
 pyproject.toml
 uv.lock
+batch_process.nu

core/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "strapdown-rs"
 authors = ["James Brodovsky"]
-version = "0.3.6"
+version = "0.3.7"
 edition = "2024"
 description = "A toolbox for building and analyzing strapdown inertial navigation systems."
 license = "MIT"

core/src/filter.rs

@@ -405,6 +405,7 @@ impl UKF {
                 ),
                 coordinate_convention: true,
             };
+            // println!("propagating: lat {}  lon {}", state.latitude.to_degrees(), state.longitude.to_degrees());
             forward(&mut state, imu_data, dt);
             // Update the sigma point with the new state
             sigma_point_vec[0] = state.latitude;
@@ -509,7 +510,7 @@ impl UKF {
             Some(inv) => inv,
             None => panic!("Innovation matrix is singular"),
         };
-        let k = &cross_covariance * s_inv;
+        let k = &cross_covariance * &s_inv;
         // check that the kalman gain and measurement diff are compatible to multiply
         if k.ncols() != measurement.get_dimension() {
             panic!("Kalman gain and measurement differential are not compatible");
@@ -522,16 +523,12 @@ impl UKF {
         self.mean_state[8] = wrap_to_2pi(self.mean_state[8]);
         // Switch to Joseph Form update here
         // P = (I - K * H) P (I - K H)^T + K R K^T
-        let i = DMatrix::<f64>::identity(self.state_size, self.state_size);
-        let p = self.covariance.clone();
-        // println!("Updating covariance matrix");
-        // println!("Kalman gain: {:?}", k.shape());
-        // println!("Cross covariance: {:?}", cross_covariance.shape());
-        let m = &i - &k * &cross_covariance.transpose();
-        self.covariance =
-            &m * &p * &m.transpose()
-                + &k * measurement.get_noise() * &k.transpose();
-        //self.covariance -= &k * s * &k.transpose();
+        // UKF form:
+        // P -= K * S * K^T + K R K^T
+        self.covariance -= &k * s * &k.transpose();
+        // Re-symmetrize to fight round-off
+        self.covariance = 0.5 * (&self.covariance + self.covariance.transpose());
+        // self.covariance -= &k * s * &k.transpose();
     }
 }
 #[derive(Clone, Debug, Default)]