discretize matrices without control library

[avangerp]

Description

Discretizing matrices A and B in pure python and numpy, Removing control dependency

Verification

run tests

solves issue #33092

[github-actions]

Thanks for contributing to openpilot! In order for us to review your PR as quickly as possible, check the following:

• Convert your PR to a draft unless it's ready to review
• Read the contributing docs
• Before marking as "ready for review", ensure:
  • the goal is clearly stated in the description
  • all the tests are passing
  • the change is something we merge
  • include a route or your device' dongle ID if relevant

[simontheflutist]

Matrix exponential is notorious for being numerically unstable: https://www.cs.jhu.edu/~misha/ReadingSeminar/Papers/Moler03.pdf

Are you sure scipy isn't the right approach here?

[maxime-desroches]

Thanks for the PR. I will close this in favor of https://github.com/commaai/openpilot/pull/33103/files since the discretization is a bit simpler.

Also check out this for how to compute A_d and B_d in a single step without the need for computing the inverse of A