[cscotti]

Here’s a C++/Eigen example implementation of something very similar: https://github.com/google/detectorgraph/blob/master/examples...

With autogenerated block diagram and description here: https://google.github.io/detectorgraph/docs/html/robotlocali...