Add AffineSystemSim class that takes Ks, Kv, and Ka constants

[calcmogul]

LinearSystemSim doesn't model static friction via Ks, but that's by design since that makes the system nonlinear. To address that need, we could add a class specifically for modeling systems of the form dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x).

[sciencewhiz]

Also requested here: https://www.chiefdelphi.com/t/dcmotorsim-but-from-ks-kv-and-ka/440603

[narmstro2020]

So I've been playing around with this.

Haven't made a PR yet.
Wanted some feedback first.

Relevant Classes https://github.com/narmstro2020/allwpilib/blob/AffineSystemSim/wpimath/src/main/java/edu/wpi/first/math/system/AffineSystem.java

https://github.com/narmstro2020/allwpilib/blob/AffineSystemSim/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AffineDCMotorSim.java

[calcmogul]

AffineSystem shouldn't inherit from LinearSystem because an affine system isn't a kind of linear system (a linear system is a special case of an affine system where c = 0). The model of an affine system has the form dx/dt = Ax + Bu + c where A and B are constant matrices and c is a constant column vector. Here's the notation to follow:

dx/dt = Ax + Bu + c
dx/dt = Ax + B(u + B⁺c)
xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
xₖ₊₁ = A_d xₖ + B_duₖ + c_d cₖ where c_d = B_d B⁺

[narmstro2020]

Deep down I knew the inheritance would be a problem. It was mostly an implementation convenience, but I can see where it can would be problematic to have it inherit.
I'll clean that up.

[calcmogul]

I'm not sure whether to always treat c as a constant or let it vary between timesteps. The latter would be more efficient, tho functions like calculateX() would need to take u and c.

[narmstro2020]

I can make that change. Other than that is the implementation good (mathematically speaking).

[calcmogul]

Use c instead of constant for the constant vector. The form xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) should be cheaper to calculate in calculateX() because it has one less matrix multiply. Cursory check looks good otherwise.

[narmstro2020]

Some cleanup based on your recommendations.

Relevant Classes https://github.com/narmstro2020/allwpilib/blob/AffineSystemSim/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AffineDCMotorSim.java

https://github.com/narmstro2020/allwpilib/blob/AffineSystemSim/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AffineDCMotorSim.java

Actually using AffineSystemSim now

https://github.com/narmstro2020/allwpilib/blob/AffineSystemSim/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AffineSystemSim.java

[narmstro2020]

AffineDCMotorSim has one of the constructor's removed.
Mostly for simplicity sake. This begs the question as to whether or not LinearSystemId would need a name change as it can be used to create both a linear and affine system. But I'll save minutiae like that for the PR.