Template EqF on Group and Manifold #2197
Conversation
…equire for template, change EqF to take in initial state, use traits for group
jenniferoum
force-pushed
the
feature/equivariant-filter
branch
from
73215df to
4ee2efe
Compare
| * @param Sigma Initial covariance | ||
| * @param m Number of sensors | ||
| */ | ||
| EqF(const G& X0, const M& x0, const Matrix& Sigma, int m); |
There was a problem hiding this comment.
X0 and x0 are related by x0 = \phi(X0, ref), and if it's an action, we can always get X0 from this. So I think: we need to give 2 M thingies: x0, and ref. X0 is derived.
Because the state is defined on the manifold, the user will want to specify the initial state on the manifold. And, more often than not, they will initialize the filter at the reference element.
EqF(const M& xi_circle, const M& xi_0, const Matrix& Sigma, int m);
EqF(const M& xi_circle, const Matrix& Sigma, int m);
| Matrix Q = Geometry::processNoise(u); // Replaces blockDiag(...) | ||
|
|
||
| X_hat = traits<G>::Compose(X_hat, traits<G>::Expmap(L * dt)); | ||
| Sigma = Phi * Sigma * Phi.transpose() + Bt * Q * Bt.transpose() * dt; |
There was a problem hiding this comment.
This is already done in the ManifoldEKF, specialized to groups in the LieGroupEKF, which is the main reason to derive form that class.
| Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result; | ||
| Matrix Dt = Geometry::outputMatrixDt(y.cal_idx, X_hat); | ||
|
|
||
| Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose(); |
There was a problem hiding this comment.
You'll also find this somewhere in the existing hierarchy.
|
Nice to see this one in gtsam 👍🏻 |
|
Thans @AlessandroFornasier !We're going to substantially rewrite and base on LieGroupEKF (or derived), we'll let you know :-) |
3 participants
Updates to template equivariant filter on group and manifold