Added Rot3 example to show state-dependent control
Frank Dellaert
parent
5af25dc30f
commit
062bdf64ea
|
|
@ -11,16 +11,15 @@
|
|||
|
||||
/**
|
||||
* @file LIEKF_NavstateExample.cpp
|
||||
* @brief Example of a Left-Invariant Extended Kalman Filter on NavState
|
||||
* using IMU (predict) and GPS (update) measurements.
|
||||
* @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
|
||||
* @date April 25, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
#include <gtsam/navigation/LIEKF.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -29,19 +28,14 @@ using namespace gtsam;
|
|||
|
||||
/**
|
||||
* @brief Left-invariant dynamics for NavState.
|
||||
* @param X Current state (unused for left-invariant error dynamics).
|
||||
* @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last
|
||||
* 3).
|
||||
* @param H Optional 9×9 Jacobian w.r.t. X (always zero here).
|
||||
* @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity.
|
||||
* @return 9×1 tangent: [ω; 0₃; a].
|
||||
*/
|
||||
Vector9 dynamics(const NavState& X, const Vector6& imu,
|
||||
OptionalJacobian<9, 9> H = {}) {
|
||||
Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) {
|
||||
auto a = imu.head<3>();
|
||||
auto w = imu.tail<3>();
|
||||
Vector9 xi;
|
||||
xi << w, Vector3::Zero(), a;
|
||||
if (H) *H = Matrix9::Zero();
|
||||
return xi;
|
||||
}
|
||||
|
||||
|
|
@ -52,60 +46,51 @@ Vector9 dynamics(const NavState& X, const Vector6& imu,
|
|||
* @return 3×1 position vector.
|
||||
*/
|
||||
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
|
||||
if (H) {
|
||||
// H = [ 0₃×3, 0₃×3, R ]
|
||||
*H << Z_3x3, Z_3x3, X.R();
|
||||
}
|
||||
if (H) *H << Z_3x3, Z_3x3, X.R().matrix();
|
||||
return X.t();
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Initial state, covariance, and time step
|
||||
NavState X0;
|
||||
// Initial state & covariances
|
||||
NavState X0; // R=I, v=0, t=0
|
||||
Matrix9 P0 = Matrix9::Identity() * 0.1;
|
||||
double dt = 1.0;
|
||||
|
||||
// Create the filter with the initial state and covariance.
|
||||
LIEKF<NavState> ekf(X0, P0);
|
||||
|
||||
// Process & measurement noise
|
||||
// Noise & timestep
|
||||
double dt = 1.0;
|
||||
Matrix9 Q = Matrix9::Identity() * 0.01;
|
||||
Matrix3 R = Matrix3::Identity() * 0.5;
|
||||
|
||||
// Create the IMU measurements of the form (linear_acceleration,
|
||||
// angular_velocity)
|
||||
Vector6 imu1, imu2;
|
||||
imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0;
|
||||
imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0;
|
||||
// Two IMU samples [a; ω]
|
||||
Vector6 imu1;
|
||||
imu1 << 0.1, 0, 0, 0, 0.2, 0;
|
||||
Vector6 imu2;
|
||||
imu2 << 0, 0.3, 0, 0.4, 0, 0;
|
||||
|
||||
// Create the GPS measurements of the form (px, py, pz)
|
||||
Vector3 z1, z2;
|
||||
z1 << 0.3, 0.0, 0.0;
|
||||
z2 << 0.6, 0.0, 0.0;
|
||||
// Two GPS fixes
|
||||
Vector3 z1;
|
||||
z1 << 0.3, 0, 0;
|
||||
Vector3 z2;
|
||||
z2 << 0.6, 0, 0;
|
||||
|
||||
cout << "=== Initialization ===\n"
|
||||
<< "X0: " << ekf.state() << "\n"
|
||||
<< "P0: " << ekf.covariance() << "\n\n";
|
||||
|
||||
ekf.predict(dynamics, imu1, dt, Q);
|
||||
cout << "--- After first predict ---\n"
|
||||
<< "X: " << ekf.state() << "\n"
|
||||
<< "P: " << ekf.covariance() << "\n\n";
|
||||
cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
|
||||
<< "\n\n";
|
||||
|
||||
// --- first predict/update ---
|
||||
ekf.predict(dynamics(imu1), dt, Q);
|
||||
cout << "--- After predict 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z1, R);
|
||||
cout << "--- After first update ---\n"
|
||||
<< "X: " << ekf.state() << "\n"
|
||||
<< "P: " << ekf.covariance() << "\n\n";
|
||||
|
||||
ekf.predict(dynamics, imu2, dt, Q);
|
||||
cout << "--- After second predict ---\n"
|
||||
<< "X: " << ekf.state() << "\n"
|
||||
<< "P: " << ekf.covariance() << "\n\n";
|
||||
cout << "--- After update 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
|
||||
// --- second predict/update ---
|
||||
ekf.predict(dynamics(imu2), dt, Q);
|
||||
cout << "--- After predict 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z2, R);
|
||||
cout << "--- After second update ---\n"
|
||||
<< "X: " << ekf.state() << "\n"
|
||||
<< "P: " << ekf.covariance() << "\n";
|
||||
cout << "--- After update 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LIEKF_Rot3Example.cpp
|
||||
* @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
|
||||
* and a single magnetometer update.
|
||||
* @date April 25, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/navigation/LIEKF.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
|
||||
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φ);
|
||||
// zero out yaw
|
||||
φ[2] = 0.0;
|
||||
Dφ.row(2).setZero();
|
||||
|
||||
if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR
|
||||
return -k * φ; // xi ∈ 𝔰𝔬(3)
|
||||
}
|
||||
|
||||
// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
|
||||
static const Vector3 m_world(0, 0, -1);
|
||||
Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
|
||||
Vector3 z = X.inverse().rotate(m_world);
|
||||
if (H) *H = -skewSymmetric(z);
|
||||
return z;
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Initial estimate (identity) and covariance
|
||||
const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
|
||||
const Matrix3 P0 = Matrix3::Identity() * 0.1;
|
||||
LIEKF<Rot3> ekf(R0, P0);
|
||||
|
||||
// Timestep, process noise, measurement noise
|
||||
double dt = 0.1;
|
||||
Matrix3 Q = Matrix3::Identity() * 0.01;
|
||||
Matrix3 Rm = Matrix3::Identity() * 0.05;
|
||||
|
||||
cout << "=== Init ===\nR:\n"
|
||||
<< ekf.state().matrix() << "\nP:\n"
|
||||
<< ekf.covariance() << "\n\n";
|
||||
|
||||
// Predict using state‐dependent f
|
||||
ekf.predict(dynamicsSO3, dt, Q);
|
||||
cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
|
||||
|
||||
// Magnetometer measurement = body‐frame reading of m_world
|
||||
Vector3 z = h_mag(R0);
|
||||
ekf.update(h_mag, z, Rm);
|
||||
cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -98,6 +98,28 @@ class LIEKF {
|
|||
|
||||
/**
|
||||
* Predict step with state-dependent dynamics:
|
||||
* xi = f(X, F)
|
||||
* U = Expmap(xi * dt)
|
||||
* A = Ad_{U^{-1}} * F
|
||||
*
|
||||
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
|
||||
*
|
||||
* @param f dynamics functor depending on state and control
|
||||
* @param dt time step
|
||||
* @param Q process noise covariance
|
||||
*/
|
||||
template <typename Dynamics>
|
||||
void predict(Dynamics&& f, double dt, const Matrix& Q) {
|
||||
typename G::Jacobian F;
|
||||
auto xi = f(X_, F);
|
||||
G U = G::Expmap(xi * dt);
|
||||
auto A = U.inverse().AdjointMap() * F;
|
||||
X_ = X_.compose(U);
|
||||
P_ = A * P_ * A.transpose() + Q;
|
||||
}
|
||||
|
||||
/**
|
||||
* Predict step with state and control input dynamics:
|
||||
* xi = f(X, u, F)
|
||||
* U = Expmap(xi * dt)
|
||||
* A = Ad_{U^{-1}} * F
|
||||
|
|
|
|||
|
|
@ -21,35 +21,33 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
// Duplicate the dynamics function under test:
|
||||
// Duplicate the dynamics function in LIEKF_Rot3Example
|
||||
namespace example {
|
||||
Vector9 dynamics(const NavState& X, const Vector6& imu,
|
||||
OptionalJacobian<9, 9> H = {}) {
|
||||
auto a = imu.head<3>();
|
||||
auto w = imu.tail<3>();
|
||||
Vector9 xi;
|
||||
xi << w, Vector3::Zero(), a;
|
||||
if (H) *H = Matrix9::Zero();
|
||||
return xi;
|
||||
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φ);
|
||||
// zero out yaw
|
||||
φ[2] = 0.0;
|
||||
Dφ.row(2).setZero();
|
||||
|
||||
if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR
|
||||
return -k * φ; // xi ∈ 𝔰𝔬(3)
|
||||
}
|
||||
} // namespace example
|
||||
|
||||
TEST(LIEKFNavState, dynamicsJacobian) {
|
||||
// Construct a nontrivial state and IMU input
|
||||
NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0),
|
||||
Vector3(0.5, -0.5, 0.5));
|
||||
Vector6 imu;
|
||||
imu << 0.1, -0.1, 0.2, // acceleration
|
||||
0.01, -0.02, 0.03; // angular velocity
|
||||
Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3);
|
||||
|
||||
// Analytic Jacobian (always zero for left-invariant dynamics)
|
||||
OptionalJacobian<9, 9> H_analytic;
|
||||
example::dynamics(X, imu, H_analytic);
|
||||
Matrix actualH = *H_analytic;
|
||||
Matrix3 actualH;
|
||||
example::dynamics(R, actualH);
|
||||
|
||||
// Numeric Jacobian w.r.t. the state X
|
||||
auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); };
|
||||
Matrix expectedH = numericalDerivative11<Vector9, NavState>(f, X, 1e-6);
|
||||
auto f = [&](const Rot3& X_) { return example::dynamics(X_); };
|
||||
Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R, 1e-6);
|
||||
|
||||
// Compare
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
|
|
|
|||
Loading…
Reference in New Issue