immutable default P0

Author: vegard-solum-4ss

src/smsfusion/_ins_v2.py

@@ -9,6 +9,20 @@
 from ._vectorops import _normalize, _skew_symmetric
 
 
+P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
+)
+
+
+
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
@@ -482,7 +496,7 @@ def __init__(
         v: ArrayLike = (0.0, 0.0, 0.0),
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(9),
+        P: ArrayLike = P0,
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
         gyro_bias_stability: float = 0.00005,