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

Templated predict/update

release/4.3a0
Frank Dellaert 2025-04-26 11:47:46 -04:00
parent 0d832a6599
commit b1c32cb6f1
3 changed files with 51 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
examples/LIEKF_NavstateExample.cpp
View File

@ -11,10 +11,10 @@
/**
* @file LIEKF_NavstateExample.cpp
* @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF
* @brief A left invariant Extended Kalman Filter example using the LIEKF
* on NavState using IMU/GPS measurements.
*
* This example uses the templated GeneralLIEKF class to estimate the state of
* This example uses the templated LIEKF class to estimate the state of
* an object using IMU/GPS measurements. The prediction stage of the LIEKF uses
* a generic dynamics function to predict the state. This simulates a navigation
* state of (pose, velocity, position)
@ -64,16 +64,9 @@ int main() {
Matrix9 P0 = Matrix9::Identity() * 0.1;
double dt = 1.0;
// Create the measurement function h_func that wraps h_gps
GeneralLIEKF<NavState, Vector3, 6>::MeasurementFunction h_func =
[](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); };
// Create the dynamics function dynamics_func
GeneralLIEKF<NavState, Vector3, 6>::Dynamics dynamics_func = dynamics;
// Create the filter with the initial state, covariance, and dynamics and
// measurement functions.
GeneralLIEKF<NavState, Vector3, 6> ekf(X0, P0, dynamics_func, h_func);
LIEKF<NavState> ekf(X0, P0);
// Create the process covariance and measurement covariance matrices Q and R.
Matrix9 Q = Matrix9::Identity() * 0.01;
@ -96,25 +89,25 @@ int main() {
cout << "P0: " << ekf.covariance() << endl;
// First prediction stage
ekf.predict(imu1, dt, Q);
ekf.predict<6>(dynamics, imu1, dt, Q);
cout << "\nFirst Prediction:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl;
// First update stage
ekf.update(z1, R);
ekf.update(h_gps, z1, R);
cout << "\nFirst Update:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl;
// Second prediction stage
ekf.predict(imu2, dt, Q);
ekf.predict<6>(dynamics, imu2, dt, Q);
cout << "\nSecond Prediction:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl;
// Second update stage
ekf.update(z2, R);
ekf.update(h_gps, z2, R);
cout << "\nSecond Update:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl;

10
examples/LIEKF_SE2_SimpleGPSExample.cpp
View File

@ -53,14 +53,10 @@ int main() {
// // Initialize the filter's state, covariance, and time interval values.
Pose2 X0(0.0, 0.0, 0.0);
Matrix3 P0 = Matrix3::Identity() * 0.1;
double dt = 1.0;
// Create the function measurement_function that wraps h_gps.
std::function<Vector2(const Pose2&, gtsam::OptionalJacobian<2, 3>)>
measurement_function = h_gps;
// Create the filter with the initial state, covariance, and measurement
// function.
LIEKF<Pose2, Vector2> ekf(X0, P0, measurement_function);
LIEKF<Pose2> ekf(X0, P0);
// Define the process covariance and measurement covariance matrices Q and R.
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
@ -100,7 +96,7 @@ int main() {
<< endl;
// First update stage
ekf.update(z1, R);
ekf.update(h_gps, z1, R);
cout << "\nFirst Update:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
@ -114,7 +110,7 @@ int main() {
<< endl;
// Second update stage
ekf.update(z2, R);
ekf.update(h_gps, z2, R);
cout << "\nSecond Update:\n";
cout << "X: " << ekf.state() << endl;
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()

128
gtsam/nonlinear/LIEKF.h
View File

@ -38,23 +38,17 @@ namespace gtsam {
* 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,
* @tparam G 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 G>
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.
static constexpr int n = traits<G>::dimension; ///< Dimension of the state.
using MeasurementFunction = std::function<Measurement(
const LieGroup&,
OptionalJacobian<m, n>)>; ///< Typedef for the measurement function.
using MatrixN =
Eigen::Matrix<double, n, n>; ///< Typedef for the identity matrix.
@ -64,14 +58,13 @@ class LIEKF {
* @param P0 Initial Covariance
* @param h Measurement function
*/
LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h) // X_ P_
: X(X0), P(P0), h_(h) {}
LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {}
/**
* @brief Get current state estimate.
* @return Const reference to the state estiamte.
* @return Const reference to the state estimate.
*/
const LieGroup& state() const { return X; }
const G& state() const { return X; }
/**
* @brief Get current covariance estimate.
@ -84,8 +77,8 @@ class LIEKF {
* @param U Lie group control input
* @param Q Process noise covariance matrix.
*/
void predict(const LieGroup& U, const Matrix& Q) {
LieGroup::Jacobian A;
void predict(const G& U, const Matrix& Q) {
typename G::Jacobian A;
X = X.compose(U, A);
P = A * P * A.transpose() + Q;
}
@ -97,17 +90,41 @@ class LIEKF {
* @param Q Process noise covariance matrix.
*/
void predict(const Vector& u, double dt, const Matrix& Q) {
predict(LieGroup::Expmap(u * dt), Q);
predict(G::Expmap(u * dt), Q);
}
/**
* @brief Prediction stage with a dynamics function that calculates the
* tangent vector xi in the tangent space.
* @tparam _p The dimension of the control vector.
* @tparam Dynamics : (G, VectorP, OptionalJacobian<n,n>) -> TangentVector
* @param f Dynamics function that depends on state and control vector
* @param u Control vector element
* @param dt Time interval
* @param Q Process noise covariance matrix.
*/
template <size_t _p, typename Dynamics>
void predict(Dynamics&& f, const Eigen::Matrix<double, _p, 1>& u, double dt,
const Matrix& Q) {
typename G::Jacobian F;
const typename G::TangentVector xi = f(X, u, F);
G U = G::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * F;
X = X.compose(U);
P = A * P * A.transpose() + Q;
}
/**
* @brief Update stage using a measurement and measurement covariance.
* @tparam Measurement
* @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement
* @param z Measurement
* @param R Measurement noise covariance matrix.
*/
void update(const Measurement& z, const Matrix& R) {
Matrix H(m, n);
Vector y = h_(X, H) - z;
template <typename Measurement, typename Prediction>
void update(Prediction&& h, const Measurement& z, const Matrix& R) {
Eigen::Matrix<double, traits<Measurement>::dimension, n> H;
Vector y = h(X, H) - z;
Matrix S = H * P * H.transpose() + R;
Matrix K = P * H.transpose() * S.inverse();
X = X.expmap(-K * y);
@ -115,79 +132,16 @@ class LIEKF {
}
protected:
LieGroup X; ///< Current state estimate.
Matrix P; ///< Current covariance estimate.
G X; ///< Current state 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.
};
/**
* @brief Create the static identity matrix I_n of size nxn for use in the
* update stage.
* @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>
const typename LIEKF<LieGroup, Measurement>::MatrixN
LIEKF<LieGroup, Measurement>::I_n =
typename LIEKF<LieGroup, Measurement>::MatrixN::Identity();
/**
* @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 a state and control vector to a tangent vector xi.
*
* @tparam LieGroup The Lie group type for the state.
* @tparam Measurement The type of the measurement.
* @tparam _p The dimension of the control vector.
*/
template <typename LieGroup, typename Measurement, size_t _p>
class GeneralLIEKF : public LIEKF<LieGroup, Measurement> {
public:
using Control =
Eigen::Matrix<double, _p, 1>; ///< Typedef for the control vector.
using TangentVector =
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
* @param X0 Initial State
* @param P0 Initial Covariance
* @param f Dynamics function that depends on state and control vector
* @param h Measurement function
*/
GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& 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.
* @param u Control vector element
* @param dt Time interval
* @param Q Process noise covariance matrix.
*/
void predict(const Control& u, double dt, const Matrix& Q) {
LieGroup::Jacobian H;
const TangentVector xi = f_(X, u, H);
LieGroup U = LieGroup::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * H;
X = X.compose(U);
P = A * P * A.transpose() + Q;
}
private:
Dynamics f_; ///< Dynamics function.
};
/// Create the static identity matrix I_n of size nxn for use in update
template <typename G>
const typename LIEKF<G>::MatrixN LIEKF<G>::I_n = LIEKF<G>::MatrixN::Identity();
} // namespace gtsam