diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 250d5cda8..9798dca02 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -31,7 +31,7 @@ using namespace gtsam; * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @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 w = imu.tail<3>(); Vector9 xi; diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp index 331c01bb1..172af894d 100644 --- a/examples/LIEKF_Rot3Example.cpp +++ b/examples/LIEKF_Rot3Example.cpp @@ -32,14 +32,14 @@ using namespace gtsam; static constexpr double k = 0.5; Vector3 dynamicsSO3(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) } // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index ecef67cf7..0985d2372 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -58,7 +58,7 @@ int main() { // Define the process covariance and measurement covariance matrices Q and R. 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. // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.