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

Review comments

release/4.3a0
Frank Dellaert 2025-04-27 13:35:24 -04:00
parent 062bdf64ea
commit 1ced4d0470
3 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/LIEKF_NavstateExample.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
* @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity.
* @return 9×1 tangent: [ω; 0₃; a]. * @return 9×1 tangent: [ω; 0₃; a].
*/ */
Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) { Vector9 dynamics(const Vector6& imu) {
auto a = imu.head<3>(); auto a = imu.head<3>();
auto w = imu.tail<3>(); auto w = imu.tail<3>();
Vector9 xi; Vector9 xi;

12
examples/LIEKF_Rot3Example.cpp
View File

@ -32,14 +32,14 @@ using namespace gtsam;
static constexpr double k = 0.5; static constexpr double k = 0.5;
Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) { Vector3 dynamicsSO3(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)
} }
// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---

2
examples/LIEKF_SE2_SimpleGPSExample.cpp
View File

@ -58,7 +58,7 @@ int main() {
// Define the process covariance and measurement covariance matrices Q and R. // Define the process covariance and measurement covariance matrices Q and R.
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); Matrix2 R = I_2x2 * 0.01;
// Define odometry movements. // Define odometry movements.
// U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta. // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.