[sysid] Add SysId (#5672)

The source is copied from this commit:
625ff04784.

sysid/docs/arm-ols-with-angle-offset.md

@@ -0,0 +1,66 @@
+# Arm OLS with angle offset
+
+If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
+for `Kg` will be wrong. An angle offset should be added to the model like so.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
+```
+Use a trig identity to split the cosine into two terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
+```
+Reorder multiplicands so the offset trig is absorbed by the OLS terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
+```
+
+## OLS
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+```
+
+### Ks, Kv, Ka
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+### Kg
+
+Take the sum of squares of the OLS terms containing the angle offset. The angle
+offset trig functions will form a trig identity that cancels out. Then, just
+solve for `Kg`.
+```
+δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))²
+δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset))
+δ²+ε² = (Kg/Ka)² (1)
+δ²+ε² = (Kg/Ka)²
+√(δ²+ε²) = Kg/Ka
+√(δ²+ε²) = Kg β
+Kg = √(δ²+ε²)/β
+```
+
+As a sanity check, when the offset is zero, ε is zero and the equation for
+`Kg` simplifies to -δ/β, the equation previously used by SysId.
+
+### Angle offset
+
+Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
+to preserve the angle quadrant. Maintaining the proper negative signs in the
+numerator and denominator are important for obtaining the correct result.
+```
+δ = -Kg/Ka cos(offset)
+ε = Kg/Ka sin(offset)
+sin(offset)/-cos(offset) = ε/δ
+sin(offset)/cos(offset) = ε/-δ
+tan(offset) = ε/-δ
+offset = atan2(ε, -δ)
+```