StateSpaceObserver::Update() correction step is incorrect

[calcmogul]

This is with regard to https://github.com/frc1678/robot-code-public/blob/master/muan/control/state_space_observer.h and the corresponding .hpp for reference.

The correction step of a Luenberger observer is supposed to use measurements from the same timestep as the model prediction. Since the correction occurs first in StateSpaceObserver::Update(), the corresponding prediction occurs at the end of the previous timestep. If Update() is given measurements taken immediately before the call (that is, they are taken during the timestep after the prediction), a factor of A^-1 needs to be premultiplied onto the correction term to step it back one timestep.

In other words, plant().x() += L() * (y - plant().y()); should be changed to plant().x() += plant().Ainv() * L() * (y - plant().y()); where plant.Ainv() returns a cached version of the inverse of A.

I have a proof in section D.4 of https://file.tavsys.net/control/state-space-guide.pdf.