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

clang-formatted, changed getFoobar() to foobar()

release/4.3a0
scottiyio 2025-04-25 14:43:19 -04:00 committed by GitHub
parent 136a7cdfd0
commit 12d422341d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 104 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
gtsam/nonlinear/LIEKF.h
View File

@ -13,7 +13,8 @@
* @file LIEKF.h * @file LIEKF.h
* @brief Base and classes for Left Invariant Extended Kalman Filters * @brief Base and classes for Left Invariant Extended Kalman Filters
* *
* Templates are implemented for a Left Invariant Extended Kalman Filter operating on Lie Groups. * Templates are implemented for a Left Invariant Extended Kalman Filter
* operating on Lie Groups.
* *
* *
* @date April 24, 2025 * @date April 24, 2025
@ -22,59 +23,66 @@
*/ */
#pragma once #pragma once
#include <functional>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <functional>
namespace gtsam { namespace gtsam {
/** /**
* @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF)
* *
* This class provides the prediction and update structure based on control inputs and a measurement function. * This class provides the prediction and update structure based on control
* * inputs and a measurement function.
* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) *
* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) * @tparam LieGroup Lie group used for state representation (e.g., Pose2,
*/ * Pose3, NavState)
* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement
* for 3D position)
*/
template <typename LieGroup, typename Measurement> template <typename LieGroup, typename Measurement>
class LIEKF { class LIEKF {
public:
static constexpr int n =
traits<LieGroup>::dimension; ///< Dimension of the state.
static constexpr int m =
traits<Measurement>::dimension; ///< Dimension of the measurement.
public: using MeasurementFunction = std::function<Measurement(
static constexpr int n = traits<LieGroup>::dimension; ///< Dimension of the state. const LieGroup&,
static constexpr int m = traits<Measurement>::dimension; ///< Dimension of the measurement. OptionalJacobian<m, n>)>; ///< Typedef for the measurement function.
using MatrixN =
using MeasurementFunction = std::function<Measurement(const LieGroup&, OptionalJacobian<m, n>)>; ///< Typedef for the measurement function. Eigen::Matrix<double, n, n>; ///< Typedef for the identity matrix.
using MatrixN = Eigen::Matrix<double, n, n>; ///< Typedef for the identity matrix.
/** /**
* @brief Construct with a measurement function * @brief Construct with a measurement function
* @param X0 Initial State * @param X0 Initial State
* @param P0 Initial Covariance * @param P0 Initial Covariance
* @param h Measurement function * @param h Measurement function
*/ */
LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_ LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_
: X(X0), P(P0), h_(h) {} : X(X0), P(P0), h_(h) {}
/**
* @brief Get current state estimate.
* @return Const reference to the state estiamte.
*/
const LieGroup& state() const { return X; }
/** /**
* @brief Get current state estimate. * @brief Get current covariance estimate.
* @return Const reference to the state estiamte. * @return Const reference to the covariance estimate.
*/ */
const LieGroup& getState() const { return X; } const Matrix& covariance() const { return P; }
/** /**
* @brief Get current covariance estimate. * @brief Prediction stage with a Lie group element U.
* @return Const reference to the covariance estimate. * @param U Lie group control input
*/ * @param Q Process noise covariance matrix.
const Matrix& getCovariance() const { return P; } */
/**
* @brief Prediction stage with a Lie group element U.
* @param U Lie group control input
* @param Q Process noise covariance matrix.
*/
void predict(const LieGroup& U, const Matrix& Q) { void predict(const LieGroup& U, const Matrix& Q) {
LieGroup::Jacobian A; LieGroup::Jacobian A;
X = X.compose(U, A); X = X.compose(U, A);
@ -82,83 +90,92 @@ public:
} }
/** /**
* @brief Prediction stage with a control vector u and a time interval dt. * @brief Prediction stage with a control vector u and a time interval dt.
* @param u Control vector element * @param u Control vector element
* @param dt Time interval * @param dt Time interval
* @param Q Process noise covariance matrix. * @param Q Process noise covariance matrix.
*/ */
void predict(const Vector& u, double dt, const Matrix& Q) { void predict(const Vector& u, double dt, const Matrix& Q) {
predict(LieGroup::Expmap(u*dt), Q);d predict(LieGroup::Expmap(u * dt), Q);
} }
/** /**
* @brief Update stage using a measurement and measurement covariance. * @brief Update stage using a measurement and measurement covariance.
* @param z Measurement * @param z Measurement
* @param R Measurement noise covariance matrix. * @param R Measurement noise covariance matrix.
*/ */
void update(const Measurement& z, const Matrix& R) { void update(const Measurement& z, const Matrix& R) {
Matrix H(m, n); Matrix H(m, n);
Vector y = h_(X, H)-z; Vector y = h_(X, H) - z;
Matrix S = H * P * H.transpose() + R; Matrix S = H * P * H.transpose() + R;
Matrix K = P * H.transpose() * S.inverse(); Matrix K = P * H.transpose() * S.inverse();
X = X.expmap(-K * y); X = X.expmap(-K * y);
P = (I_n - K * H) * P; // move Identity to be a constant. P = (I_n - K * H) * P; // move Identity to be a constant.
} }
protected: protected:
LieGroup X; ///< Current state estimate. LieGroup X; ///< Current state estimate.
Matrix P; ///< Current covariance estimate. Matrix P; ///< Current covariance estimate.
private:
MeasurementFunction h_; ///< Measurement function
static const MatrixN I_n; ///< A nxn identity matrix used in the update stage of the LIEKF.
private:
MeasurementFunction h_; ///< Measurement function
static const MatrixN
I_n; ///< A nxn identity matrix used in the update stage of the LIEKF.
}; };
/** /**
* @brief Create the static identity matrix I_n of size nxn for use in the update stage. * @brief Create the static identity matrix I_n of size nxn for use in the
* @tparam LieGroup Lie group used for state representation (e.g., Pose2, Pose3, NavState) * update stage.
* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement for 3D position) * @tparam LieGroup Lie group used for state representation (e.g., Pose2,
*/ * Pose3, NavState)
* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement
* for 3D position)
*/
template <typename LieGroup, typename Measurement> template <typename LieGroup, typename Measurement>
const typename LIEKF<LieGroup, Measurement>::MatrixN LIEKF<LieGroup, Measurement>::I_n const typename LIEKF<LieGroup, Measurement>::MatrixN
= typename LIEKF<LieGroup, Measurement>::MatrixN::Identity(); LIEKF<LieGroup, Measurement>::I_n =
typename LIEKF<LieGroup, Measurement>::MatrixN::Identity();
/** /**
* @brief General Left Invariant Extended Kalman Filter with dynamics function. * @brief General Left Invariant Extended Kalman Filter with dynamics function.
* *
* This class extends the LIEKF class to include a dynamics function f. The dynamics maps * This class extends the LIEKF class to include a dynamics function f. The
* a state and control vector to a tangent vector xi. * dynamics maps a state and control vector to a tangent vector xi.
* *
* @tparam LieGroup The Lie group type for the state. * @tparam LieGroup The Lie group type for the state.
* @tparam Measurement The type of the measurement. * @tparam Measurement The type of the measurement.
* @tparam _p The dimension of the control vector. * @tparam _p The dimension of the control vector.
*/ */
template <typename LieGroup, typename Measurement, size_t _p> template <typename LieGroup, typename Measurement, size_t _p>
class GeneralLIEKF:public LIEKF<LieGroup, Measurement> { class GeneralLIEKF : public LIEKF<LieGroup, Measurement> {
public: public:
using Control = Eigen::Matrix<double, _p, 1>; ///< Typedef for the control vector. using Control =
using TangentVector = typename traits<LieGroup>::TangentVector; ///< Typedef for the tangent vector. Eigen::Matrix<double, _p, 1>; ///< Typedef for the control vector.
using Dynamics = std::function<TangentVector(const LieGroup&, const Control&, ///< Typedef for the dynamics function. using TangentVector =
OptionalJacobian<n, n>)>; typename traits<LieGroup>::TangentVector; ///< Typedef for the tangent
///< vector.
using Dynamics = std::function<TangentVector(
const LieGroup&, const Control&, ///< Typedef for the dynamics function.
OptionalJacobian<n, n>)>;
/** /**
* @brief Construct with general dynamics * @brief Construct with general dynamics
* @param X0 Initial State * @param X0 Initial State
* @param P0 Initial Covariance * @param P0 Initial Covariance
* @param f Dynamics function that depends on state and control vector * @param f Dynamics function that depends on state and control vector
* @param h Measurement function * @param h Measurement function
*/ */
GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f, MeasurementFunction& GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f,
h) : LIEKF(X0, P0, h), f_(f) {} MeasurementFunction& h)
: LIEKF(X0, P0, h), f_(f) {}
/** /**
* @brief Prediction stage with a dynamics function that calculates the tangent vector xi in the tangent space. * @brief Prediction stage with a dynamics function that calculates the
* @param u Control vector element * tangent vector xi in the tangent space.
* @param dt Time interval * @param u Control vector element
* @param Q Process noise covariance matrix. * @param dt Time interval
*/ * @param Q Process noise covariance matrix.
*/
void predict(const Control& u, double dt, const Matrix& Q) { void predict(const Control& u, double dt, const Matrix& Q) {
LieGroup::Jacobian H; LieGroup::Jacobian H;
const TangentVector xi = f_(X, u, H); const TangentVector xi = f_(X, u, H);
@ -167,10 +184,9 @@ h) : LIEKF(X0, P0, h), f_(f) {}
X = X.compose(U); X = X.compose(U);
P = A * P * A.transpose() + Q; P = A * P * A.transpose() + Q;
} }
private:
Dynamics f_; ///< Dynamics function. private:
Dynamics f_; ///< Dynamics function.
}; };
}// ends namespace } // namespace gtsam