Skip to content

Commit 07fe092

Browse files
mgrouchmgrouch
authored
Update Kalman3D_Wave.h
1 parent aa6ad1f commit 07fe092

File tree

1 file changed

+100
-176
lines changed

1 file changed

+100
-176
lines changed

‎bbn_wave_freq_m5atomS3/Kalman3D_Wave.h‎

Lines changed: 100 additions & 176 deletions
Original file line numberDiff line numberDiff line change
@@ -378,45 +378,14 @@ class EIGEN_ALIGN_MAX Kalman3D_Wave {
378378
// Quaternion & small-angle helpers (kept)
379379
void applyQuaternionCorrectionFromErrorState(); // apply correction to qref using xext(0..2)
380380
void normalizeQuat();
381-
381+
void vanLoanDiscretization_12x3(const Eigen::Matrix<T,12,12>& A,
382+
const Eigen::Matrix<T,12,3>& G,
383+
const Eigen::Matrix<T,3,3>& Sigma_c,
384+
T Ts,
385+
Eigen::Matrix<T,12,12>& Phi,
386+
Eigen::Matrix<T,12,12>& Qd) const;
382387
static void PhiAxis4x1_analytic(T tau, T h, Eigen::Matrix<T,4,4>& Phi_axis);
383388
static void QdAxis4x1_analytic(T tau, T h, T sigma2, Eigen::Matrix<T,4,4>& Qd_axis);
384-
385-
// Propagate top-left attitude (+ optional gyro bias) covariance block
386-
// This covers the 3x3 (attitude-only) or 6x6 (attitude+gyro bias) base state.
387-
// Equivalent to P ← Φ P Φᵀ + Q
388-
template<typename PBlockType>
389-
void propagate_att_bias_cov(PBlockType Pblock,
390-
const Eigen::Matrix<T,BASE_N,BASE_N>& Phi_block,
391-
const Eigen::Matrix<T,BASE_N,BASE_N>& Q_block)
392-
{
393-
// Propagate covariance
394-
Pblock = Phi_block * Pblock * Phi_block.transpose();
395-
396-
// Add process noise
397-
Pblock += Q_block;
398-
399-
// Enforce symmetry
400-
Pblock = T(0.5) * (Pblock + Pblock.transpose());
401-
}
402-
403-
// Propagate one axis [v, p, S, a] 4x4 covariance
404-
// Each axis is independent, so we update three 4x4 diagonal blocks separately.
405-
// Equivalent to P ← Φ P Φᵀ + Q , with symmetry enforced.
406-
template<typename PBlockType>
407-
void propagate_axis_cov(PBlockType Pblock,
408-
const Eigen::Matrix<T,4,4>& Phi_axis,
409-
const Eigen::Matrix<T,4,4>& Qd_axis)
410-
{
411-
// Propagate covariance
412-
Pblock = Phi_axis * Pblock * Phi_axis.transpose();
413-
414-
// Add process noise
415-
Pblock += Qd_axis;
416-
417-
// Enforce symmetry
418-
Pblock = T(0.5) * (Pblock + Pblock.transpose());
419-
}
420389
};
421390

422391
// Implementation
@@ -576,10 +545,9 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::initialize_from_acc(Vect
576545
}
577546

578547
template<typename T, bool with_gyro_bias, bool with_accel_bias>
579-
void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::time_update(
580-
Vector3 const& gyr, T Ts)
548+
void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::time_update(Vector3 const& gyr, T Ts)
581549
{
582-
// ===== Mean propagation =====
550+
// Attitude mean propagation
583551
Vector3 gyro_bias;
584552
if constexpr (with_gyro_bias) {
585553
gyro_bias = xext.template segment<3>(3);
@@ -590,15 +558,16 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::time_update(
590558

591559
// Δθ = ω Ts → quaternion increment
592560
Eigen::Quaternion<T> dq = quat_from_delta_theta((last_gyr_bias_corrected * Ts).eval());
561+
562+
// Propagate: right-multiply (matches correction side and F/Jacobians signs )
593563
qref = qref * dq;
594564
qref.normalize();
595565

596-
// Build F, Q
597-
MatrixNX F_a_ext;
598-
MatrixNX Q_a_ext;
566+
// Build exact discrete transition & process Q
567+
MatrixNX F_a_ext; MatrixNX Q_a_ext;
599568
assembleExtendedFandQ(Vector3::Zero(), Ts, F_a_ext, Q_a_ext);
600569

601-
// Linear state mean [v,p,S,a_w]
570+
// Mean propagation for linear subsystem [v,p,S,a_w]
602571
Eigen::Matrix<T,12,1> x_lin_prev;
603572
x_lin_prev.template segment<3>(0) = xext.template segment<3>(OFF_V);
604573
x_lin_prev.template segment<3>(3) = xext.template segment<3>(OFF_P);
@@ -608,80 +577,82 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::time_update(
608577
const auto Phi_lin = F_a_ext.template block<12,12>(OFF_V, OFF_V);
609578
Eigen::Matrix<T,12,1> x_lin_next = Phi_lin * x_lin_prev;
610579

580+
// write back mean
611581
xext.template segment<3>(OFF_V) = x_lin_next.template segment<3>(0);
612582
xext.template segment<3>(OFF_P) = x_lin_next.template segment<3>(3);
613583
xext.template segment<3>(OFF_S) = x_lin_next.template segment<3>(6);
614584
xext.template segment<3>(OFF_AW) = x_lin_next.template segment<3>(9);
615585

616-
// ===== Covariance propagation (block-wise exact) =====
617-
constexpr int NA = BASE_N; // attitude (+ gyro bias)
618-
constexpr int NL = 12; // [v p S a_w]
619-
constexpr int NC = with_accel_bias ? 3 : 0;
620-
621-
// F and Q blocks
622-
const Eigen::Matrix<T,NA,NA> F_AA = F_a_ext.template block<NA,NA>(0,0);
623-
const Eigen::Matrix<T,NL,NL> F_LL = F_a_ext.template block<NL,NL>(OFF_V, OFF_V);
624-
Eigen::Matrix<T,NC,NC> F_CC;
625-
if constexpr (with_accel_bias) F_CC.setIdentity();
626-
627-
const Eigen::Matrix<T,NA,NA> Q_AA = Q_a_ext.template block<NA,NA>(0,0);
628-
const Eigen::Matrix<T,NL,NL> Q_LL = Q_a_ext.template block<NL,NL>(OFF_V, OFF_V);
629-
Eigen::Matrix<T,NC,NC> Q_CC;
630-
if constexpr (with_accel_bias) Q_CC = Q_a_ext.template block<NC,NC>(OFF_BA, OFF_BA);
631-
632-
// Old P blocks
633-
const Eigen::Matrix<T,NA,NA> P_AA = Pext.template block<NA,NA>(0,0);
634-
const Eigen::Matrix<T,NL,NL> P_LL = Pext.template block<NL,NL>(OFF_V, OFF_V);
635-
Eigen::Matrix<T,NA,NL> P_AL = Pext.template block<NA,NL>(0, OFF_V);
636-
637-
Eigen::Matrix<T,NC,NC> P_CC;
638-
Eigen::Matrix<T,NA,NC> P_AC;
639-
Eigen::Matrix<T,NL,NC> P_LC;
640-
if constexpr (with_accel_bias) {
641-
P_CC = Pext.template block<NC,NC>(OFF_BA, OFF_BA);
642-
P_AC = Pext.template block<NA,NC>(0, OFF_BA);
643-
P_LC = Pext.template block<NL,NC>(OFF_V, OFF_BA);
644-
}
645-
646-
// Diagonal block updates
647-
Eigen::Matrix<T,NA,NA> P_AA_new = F_AA * P_AA * F_AA.transpose() + Q_AA;
648-
Eigen::Matrix<T,NL,NL> P_LL_new = F_LL * P_LL * F_LL.transpose() + Q_LL;
586+
// Covariance propagation (optimized for current F/Q structure)
587+
{
588+
constexpr int NA = BASE_N;
589+
constexpr int NL = 12;
590+
591+
const auto F_AA = F_a_ext.template block<NA,NA>(0,0);
592+
const auto F_LL = F_a_ext.template block<NL,NL>(OFF_V,OFF_V);
593+
594+
Eigen::Matrix<T,NA,NA> P_AA = Pext.template block<NA,NA>(0,0);
595+
Eigen::Matrix<T,NL,NL> P_LL = Pext.template block<NL,NL>(OFF_V,OFF_V);
596+
Eigen::Matrix<T,NA,NL> P_AL = Pext.template block<NA,NL>(0,OFF_V);
597+
598+
Eigen::Matrix<T,NA,NA> P_AA_new;
599+
{
600+
Eigen::Matrix<T,NA,NA> tmp;
601+
tmp.noalias() = F_AA * P_AA;
602+
P_AA_new.noalias() = tmp * F_AA.transpose();
603+
P_AA_new += Q_a_ext.template block<NA,NA>(0,0);
604+
}
649605

650-
Eigen::Matrix<T,NC,NC> P_CC_new;
651-
if constexpr (with_accel_bias) {
652-
P_CC_new = P_CC + Q_CC; // F_CC = I
653-
}
606+
Eigen::Matrix<T,NL,NL> P_LL_new;
607+
{
608+
Eigen::Matrix<T,NL,NL> tmp;
609+
tmp.noalias() = F_LL * P_LL;
610+
P_LL_new.noalias() = tmp * F_LL.transpose();
611+
P_LL_new += Q_a_ext.template block<NL,NL>(OFF_V,OFF_V);
612+
}
654613

655-
// Cross blocks
656-
Eigen::Matrix<T,NA,NL> P_AL_new = F_AA * P_AL * F_LL.transpose();
657-
Eigen::Matrix<T,NA,NC> P_AC_new;
658-
Eigen::Matrix<T,NL,NC> P_LC_new;
659-
if constexpr (with_accel_bias) {
660-
P_AC_new = F_AA * P_AC;
661-
P_LC_new = F_LL * P_LC;
662-
}
614+
Eigen::Matrix<T,NA,NL> P_AL_new;
615+
{
616+
Eigen::Matrix<T,NA,NL> tmp;
617+
tmp.noalias() = F_AA * P_AL;
618+
P_AL_new.noalias() = tmp * F_LL.transpose();
619+
}
663620

664-
// Write back
665-
Pext.template block<NA,NA>(0,0) = P_AA_new;
666-
Pext.template block<NL,NL>(OFF_V,OFF_V) = P_LL_new;
667-
Pext.template block<NA,NL>(0,OFF_V) = P_AL_new;
668-
Pext.template block<NL,NA>(OFF_V,0) = P_AL_new.transpose();
621+
if constexpr (with_accel_bias) {
622+
constexpr int NB = 3;
623+
Eigen::Matrix<T,NB,NB> P_BB = Pext.template block<NB,NB>(OFF_BA,OFF_BA);
624+
Eigen::Matrix<T,NA,NB> P_AB = Pext.template block<NA,NB>(0,OFF_BA);
625+
Eigen::Matrix<T,NL,NB> P_LB = Pext.template block<NL,NB>(OFF_V,OFF_BA);
626+
627+
Eigen::Matrix<T,NA,NB> P_AB_new = F_AA * P_AB;
628+
Eigen::Matrix<T,NL,NB> P_LB_new = F_LL * P_LB;
629+
Eigen::Matrix<T,NB,NB> P_BB_new = P_BB + Q_a_ext.template block<NB,NB>(OFF_BA,OFF_BA);
630+
631+
Pext.template block<NA,NA>(0,0) = P_AA_new;
632+
Pext.template block<NL,NL>(OFF_V,OFF_V) = P_LL_new;
633+
Pext.template block<NA,NL>(0,OFF_V) = P_AL_new;
634+
Pext.template block<NL,NA>(OFF_V,0) = P_AL_new.transpose();
635+
636+
Pext.template block<NA,NB>(0,OFF_BA) = P_AB_new;
637+
Pext.template block<NB,NA>(OFF_BA,0) = P_AB_new.transpose();
638+
Pext.template block<NL,NB>(OFF_V,OFF_BA) = P_LB_new;
639+
Pext.template block<NB,NL>(OFF_BA,OFF_V) = P_LB_new.transpose();
640+
Pext.template block<NB,NB>(OFF_BA,OFF_BA) = P_BB_new;
641+
} else {
642+
Pext.template block<NA,NA>(0,0) = P_AA_new;
643+
Pext.template block<NL,NL>(OFF_V,OFF_V) = P_LL_new;
644+
Pext.template block<NA,NL>(0,OFF_V) = P_AL_new;
645+
Pext.template block<NL,NA>(OFF_V,0) = P_AL_new.transpose();
646+
}
669647

670-
if constexpr (with_accel_bias) {
671-
Pext.template block<NC,NC>(OFF_BA,OFF_BA) = P_CC_new;
672-
Pext.template block<NA,NC>(0,OFF_BA) = P_AC_new;
673-
Pext.template block<NC,NA>(OFF_BA,0) = P_AC_new.transpose();
674-
Pext.template block<NL,NC>(OFF_V,OFF_BA) = P_LC_new;
675-
Pext.template block<NC,NL>(OFF_BA,OFF_V) = P_LC_new.transpose();
648+
// Optional symmetry enforcement (remove once validated):
649+
Pext = T(0.5) * (Pext + Pext.transpose());
676650
}
677651

678-
// Enforce symmetry
679-
Pext = T(0.5) * (Pext + Pext.transpose());
680-
681-
// Mirror base block
682-
Pbase = Pext.topLeftCorner(NA, NA);
652+
// Mirror base covariance
653+
Pbase = Pext.topLeftCorner(BASE_N, BASE_N);
683654

684-
// ===== periodic pseudo-measurement =====
655+
// Drift correction on S
685656
if (++pseudo_update_counter_ >= PSEUDO_UPDATE_PERIOD) {
686657
applyIntegralZeroPseudoMeas();
687658
pseudo_update_counter_ = 0;
@@ -729,93 +700,46 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_parti
729700
}
730701

731702
template<typename T, bool with_gyro_bias, bool with_accel_bias>
732-
void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_only(
733-
Vector3 const& acc_meas, T tempC)
734-
{
735-
// Gate accel magnitude
736-
const T g_meas = acc_meas.norm();
737-
if (std::abs(g_meas - gravity_magnitude_) > T(2.0) * gravity_magnitude_) return;
738-
739-
// Predicted specific force
740-
const Vector3 v1hat = accelerometer_measurement_func(tempC);
741-
742-
// Jacobians wrt the only columns C touches
743-
const Matrix3 J_att = -skew_symmetric_matrix(v1hat); // d f / d θ
744-
const Matrix3 J_aw = R_wb(); // d f / d a_w
745-
Matrix3 J_ba; // d f / d b_a
746-
if constexpr (with_accel_bias) J_ba.setIdentity();
747-
748-
const Vector3 r = acc_meas - v1hat;
749-
750-
// Column offsets
751-
constexpr int OFF_TH = 0; // attitude error starts at 0
752-
const int off_aw = OFF_AW;
753-
const int off_ba = OFF_BA;
703+
void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_only(Vector3 const& acc_meas, T tempC) {
754704

755-
// ---------- S = C P Cᵀ + Racc (3×3) ----------
756-
Matrix3 S_mat = Racc;
757-
758-
const Matrix3 P_th_th = Pext.template block<3,3>(OFF_TH, OFF_TH);
759-
const Matrix3 P_th_aw = Pext.template block<3,3>(OFF_TH, off_aw);
760-
const Matrix3 P_aw_aw = Pext.template block<3,3>(off_aw, off_aw);
705+
// Accel magnitude sanity check
706+
T g_meas = acc_meas.norm();
707+
if (std::abs(g_meas - gravity_magnitude_) > T(2.0 * gravity_magnitude_)) {
708+
return; // reject this update
709+
}
761710

762-
S_mat.noalias() += J_att * P_th_th * J_att.transpose();
763-
S_mat.noalias() += J_att * P_th_aw * J_aw.transpose();
764-
S_mat.noalias() += J_aw * P_th_aw.transpose() * J_att.transpose();
765-
S_mat.noalias() += J_aw * P_aw_aw * J_aw.transpose();
711+
const Vector3 v1hat = accelerometer_measurement_func(tempC);
766712

713+
// Cext: (3 x NX)
714+
Matrix<T, 3, NX> Cext = Matrix<T, 3, NX>::Zero();
715+
// d f_b / d (attitude error)
716+
Cext.template block<3,3>(0,0) = -skew_symmetric_matrix(v1hat);
717+
// d f_b / d a_w
718+
Cext.template block<3,3>(0, OFF_AW) = R_wb();
767719
if constexpr (with_accel_bias) {
768-
const Matrix3 P_th_ba = Pext.template block<3,3>(OFF_TH, off_ba);
769-
const Matrix3 P_aw_ba = Pext.template block<3,3>(off_aw, off_ba);
770-
const Matrix3 P_ba_ba = Pext.template block<3,3>(off_ba, off_ba);
771-
772-
S_mat.noalias() += J_att * P_th_ba * J_ba.transpose();
773-
S_mat.noalias() += J_aw * P_aw_ba * J_ba.transpose();
774-
775-
S_mat.noalias() += J_ba * P_th_ba.transpose() * J_att.transpose();
776-
S_mat.noalias() += J_ba * P_aw_ba.transpose() * J_aw.transpose();
777-
778-
S_mat.noalias() += J_ba * P_ba_ba * J_ba.transpose();
720+
Cext.template block<3,3>(0,OFF_BA) = Matrix3::Identity(); // d f_b / d b_acc
779721
}
780722

781-
// ---------- PCᵀ = P Cᵀ (NX×3) ----------
782-
Eigen::Matrix<T,NX,3> PCt; PCt.setZero();
783-
{
784-
const auto P_all_th = Pext.template block<NX,3>(0, OFF_TH);
785-
const auto P_all_aw = Pext.template block<NX,3>(0, off_aw);
786-
PCt.noalias() += P_all_th * J_att.transpose();
787-
PCt.noalias() += P_all_aw * J_aw.transpose();
723+
Vector3 inno = acc_meas - v1hat;
788724

789-
if constexpr (with_accel_bias) {
790-
const auto P_all_ba = Pext.template block<NX,3>(0, off_ba);
791-
PCt.noalias() += P_all_ba * J_ba.transpose(); // J_ba = I
792-
}
793-
}
725+
Matrix3 S_mat = Cext * Pext * Cext.transpose() + Racc;
726+
Matrix<T, NX, 3> PCt = Pext * Cext.transpose();
794727

795-
// ---------- K = PCᵀ S^{-1} ----------
796728
Eigen::LDLT<Matrix3> ldlt(S_mat);
797729
if (ldlt.info() != Eigen::Success) {
798-
S_mat += Matrix3::Identity()
799-
* std::max(std::numeric_limits<T>::epsilon(), T(1e-6) * Racc.norm());
730+
S_mat += Matrix3::Identity() * std::max(std::numeric_limits<T>::epsilon(), T(1e-6) * Racc.norm());
800731
ldlt.compute(S_mat);
801732
if (ldlt.info() != Eigen::Success) return;
802733
}
803-
const Eigen::Matrix<T,NX,3> K = PCt * ldlt.solve(Matrix3::Identity());
804-
805-
// ---------- State & Covariance (Joseph, full P) ----------
806-
xext.noalias() += K * r;
807-
808-
const Eigen::Matrix<T,3,NX> CP = PCt.transpose(); // CP = (PCᵀ)ᵀ
809-
const Eigen::Matrix<T,NX,NX> KCP = K * CP;
810-
const Eigen::Matrix<T,NX,NX> KSKt = K * S_mat * K.transpose();
734+
Matrix<T, NX, 3> K = PCt * ldlt.solve(Matrix3::Identity());
811735

812-
Pext.noalias() -= KCP;
813-
Pext.noalias() -= KCP.transpose();
814-
Pext.noalias() += KSKt;
736+
xext.noalias() += K * inno;
815737

738+
MatrixNX I = MatrixNX::Identity();
739+
Pext = (I - K * Cext) * Pext * (I - K * Cext).transpose() + K * Racc * K.transpose();
816740
Pext = T(0.5) * (Pext + Pext.transpose());
741+
817742
applyQuaternionCorrectionFromErrorState();
818-
Pbase = Pext.topLeftCorner(BASE_N, BASE_N);
819743
}
820744

821745
template<typename T, bool with_gyro_bias, bool with_accel_bias>

0 commit comments

Comments
 (0)