search

uf-mil / mil

Monorepo of the Machine Intelligence Lab at the University of Florida
https://mil.ufl.edu/docs
Other
45 stars 29 forks source link

Update/Rewrite odom_estimator to accept inclinometer #152

Closed kev-the-dev closed 5 months ago

kev-the-dev commented 4 years ago

The new sub will have an inclinometer, in addition to DVL, IMU, depth, and magnetometer. We need to update the kalman filter in odom_estimator to accept this sensor in our state estimation.

One choice to solve this issue is to write a whole new kalman filter from scratch, which has been desired for some time as the current one is undocumented and has poor readability.

forrestv commented 4 years ago

I guess this got closed for one reason or another, but if anyone's interested in understanding (and potentially rewriting) odom_estimator, I'd highly recommend first reading the paper "Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds" and Jason's excellent "MIL GNC Notes", which document the ideas, math, and some of the implementation behind it. The code is a relatively direct implementation of that math, without too much extra boilerplate, mostly thanks to the unscented Kalman filter not requiring the explicit calculation of huge Jacobian matrices, along with some macro metaprogramming that generates manifold operators (which is similar to that described in the first paper, section 5).

I should probably commit references to those two papers directly into a little README in odom_estimator for posterity...

I still think that the general ideas behind odom_estimator (a UKF operating on a local linearization of a manifold with the manifold operators being generated via metaprogramming) are all sound, and if I rewrote it today, the only thing I'd do differently is to do it all in ENU coordinates rather than ECEF/ECI, which is mostly just a historical artifact of odom_estimator previously supporting GPS measurements, but just obfuscates it now.

I did notice that the auto-reformatting that happened a while ago made some of odom_estimator like this file a bit uglier. odom_estimator makes use of C++ macros to implement a manifold Cartesian product, and the formatter didn't seem to like the syntax used when calling the macro too much...

With Jason's notes and the very-useful technique of running the manifold code through g++ -E to see what the macros expand to, I don't think that any of odom_estimator should be that mysterious. In any case, I'd be happy to explain any of it and walk anyone who's curious through it.

MarshallRawson commented 4 years ago

Was referencing wrong data sheet

cbrxyz commented 5 months ago

Closing this issue because we are no longer planning to integrate an inclinometer into sub.