diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp new file mode 100644 index 000000000..ae6396be2 --- /dev/null +++ b/examples/LIEKF_SE2_SimpleGPSExample.cpp @@ -0,0 +1,74 @@ +// +// Created by Scott on 4/18/2025. +// +#include +#include +#include + +using namespace std; +using namespace gtsam; + + // Measurement Processor + Vector2 h_gps(const Pose2& X, + OptionalJacobian<2,3> H = {}) { + return X.translation(H); + } + int main() { + static const int dim = traits::dimension; + + // Initialization + Pose2 X0(0.0, 0.0, 0.0); + Matrix3 P0 = Matrix3::Identity() * 0.1; + double dt = 1.0; + + // Define GPS measurements + Matrix23 H; + h_gps(X0, H); + Vector2 z1, z2; + z1 << 1.0, 0.0; + z2 << 1.0, 1.0; + + std::function)> measurement_function = h_gps; + LIEKF ekf(X0, P0, measurement_function); + + // Define Covariances + Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); + Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal(); + + // Define odometry movements + Pose2 U1(1.0,1.0,0.5), U2(1.0,1.0,0.0); + + // Define a transformation matrix to convert the covariance into (theta, x, y) form. + Matrix3 TransformP; + TransformP << 0, 0, 1, + 1,0,0, + 0,1,0; + + // Predict / update stages + cout << "\nInitialization:\n"; + cout << "X0: " << ekf.getState() << endl; + cout << "P0: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + + ekf.predict(U1, Q); + cout << "\nFirst Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z1, R); + cout << "\nFirst Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.predict(U2, Q); + cout << "\nSecond Prediction:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + ekf.update(z2, R); + cout << "\nSecond Update:\n"; + cout << "X: " << ekf.getState() << endl; + cout << "P: " << TransformP * ekf.getCovariance() * TransformP.transpose() << endl; + + return 0; + } \ No newline at end of file