12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/tests/testLIEKF.cpp
View File

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