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

Delete examples/SE2LIEKF_NoFactors.cpp

release/4.3a0
scottiyio 2025-04-24 22:06:46 -04:00 committed by GitHub
parent c521dbc459
commit 136a7cdfd0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 0 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
examples/SE2LIEKF_NoFactors.cpp
View File

@ -1,124 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SE2LIEKF_NoFactors.cpp
*
* A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements.
* No factors are used here.
* This data was compared to a left invariant EKF on SE(2) using identical measurements and noise from the source of the
* InEKF plugin https://inekf.readthedocs.io/en/latest/
* Based on the paper "An Introduction to the Invariant Extended Kalman Filter" by
* Easton R. Potokar, Randal W. Beard, and Joshua G. Mangelson
* @date April 1, 2025
* @author Scott Baker
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <Eigen/Dense>
#include <gtsam/base/Matrix.h>
using namespace std;
using namespace gtsam;
#define PI 3.14159265358
// Create an adjoint class
// Function for the prediction stage.
// Note that U is the exponential map of some control vector (u*dt)
void predict(Pose2& X, Matrix& P, Vector3& u, Matrix& Q)
{
//Pose2 U = Pose2::Expmap(u*dt);
Pose2 U(u(0), u(1), u(2));
Matrix Adj = U.inverse().AdjointMap();
X = X.compose(U);
P = Adj * P * Adj.transpose() + Q;
}
// Update stage that uses GPS measurements in a left invariant update way.
void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R)
{
// find residual R^T * (z - zhat)
Point2 zhat = X.translation();
Point2 e_world = z-zhat;
Point2 residual = X.rotation().unrotate(e_world);
Matrix S = H*P*H.transpose() + R;
Matrix K = P*H.transpose()*S.inverse();
Vector3 delta = K*residual;
X = X * Pose2::Expmap(delta);
P = (Matrix3::Identity() - K*H) * P;
}
int main() {
// Define movements and measurements
const double dt = 1.0;
Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry
Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps
// Set up noise models (uses simple GPS)
// Odometry Process
Vector3 pModel(0.05, 0.05, 0.001); // covariance of the odometry process (rad)
Matrix Q = pModel.asDiagonal(); // Q covariance matrix
// GPS process
Vector2 rModel(0.01, 0.01);
Matrix R = rModel.asDiagonal();
Matrix H(2,3);
H << 1,0,0,
0,1,0;
// A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure.
Matrix3 TransformP;
TransformP << 0, 0, 1,
1,0,0,
0,1,0;
// Initialize
Pose2 X(0, 0, 0);
Matrix P = Matrix3::Identity()*0.1;
cout << "Initial X\n" << X << endl;
cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl;
// First Prediction
predict(X, P, u1, dt, Q);
cout << "Predicted X1\n" << X << endl;
cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl;
// First Update
update(X, P, z1, H, R);
cout << "Updated X1\n" << X << endl;
cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl;
// Second Prediction
predict(X, P, u2, dt, Q);
cout << "Predicted X2\n" << X << endl;
cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl;
// Second Update
update(X, P, z2, H, R);
cout << "Updated X2\n" << X << endl;
cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl;
return 0;
}