Merge branch 'imu-filter'

unmannedlab · Nov 22, 2024 · 5bc4bbd · 5bc4bbd
2 parents 0ef6fef + 4a76928
commit 5bc4bbd
Show file tree

Hide file tree

Showing 88 changed files with 1,783 additions and 2,497 deletions.
diff --git a/.vscode/ekf_cal-schema.json b/.vscode/ekf_cal-schema.json
@@ -17,7 +17,7 @@
                         "data_logging_on": {
                             "type": "boolean"
                         },
-                        "body_data_rate": {
+                        "data_log_rate": {
                             "type": "number"
                         },
                         "augmenting_type": {
@@ -41,17 +41,8 @@
                                 "vel": {
                                     "type": "number"
                                 },
-                                "acc": {
-                                    "type": "number"
-                                },
                                 "ang_pos": {
                                     "type": "number"
-                                },
-                                "ang_vel": {
-                                    "type": "number"
-                                },
-                                "ang_acc": {
-                                    "type": "number"
                                 }
                             }
                         },
@@ -78,15 +69,15 @@
                         "gps_init_ang_thresh": {
                             "type": "number"
                         },
+                        "use_root_covariance": {
+                            "type": "boolean"
+                        },
                         "sim_params": {
                             "type": "object",
                             "properties": {
                                 "seed": {
                                     "type": "number"
                                 },
-                                "use_seed": {
-                                    "type": "boolean"
-                                },
                                 "max_time": {
                                     "type": "number"
                                 },
@@ -245,9 +236,6 @@
                                 "^.*$": {
                                     "type": "object",
                                     "properties": {
-                                        "use_for_prediction": {
-                                            "type": "boolean"
-                                        },
                                         "is_extrinsic": {
                                             "type": "boolean"
                                         },
@@ -265,7 +253,7 @@
                                             "items": {
                                                 "type": "number"
                                             },
-                                            "minItems": 12,
+                                            "minItems": 6,
                                             "maxItems": 12
                                         },
                                         "pos_i_in_b": {
@@ -318,9 +306,6 @@
                                                 "no_errors": {
                                                     "type": "boolean"
                                                 },
-                                                "time_bias_error": {
-                                                    "type": "number"
-                                                },
                                                 "time_error": {
                                                     "type": "number"
                                                 },
@@ -406,6 +391,9 @@
                                             "minItems": 4,
                                             "maxItems": 4
                                         },
+                                        "is_extrinsic": {
+                                            "type": "boolean"
+                                        },
                                         "variance": {
                                             "type": "array",
                                             "items": {
@@ -464,9 +452,6 @@
                                                 "no_errors": {
                                                     "type": "boolean"
                                                 },
-                                                "time_bias_error": {
-                                                    "type": "number"
-                                                },
                                                 "time_error": {
                                                     "type": "number"
                                                 },
@@ -700,9 +685,6 @@
                                                 "no_errors": {
                                                     "type": "boolean"
                                                 },
-                                                "time_bias_error": {
-                                                    "type": "number"
-                                                },
                                                 "time_error": {
                                                     "type": "number"
                                                 },

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -18,6 +18,7 @@
         "editor.defaultFormatter": "zachflower.uncrustify"
     },
     "cSpell.words": [
+        "AngleAxisd",
         "ARUCO",
         "arxiv",
         "axvline",
@@ -32,6 +33,7 @@
         "doxyfile",
         "doxygen",
         "dropna",
+        "ECEF",
         "eigen",
         "extrinsics",
         "fargs",
@@ -57,6 +59,7 @@
         "Kabsch",
         "Kalman",
         "keypoint",
+        "Keypoints",
         "mainpage",
         "mathcal",
         "MATHHELPER",
@@ -74,6 +77,7 @@
         "Rehder",
         "Rodrigues",
         "Roumeliotis",
+        "rowwise",
         "RVIZ",
         "slerp",
         "Srikanth",

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -99,6 +99,7 @@ add_library(EKF_UTL ${EKF_UTL_SRCS})
 set(EKF_SRCS
     src/ekf/ekf.cpp
     src/ekf/types.cpp
+    src/ekf/imu_filter.cpp
     src/ekf/update/fiducial_updater.cpp
     src/ekf/update/gps_updater.cpp
     src/ekf/update/imu_updater.cpp

diff --git a/TODO b/TODO
@@ -1,39 +1,24 @@
 Package Features:
     - Implement filter status output
-    - Implement flag for calibration settling
-    - Implement flag for calibration shifting
-    - Interpolation between stochastic clones
     - Intrinsic calibration filtering
 
 Software Work:
-    - Auto-generate rviz configuration from yaml
-    - CI/CD for unit tests
-    - Integration tests
-    - Add mutex to EKF class and have mutex locked by updaters
-    - Switch to reentrant callback group
     - Show track images from simulation
     - Add data drops to input file
     - Add extrinsic step disturbance
     - Add IMU bias instability
         - Plot Allan Variance
-    - Compute Cramer Rao Lower Bound
-        - https://iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF24.pdf
-        - https://www.cs.cmu.edu/~aarti/Class/10704_Spring15/lecs/lec23.pdf
     - Outlier rejection by gating / Mahalanobis distance
     - Play rosbags through as fast as possible given yaml
-    - Remove time skew from code
-    - Update time bias to be per-sensor
     - RANSAC Features
-    - Calculate CR Lower Bound
 
     Additional Trackers:
-        - FiducialTracker
-            - AprilGridTracker
+        - AprilGridTracker
 
 Documentation:
     - Simulation diagram
     - Add gif to main readme
-    - Create Simulated Camera video feed
+    - Create Simulated Camera video feed 
         - Circles for points, radius based on distance
         - Polygon for Board edges
         - https://docs.opencv.org/4.6.0/d3/d96/tutorial_basic_geometric_drawing.html
@@ -43,8 +28,6 @@ Documentation:
     - Add page for initialization
     - Add page for augmenting states
     - Add Jacobians to documentation
-    - Add Setup SVG to main ReadMe
-    - Add page for code coverage
     - Add page for Multi-IMU Filter
 
 Data: