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

More symmetric templating

release/4.3a0
Frank Dellaert 2025-04-26 12:14:12 -04:00
parent b1c32cb6f1
commit 9352465494
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/LIEKF_NavstateExample.cpp
View File

@ -89,7 +89,7 @@ int main() {
cout << "P0: " << ekf.covariance() << endl; cout << "P0: " << ekf.covariance() << endl;
// First prediction stage // First prediction stage
ekf.predict<6>(dynamics, imu1, dt, Q); ekf.predict(dynamics, imu1, dt, Q);
cout << "\nFirst Prediction:\n"; cout << "\nFirst Prediction:\n";
cout << "X: " << ekf.state() << endl; cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl; cout << "P: " << ekf.covariance() << endl;
@ -101,7 +101,7 @@ int main() {
cout << "P: " << ekf.covariance() << endl; cout << "P: " << ekf.covariance() << endl;
// Second prediction stage // Second prediction stage
ekf.predict<6>(dynamics, imu2, dt, Q); ekf.predict(dynamics, imu2, dt, Q);
cout << "\nSecond Prediction:\n"; cout << "\nSecond Prediction:\n";
cout << "X: " << ekf.state() << endl; cout << "X: " << ekf.state() << endl;
cout << "P: " << ekf.covariance() << endl; cout << "P: " << ekf.covariance() << endl;

24
gtsam/nonlinear/LIEKF.h
View File

@ -84,10 +84,13 @@ class LIEKF {
} }
/** /**
* @brief Prediction stage with a control vector u and a time interval dt. * @brief Prediction stage with a tangent vector xi 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.
*
* @note Use this if your dynamics does not depend on the state, e.g.
* predict(f(u), dt, q) where u is a control input
*/ */
void predict(const Vector& u, double dt, const Matrix& Q) { void predict(const Vector& u, double dt, const Matrix& Q) {
predict(G::Expmap(u * dt), Q); predict(G::Expmap(u * dt), Q);
@ -95,28 +98,27 @@ class LIEKF {
/** /**
* @brief Prediction stage with a dynamics function that calculates the * @brief Prediction stage with a dynamics function that calculates the
* tangent vector xi in the tangent space. * tangent vector xi that *depends on the state*.
* @tparam _p The dimension of the control vector. * @tparam Control The control input type
* @tparam Dynamics : (G, VectorP, OptionalJacobian<n,n>) -> TangentVector * @tparam Dynamics : (G, Control, OptionalJacobian<n,n>) -> TangentVector
* @param f Dynamics function that depends on state and control vector * @param f Dynamics function that depends on state and control input
* @param u Control vector element * @param u Control input
* @param dt Time interval * @param dt Time interval
* @param Q Process noise covariance matrix. * @param Q Process noise covariance matrix.
*/ */
template <size_t _p, typename Dynamics> template <typename Control, typename Dynamics>
void predict(Dynamics&& f, const Eigen::Matrix<double, _p, 1>& u, double dt, void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
const Matrix& Q) {
typename G::Jacobian F; typename G::Jacobian F;
const typename G::TangentVector xi = f(X, u, F); const typename G::TangentVector xi = f(X, u, F);
G U = G::Expmap(xi * dt); G U = G::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * F; auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f
X = X.compose(U); X = X.compose(U);
P = A * P * A.transpose() + Q; P = A * P * A.transpose() + Q;
} }
/** /**
* @brief Update stage using a measurement and measurement covariance. * @brief Update stage using a measurement and measurement covariance.
* @tparam Measurement * @tparam Measurement The measurement output type
* @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement * @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement
* @param z Measurement * @param z Measurement
* @param R Measurement noise covariance matrix. * @param R Measurement noise covariance matrix.