From 412580b331965809323fb4983e31d7a4f0580527 Mon Sep 17 00:00:00 2001 From: jonas Date: Fri, 4 Oct 2024 16:02:52 +0200 Subject: [PATCH] Orientation filter, use states for eq of motion derivations (python) --- .../python_derivation/KF_unified.py | 33 +++++++++++++++---- .../Position/python_derivation/KF_unified.py | 2 -- 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py b/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py index c74c5a3d88cd..3fd7a45638bb 100755 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py +++ b/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py @@ -94,13 +94,34 @@ class MState(sf.Matrix): class VMeas(sf.Matrix): SHAPE = (1, State.storage_dim()) -def get_Phi(dt: sf.Scalar) -> T.Tuple[sf.Matrix, sf.Matrix]: +def get_Phi(dt: sf.Scalar) -> sf.Matrix: + Phi = sf.Matrix.zeros(State.storage_dim(), State.storage_dim()) + + # Helper function to set blocks in the Phi matrix + def set_Phi_block(row_key, col_key, value): + idx_row = tangent_idx[row_key].idx + dof_row = tangent_idx[row_key].dof + idx_col = tangent_idx[col_key].idx + dof_col = tangent_idx[col_key].dof + if isinstance(value, sf.Matrix): + # Ensure the value matrix has the correct shape + assert value.shape == (dof_row, dof_col), "Value matrix shape mismatch" + Phi[idx_row:idx_row + dof_row, idx_col:idx_col + dof_col] = value + else: + # Scalar value; create a block matrix + block = value * sf.Matrix.eye(dof_row, dof_col) + Phi[idx_row:idx_row + dof_row, idx_col:idx_col + dof_col] = block + + # Set the diagonal elements of Phi to 1 + for key in State.keys_recursive(): + idx = tangent_idx[key].idx + dof = tangent_idx[key].dof + for i in range(dof): + Phi[idx + i, idx + i] = 1 + if moving: - Phi = sf.Matrix([ - [1, dt], - [0, 1]]) - else: - Phi = sf.Matrix([[1]]) + set_Phi_block('yaw', 'yaw_rate', dt) + return Phi def predictState(dt: sf.Scalar, state: VState) -> VState: diff --git a/src/modules/vision_target_estimator/Position/python_derivation/KF_unified.py b/src/modules/vision_target_estimator/Position/python_derivation/KF_unified.py index 191e2d59dec6..025a6df712cd 100755 --- a/src/modules/vision_target_estimator/Position/python_derivation/KF_unified.py +++ b/src/modules/vision_target_estimator/Position/python_derivation/KF_unified.py @@ -97,9 +97,7 @@ class MState(sf.Matrix): class VMeas(sf.Matrix): SHAPE = (1, State.storage_dim()) - def get_Phi(dt: sf.Scalar) -> sf.Matrix: - n = State.storage_dim() Phi = sf.Matrix.zeros(State.storage_dim(), State.storage_dim()) # Helper functions to set blocks in Phi matrix