[wpimath] Add full state support to LinearSystemId functions

[narmstro2020]

Addresses issue #6513

C++ version needs to be done, but I need some feedback before proceeding.

This relies on LinearSystem providing a function to trim the C and D matrices to a single row for use in the KalmanFilter, LinearQuadraticRegular, and LinearSystemLoop classes.

The trimmed version of the LinearSystem is passed into these classes.
This would add more of a implementation burden on users who wish to use this in the classes.
While what I've written works in principle, would it be better to modify the KalmanFilter, LQR, and LinearSystemLoop classes to take in the LinearSystem and extract what they need instead?

DCMotorSim, ElevatorSim, and SingleJointedArmSim have been modified to use the velocity output of the LinearSystem instead the state.

[calcmogul]

While what I've written works in principle, would it be better to modify the KalmanFilter, LQR, and LinearSystemLoop classes to take in the LinearSystem and extract what they need instead?

Those classes can take any linear system, so they can't know what to slice unless the user tells them. At that point, the user might as well do the slicing via the LinearSystem method to separate concerns.

[narmstro2020]

Thanks for the help @calcmogul
I'll plan on working on the C++ version tomorrow.

[narmstro2020]

@calcmogul
I've started the C++ version. LinearSystemId, and all of the simulation classes have been modified accordingly.
I need some help with the C++ Slice function in LinearSystem. I started it, but wasn't sure how to extract the rows like in the Java version.

[calcmogul]

Declare the parameter pack like this: https://godbolt.org/z/aoz7YYjfo.

Use "Array of indices" from https://eigen.tuxfamily.org/dox/group__TutorialSlicingIndexing.html to slice the matrices.

If you want to sort and deduplicate the indices first, copy them into a std::array via std::array arr{outputIndices...};, then use https://en.cppreference.com/w/cpp/algorithm/ranges/sort piped into https://en.cppreference.com/w/cpp/algorithm/ranges/unique.

[narmstro2020]

@calcmogul
It's a start, but I'm still having some struggles getting this to compile.

[calcmogul]

I got it to compile, but the precondition checks need some work still.

[narmstro2020]

@calcmogul
How so on the condition checks?

[calcmogul]

If the user adds a duplicate index, the function uses the wrong output dimension for the return type (too large). We should probably just throw an error on duplicates.

[narmstro2020]

/format

[narmstro2020]

/format

[narmstro2020]

Simple checks for duplication.

[narmstro2020]

How is this one looking. Anything else I need to do?

[narmstro2020]

Thank you. Requested changes made and committed.

[narmstro2020]

/format

[narmstro2020]

Thanks again.