Update testLIEKF.cpp

release/4.3a0
Frank Dellaert 2025-04-27 17:59:34 -04:00 committed by GitHub
parent a0c6902c8e
commit 4f2a62aa3a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 6 additions and 6 deletions

View File

@ -26,14 +26,14 @@ namespace example {
static constexpr double k = 0.5;
Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
// φ = Logmap(R), Dφ = ∂φ/∂δR
Matrix3 Dφ;
Vector3 φ = Rot3::Logmap(X, Dφ);
Matrix3 D_phi;
Vector3 phi = Rot3::Logmap(X, D_phi);
// zero out yaw
φ[2] = 0.0;
Dφ.row(2).setZero();
phi[2] = 0.0;
D_phi.row(2).setZero();
if (H) *H = -k * Dφ; // ∂(kφ)/∂δR
return -k * φ; // xi ∈ 𝔰𝔬(3)
if (H) *H = -k * D_phi; // ∂(kφ)/∂δR
return -k * phi; // xi ∈ 𝔰𝔬(3)
}
} // namespace example