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

predictMean for state=control f

release/4.3a0
Frank Dellaert 2025-04-27 21:01:12 -04:00
parent 104341f4cf
commit 87c4445f53
2 changed files with 78 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/navigation/LIEKF.h
View File

@ -96,53 +96,51 @@ class LIEKF {
predict(G::Expmap(u * dt), Q); predict(G::Expmap(u * dt), Q);
} }
/**
* SFINAE check for correctly typed state-dependent dynamics function.
* xi = f(X, F)
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
*/
template <typename Dynamics>
using enable_if_dynamics = std::enable_if_t<
!std::is_convertible_v<Dynamics, typename G::TangentVector> &&
std::is_invocable_r_v<typename G::TangentVector, Dynamics, const G&,
OptionalJacobian<n, n>&>>;
/** /**
* Predict mean only with state-dependent dynamics: * Predict mean only with state-dependent dynamics:
* xi = f(X, F) * xi = f(X, F)
* U = Expmap(xi * dt) * X_.expmap(xi * dt)
* A = Ad_{U^{-1}} * F
* *
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&) * @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
*
* @param f dynamics functor depending on state and control * @param f dynamics functor depending on state and control
* @param dt time step * @param dt time step
* @param Q process noise covariance * @param Q process noise covariance
*/ */
template <typename Dynamics, template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
typename = std::enable_if_t<
// not a plain vector…
!std::is_convertible_v<Dynamics, typename G::TangentVector>
// …and is callable as a true functor
&& std::is_invocable_r_v<typename G::TangentVector, Dynamics,
const G&, OptionalJacobian<n, n>>>>
G predictMean(Dynamics&& f, double dt, const Matrix& Q, G predictMean(Dynamics&& f, double dt, const Matrix& Q,
OptionalJacobian<n, n> A = {}) const { OptionalJacobian<n, n> A = {}) const {
typename G::Jacobian F1, F2; typename G::Jacobian Df, Dexp;
typename G::TangentVector xi = f(X_, F1); typename G::TangentVector xi = f(X_, Df);
G U = G::Expmap(xi * dt, F2); G U = G::Expmap(xi * dt, Dexp);
if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt; // derivative of mean = d[X.compose(U)]/dLog(X)
// = Ad_{U⁻¹} (the “left‐invariant” part)
// plus the effect of X→U via Expmap:
// Dexp * Df * dt (how U moves when X moves through f)
if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt;
return X_.compose(U); return X_.compose(U);
} }
/** /**
* Predict step with state-dependent dynamics: * Predict step with state-dependent dynamics:
* xi = f(X, F) * xi = f(X, F)
* U = Expmap(xi * dt) * X_.expmap(xi * dt)
* A = Ad_{U^{-1}} * F
* *
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&) * @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
*
* @param f dynamics functor depending on state and control * @param f dynamics functor depending on state and control
* @param dt time step * @param dt time step
* @param Q process noise covariance * @param Q process noise covariance
*/ */
template <typename Dynamics, template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
typename = std::enable_if_t<
// not a plain vector…
!std::is_convertible_v<Dynamics, typename G::TangentVector>
// …and is callable as a true functor
&& std::is_invocable_r_v<typename G::TangentVector, Dynamics,
const G&, OptionalJacobian<n, n>>>>
void predict(Dynamics&& f, double dt, const Matrix& Q) { void predict(Dynamics&& f, double dt, const Matrix& Q) {
typename G::Jacobian A; typename G::Jacobian A;
X_ = predictMean(f, dt, Q, &A); X_ = predictMean(f, dt, Q, &A);
@ -150,10 +148,9 @@ class LIEKF {
} }
/** /**
* Predict step with state and control input dynamics: * Predict mean only with state and control input dynamics:
* xi = f(X, u, F) * xi = f(X, u, F)
* U = Expmap(xi * dt) * X_.expmap(xi * dt)
* A = Ad_{U^{-1}} * F
* *
* @tparam Control control input type * @tparam Control control input type
* @tparam Dynamics signature: G f(const G&, const Control&, * @tparam Dynamics signature: G f(const G&, const Control&,
@ -164,14 +161,30 @@ class LIEKF {
* @param dt time step * @param dt time step
* @param Q process noise covariance * @param Q process noise covariance
*/ */
template <typename Control, typename Dynamics, template <typename Control, typename Dynamics>
typename = std::enable_if_t< G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q,
std::is_invocable_r_v<typename G::TangentVector, // return type OptionalJacobian<n, n> A = {}) {
Dynamics, // functor auto g = [f, u](const G& X, OptionalJacobian<n, n> F) {
const G&, // X return f(X, u, F);
const Control&, // u };
OptionalJacobian<n, n> // H return predictMean(g, dt, Q, A);
>>> }
/**
* Predict step with state and control input dynamics:
* xi = f(X, u, F)
* X_.expmap(xi * dt)
*
* @tparam Control control input type
* @tparam Dynamics signature: G f(const G&, const Control&,
* OptionalJacobian<n,n>&)
*
* @param f dynamics functor depending on state and control
* @param u control input
* @param dt time step
* @param Q process noise covariance
*/
template <typename Control, typename Dynamics>
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
auto g = [f, u](const G& X, OptionalJacobian<n, n> F) { auto g = [f, u](const G& X, OptionalJacobian<n, n> F) {
return f(X, u, F); return f(X, u, F);

30
gtsam/navigation/tests/testLIEKF.cpp
View File

@ -79,6 +79,36 @@ TEST(IEKF, PredictNumericState) {
EXPECT(assert_equal(expectedH, actualH)); EXPECT(assert_equal(expectedH, actualH));
} }
TEST(IEKF, StateAndControl) {
auto f = [](const Rot3& X, const Vector2& dummy_u,
OptionalJacobian<3, 3> H = {}) {
return exampleSO3::dynamics(X, H);
};
// GIVEN
Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
Matrix3 P0 = Matrix3::Identity() * 0.2;
Vector2 dummy_u(1, 2);
double dt = 0.1;
Matrix3 Q = Matrix3::Zero();
// Analytic Jacobian
Matrix3 actualH;
LIEKF<Rot3> iekf0(R0, P0);
iekf0.predictMean(f, dummy_u, dt, Q, actualH);
// wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 {
LIEKF<Rot3> iekf(R, P0);
return iekf.predictMean(f, dummy_u, dt, Q);
};
// numeric Jacobian of g at R0
Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
EXPECT(assert_equal(expectedH, actualH));
}
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);