Skip to content

Commit 8935872

Browse files
mgrouchmgrouch
authored
Update Kalman3D_Wave.h
1 parent 53889bd commit 8935872

File tree

1 file changed

+80
-112
lines changed

1 file changed

+80
-112
lines changed

‎bbn_wave_freq_m5atomS3/Kalman3D_Wave.h‎

Lines changed: 80 additions & 112 deletions
Original file line numberDiff line numberDiff line change
@@ -411,86 +411,6 @@ class EIGEN_ALIGN_MAX Kalman3D_Wave {
411411
// Symmetrize for numerical hygiene
412412
Pext = T(0.5) * (Pext + Pext.transpose());
413413
}
414-
415-
// Unified builder for (PCᵀ, S) = (P Cᵀ, C P Cᵀ + R)
416-
// Mirrors the summation order of the original measurement updates
417-
// to preserve float numerical precision.
418-
// - J_att: 3x3 (can be all-zero if not used).
419-
// - J_aw : nullptr if not used; 3x3 if used.
420-
// - include_ba: include accel bias block.
421-
// - direct_offset: >=0 for direct observed 3x1 block (e.g. OFF_S), else -1.
422-
// - Rm: measurement noise 3x3.
423-
EIGEN_STRONG_INLINE void build_PCt_and_S_sparse_(
424-
const Matrix3& J_att,
425-
const Matrix3* J_aw,
426-
bool include_ba,
427-
int direct_offset,
428-
const Matrix3& Rm,
429-
Eigen::Matrix<T,NX,3>& PCt,
430-
Matrix3& S_mat) const
431-
{
432-
PCt.setZero();
433-
S_mat = Rm;
434-
435-
constexpr int OFF_TH = 0;
436-
437-
// --- θ block ---
438-
{
439-
bool use_theta = !J_att.isZero(1e-20); // small tolerance
440-
if (use_theta) {
441-
const auto P_all_th = Pext.template block<NX,3>(0, OFF_TH);
442-
const Matrix3 P_th_th = Pext.template block<3,3>(OFF_TH, OFF_TH);
443-
PCt.noalias() += P_all_th * J_att.transpose();
444-
S_mat.noalias() += J_att * P_th_th * J_att.transpose();
445-
}
446-
447-
// --- θ–a_w cross terms ---
448-
if (J_aw && use_theta) {
449-
const Matrix3 P_th_aw = Pext.template block<3,3>(OFF_TH, OFF_AW);
450-
S_mat.noalias() += J_att * P_th_aw * J_aw->transpose();
451-
S_mat.noalias() += (*J_aw) * P_th_aw.transpose() * J_att.transpose();
452-
}
453-
454-
// --- a_w block ---
455-
if (J_aw) {
456-
const auto P_all_aw = Pext.template block<NX,3>(0, OFF_AW);
457-
const Matrix3 P_aw_aw = Pext.template block<3,3>(OFF_AW, OFF_AW);
458-
PCt.noalias() += P_all_aw * J_aw->transpose();
459-
S_mat.noalias() += (*J_aw) * P_aw_aw * J_aw->transpose();
460-
}
461-
462-
// --- bias block ---
463-
if constexpr (with_accel_bias) {
464-
if (include_ba) {
465-
// θ–b_a cross
466-
if (use_theta) {
467-
const Matrix3 P_th_ba = Pext.template block<3,3>(OFF_TH, OFF_BA);
468-
S_mat.noalias() += J_att * P_th_ba;
469-
S_mat.noalias() += P_th_ba.transpose() * J_att.transpose();
470-
}
471-
// a_w–b_a cross
472-
if (J_aw) {
473-
const Matrix3 P_aw_ba = Pext.template block<3,3>(OFF_AW, OFF_BA);
474-
S_mat.noalias() += (*J_aw) * P_aw_ba;
475-
S_mat.noalias() += P_aw_ba.transpose() * J_aw->transpose();
476-
}
477-
// bias block itself
478-
const auto P_all_ba = Pext.template block<NX,3>(0, OFF_BA);
479-
const Matrix3 P_ba_ba = Pext.template block<3,3>(OFF_BA, OFF_BA);
480-
PCt.noalias() += P_all_ba;
481-
S_mat.noalias() += P_ba_ba;
482-
}
483-
}
484-
485-
// --- direct observed block (e.g. S pseudo-measurement) ---
486-
if (direct_offset >= 0) {
487-
const auto P_all_dir = Pext.template block<NX,3>(0, direct_offset);
488-
const Matrix3 P_dir_dir = Pext.template block<3,3>(direct_offset, direct_offset);
489-
PCt.noalias() += P_all_dir;
490-
S_mat.noalias() += P_dir_dir;
491-
}
492-
}
493-
}
494414
};
495415

496416
// Implementation
@@ -853,32 +773,78 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_o
853773
const T g_meas = acc_meas.norm();
854774
if (std::abs(g_meas - gravity_magnitude_) > T(2.0) * gravity_magnitude_) return;
855775

856-
// Predicted specific force: f_b = R_wb (a_w - g) + b_a(temp)
776+
// Predicted specific force
857777
const Vector3 v1hat = accelerometer_measurement_func(tempC);
858778

859779
// Jacobians wrt the only columns C touches
860780
const Matrix3 J_att = -skew_symmetric_matrix(v1hat); // d f / d θ
861781
const Matrix3 J_aw = R_wb(); // d f / d a_w
782+
Matrix3 J_ba; // d f / d b_a
783+
if constexpr (with_accel_bias) J_ba.setIdentity();
862784

863785
const Vector3 r = acc_meas - v1hat;
864786

865-
// Build PCᵀ and S (θ + a_w + b_a)
866-
Eigen::Matrix<T,NX,3> PCt;
867-
Matrix3 S_mat;
868-
build_PCt_and_S_sparse_(J_att, &J_aw, true, -1, Racc, PCt, S_mat);
787+
// Innovation covariance S = C P Cᵀ + Racc (3×3)
788+
Matrix3 S_mat = Racc;
789+
{
790+
constexpr int OFF_TH = 0;
791+
const int off_aw = OFF_AW;
792+
const int off_ba = OFF_BA;
793+
794+
const Matrix3 P_th_th = Pext.template block<3,3>(OFF_TH, OFF_TH);
795+
const Matrix3 P_th_aw = Pext.template block<3,3>(OFF_TH, off_aw);
796+
const Matrix3 P_aw_aw = Pext.template block<3,3>(off_aw, off_aw);
797+
798+
S_mat.noalias() += J_att * P_th_th * J_att.transpose();
799+
S_mat.noalias() += J_att * P_th_aw * J_aw.transpose();
800+
S_mat.noalias() += J_aw * P_th_aw.transpose() * J_att.transpose();
801+
S_mat.noalias() += J_aw * P_aw_aw * J_aw.transpose();
802+
803+
if constexpr (with_accel_bias) {
804+
const Matrix3 P_th_ba = Pext.template block<3,3>(OFF_TH, off_ba);
805+
const Matrix3 P_aw_ba = Pext.template block<3,3>(off_aw, off_ba);
806+
const Matrix3 P_ba_ba = Pext.template block<3,3>(off_ba, off_ba);
869807

870-
// Gain and update
808+
S_mat.noalias() += J_att * P_th_ba * J_ba.transpose();
809+
S_mat.noalias() += J_aw * P_aw_ba * J_ba.transpose();
810+
811+
S_mat.noalias() += J_ba * P_th_ba.transpose() * J_att.transpose();
812+
S_mat.noalias() += J_ba * P_aw_ba.transpose() * J_aw.transpose();
813+
814+
S_mat.noalias() += J_ba * P_ba_ba * J_ba.transpose();
815+
}
816+
}
817+
818+
// PCᵀ = P Cᵀ (NX×3)
819+
Eigen::Matrix<T,NX,3> PCt; PCt.setZero();
820+
{
821+
constexpr int OFF_TH = 0;
822+
const auto P_all_th = Pext.template block<NX,3>(0, OFF_TH);
823+
const auto P_all_aw = Pext.template block<NX,3>(0, OFF_AW);
824+
PCt.noalias() += P_all_th * J_att.transpose();
825+
PCt.noalias() += P_all_aw * J_aw.transpose();
826+
827+
if constexpr (with_accel_bias) {
828+
const auto P_all_ba = Pext.template block<NX,3>(0, OFF_BA);
829+
PCt.noalias() += P_all_ba * J_ba.transpose(); // J_ba = I
830+
}
831+
}
832+
833+
// Gain
871834
Eigen::LDLT<Matrix3> ldlt;
872835
if (!safe_ldlt3_(S_mat, ldlt, Racc.norm())) return;
873836
const Eigen::Matrix<T,NX,3> K = PCt * ldlt.solve(Matrix3::Identity());
874837

838+
// State update
875839
xext.noalias() += K * r;
876840

877-
// Joseph covariance update
841+
// Covariance update
878842
joseph_update3_(K, S_mat, PCt);
879843

880-
// Apply quaternion correction and mirror base block
844+
// Apply quaternion correction
881845
applyQuaternionCorrectionFromErrorState();
846+
847+
// Mirror base block
882848
Pbase = Pext.topLeftCorner(BASE_N, BASE_N);
883849
}
884850

@@ -894,35 +860,36 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_mag_o
894860
const T n_pred = v2hat.norm();
895861
if (n_meas < T(1e-6) || n_pred < T(1e-6)) return;
896862

863+
// Unit vectors for dot product check
897864
Vector3 meas_n = mag_meas_body / n_meas;
898865
Vector3 pred_n = v2hat / n_pred;
899-
const T dotp = meas_n.dot(pred_n);
866+
T dotp = meas_n.dot(pred_n);
900867

901-
const T DOT_DANGEROUS = T(0.2); // if |dot| < 0.2 (~78°) → yaw-only branch
868+
const T DOT_DANGEROUS = T(0.2); // if |dot| < 0.2 (~>78°) → yaw-only branch
902869
const T YAW_CLAMP = T(0.105); // ~6° clamp per update
903870

904871
if (std::abs(dotp) >= DOT_DANGEROUS) {
905-
// -------- BRANCH 1: SAFE FULL 3D UPDATE --------
872+
// BRANCH 1: SAFE FULL 3D UPDATE
906873
const Vector3 meas_fixed = (dotp >= T(0)) ? mag_meas_body : -mag_meas_body;
907874
const Vector3 r = meas_fixed - v2hat;
908875

909876
const Matrix3 J_att = -skew_symmetric_matrix(v2hat);
910877

911-
Eigen::Matrix<T,NX,3> PCt;
912-
Matrix3 S_mat;
913-
// θ only; no a_w, no b_a, no direct block
914-
build_PCt_and_S_sparse_(J_att, /*J_aw*/nullptr, /*include_ba*/false,
915-
/*direct_offset*/-1, Rmag, PCt, S_mat);
878+
Matrix3 S_mat = Rmag;
879+
const Matrix3 P_th_th = Pext.template block<3,3>(0,0);
880+
S_mat.noalias() += J_att * P_th_th * J_att.transpose();
881+
882+
Eigen::Matrix<T,NX,3> PCt = Pext.template block<NX,3>(0,0) * J_att.transpose();
916883

917884
Eigen::LDLT<Matrix3> ldlt;
918885
if (!safe_ldlt3_(S_mat, ldlt, Rmag.norm())) return;
919886
const Eigen::Matrix<T,NX,3> K = PCt * ldlt.solve(Matrix3::Identity());
920887

921888
xext.noalias() += K * r;
922-
joseph_update3_(K, S_mat, PCt);
923889

890+
joseph_update3_(K, S_mat, PCt);
924891
} else {
925-
// -------- BRANCH 2: YAW-ONLY PROJECTED UPDATE --------
892+
// BRANCH 2: YAW-ONLY PROJECTED UPDATE
926893
const Vector3 gb = (R_wb() * Vector3(0,0,1)).normalized();
927894
const Matrix3 Hb = Matrix3::Identity() - gb * gb.transpose();
928895

@@ -937,16 +904,15 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_mag_o
937904

938905
const Vector3 r = Hb * (mag_meas_body - v2hat);
939906
const Matrix3 J_att_proj = Hb * (-skew_symmetric_matrix(v2hat));
940-
const Matrix3 R_proj = Hb * Rmag * Hb.transpose();
941907

942-
Eigen::Matrix<T,NX,3> PCt;
943-
Matrix3 S_mat;
944-
// θ (projected); no a_w, no b_a, no direct block
945-
build_PCt_and_S_sparse_(J_att_proj, /*J_aw*/nullptr, /*include_ba*/false,
946-
/*direct_offset*/-1, R_proj, PCt, S_mat);
908+
Matrix3 S_mat = Hb * Rmag * Hb.transpose();
909+
const Matrix3 P_th_th = Pext.template block<3,3>(0,0);
910+
S_mat.noalias() += J_att_proj * P_th_th * J_att_proj.transpose();
911+
912+
Eigen::Matrix<T,NX,3> PCt = Pext.template block<NX,3>(0,0) * J_att_proj.transpose();
947913

948914
Eigen::LDLT<Matrix3> ldlt;
949-
if (!safe_ldlt3_(S_mat, ldlt, R_proj.norm())) return;
915+
if (!safe_ldlt3_(S_mat, ldlt, S_mat.norm())) return;
950916
const Eigen::Matrix<T,NX,3> K = PCt * ldlt.solve(Matrix3::Identity());
951917

952918
xext.noalias() += K * r;
@@ -960,8 +926,10 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_mag_o
960926
joseph_update3_(K, S_mat, PCt);
961927
}
962928

963-
// Apply quaternion correction & mirror base block
929+
// Apply quaternion correction
964930
applyQuaternionCorrectionFromErrorState();
931+
932+
// Mirror base block
965933
Pbase = Pext.topLeftCorner(BASE_N, BASE_N);
966934
}
967935

@@ -1022,11 +990,11 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::applyIntegralZeroPseudoM
1022990
// Innovation: target S = 0
1023991
const Vector3 r = -xext.template segment<3>(off_S);
1024992

1025-
// PCᵀ and S for direct-observed S block
1026-
Eigen::Matrix<T,NX,3> PCt;
1027-
Matrix3 S_mat;
1028-
build_PCt_and_S_sparse_(Matrix3::Zero(), /*J_aw*/nullptr, /*include_ba*/false,
1029-
/*direct_offset*/off_S, R_S, PCt, S_mat);
993+
// Innovation covariance S = P_SS + R_S
994+
Matrix3 S_mat = Pext.template block<3,3>(off_S, off_S) + R_S;
995+
996+
// Cross covariance PCᵀ = P(:,S) (NX×3)
997+
Eigen::Matrix<T,NX,3> PCt = Pext.template block<NX,3>(0, off_S);
1030998

1031999
Eigen::LDLT<Matrix3> ldlt;
10321000
if (!safe_ldlt3_(S_mat, ldlt, R_S.norm())) return;
@@ -1038,7 +1006,7 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::applyIntegralZeroPseudoM
10381006
// Covariance update
10391007
joseph_update3_(K, S_mat, PCt);
10401008

1041-
// Attitude may get nudged via cross-covariances
1009+
// Apply quaternion correction (attitude may get nudged via cross-covariances)
10421010
applyQuaternionCorrectionFromErrorState();
10431011

10441012
// Mirror base block

0 commit comments

Comments
 (0)