Left invariant EKF header file, with an example on SE(2) and NavState#2113
Left invariant EKF header file, with an example on SE(2) and NavState#2113scottiyio wants to merge 10 commits intoborglab:developfrom
Conversation
A Left Invariant IEKF on SE(2).
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
Cool!
Like we discussed, examples need docs. Not doxygen, which is for APIs, but a bit more. Look at other examples.
Maybe change methods "getFooBar" to just "fooBar".
Please format all files with clang-format.
| Vector9 result; | ||
| result << w, Z_3x1, a; | ||
| if (H) { | ||
| *H = Matrix::Zero(9, 9); |
There was a problem hiding this comment.
H->setZero() will be faster.
These dynamics don't depend on the state so maybe we have to rethink this. No sense in multiplying with a bunch of zeros. Let's talk in the meeting.
| return result; | ||
| } | ||
|
|
||
| // define a GPS measurement processor. The GPS measurement processor returns |
There was a problem hiding this comment.
measurement function, not processor.
| // define a GPS measurement processor. The GPS measurement processor returns | ||
| // the expected measurement h(x) = translation of X with a Jacobian H used in | ||
| // the update stage of the LIEKF. | ||
| Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { |
There was a problem hiding this comment.
Probably need to add this to NavState and properly unit test
|
|
||
| // Create the filter with the initial state, covariance, and dynamics and | ||
| // measurement functions. | ||
| GeneralLIEKF<NavState, Vector3, 6> ekf(X0, P0, dynamics_func, h_func); |
There was a problem hiding this comment.
Why can't we just pass dynamics and h_gps here? Ask the AI :-)
| // Create the IMU measurements of the form (linear_acceleration, | ||
| // angular_velocity) | ||
| Vector6 imu1, imu2; | ||
| imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
There was a problem hiding this comment.
was good for compiling but now we need values?
| * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement | ||
| * for 3D position) | ||
| */ | ||
|
|
| } | ||
|
|
||
| protected: | ||
| LieGroup X; ///< Current state estimate. |
There was a problem hiding this comment.
private or protected members take underscore
| Matrix S = H * P * H.transpose() + R; | ||
| Matrix K = P * H.transpose() * S.inverse(); | ||
| X = X.expmap(-K * y); | ||
| P = (I_n - K * H) * P; // move Identity to be a constant. |
| I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. | ||
| }; | ||
|
|
||
| /** |
There was a problem hiding this comment.
this needs no doxygen, just say what this is in one-liner comment.
| * @tparam _p The dimension of the control vector. | ||
| */ | ||
| template <typename LieGroup, typename Measurement, size_t _p> | ||
| class GeneralLIEKF : public LIEKF<LieGroup, Measurement> { |
There was a problem hiding this comment.
Starting to think that rather than subclassing the filter, we should instead have different types of dynamics. Not sure how yet, let's chat.
|
Closing as superseded by #2115 |
2 participants
No description provided.