Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix feedforward returning NaN when kᵥ = 0 (#7790)

Browse Source
This commit is contained in:
Tyler Veness
2025-02-25 19:07:51 -08:00
committed by GitHub
parent 75321f1d84
commit 822457d45b
5 changed files with 40 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/algorithms.md
View File

@@ -92,7 +92,7 @@ Substitute these into the feedforward equation.
uₖ = kₛ sgn(x) + kᵥxₖ₊₁
```
Simplify the model when ka ≠ 0
Simplify the model when kₐ ≠ 0
```
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -107,6 +107,21 @@ where
B_d = A⁻¹(eᴬᵀ - I)B
```
When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
```
[A B]
[0 0]T [A_d B_d]
e = [ 0 I ]
[0 B]
[0 0]T [1 BT]
e = [0 1]
A_d = 1
B_d = BT
```
## Elevator feedforward
### Derivation
@@ -199,7 +214,7 @@ Substitute these into the feedforward equation.
uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
```
Simplify the model when ka ≠ 0
Simplify the model when kₐ ≠ 0
```
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -214,6 +229,21 @@ where
B_d = A⁻¹(eᴬᵀ - I)B
```
When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
```
[A B]
[0 0]T [A_d B_d]
e = [ 0 I ]
[0 B]
[0 0]T [1 BT]
e = [0 1]
A_d = 1
B_d = BT
```
## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
### Derivation