Add State Space Documentation

source/docs/software/advanced-controls/index.rst

@@ -12,3 +12,4 @@ This section covers advanced control features in WPILib, such as various feedbac
    geometry/index
    controllers/index
    trajectories/index
+   state-space/index

source/docs/software/advanced-controls/introduction/controls-glossary.rst

@@ -9,8 +9,18 @@ Controls Glossary
    input
       An input to the :term:`plant` (hence the name) that can be used to change the :term:`plant's <plant>` :term:`state`.
 
+            - Ex. A flywheel will have 1 input: the voltage of the motor driving it.
+            - Ex. A drivetrain might have 2 inputs: the voltages of the left and right motors.
+
+        Inputs are often represented by the variable :math:`\mathbf{u}`, a column vector with one entry per :term:`input` to the :term:`system`.
+
    output
-      Measurements from sensors.
+      Measurements from sensors. There can be more measurements then states. These outputs are used in the "correct" step of Kalman Filters.
+
+            - Ex. A flywheel might have 1 :term:`output` from a encoder that measures it's velocity.
+            - Ex. A drivetrain might use solvePNP and V-SLAM to find it's x/y/heading position on the field. It's fine that there are 6 measurements (solvePNP x/y/heading and V-SLAM x/y/heading) and 3 states (robot x/y/heading).
+
+        Outputs of a :term:`system` are often represented using the variable :math:`\mathbf{y}`, a column vector with one entry per :term:`output` (or thing we can measure). For example, if our :term:`system` had states for velocity and acceleration but our sensor could only measure velocity, our, our :term:`output` vector would only include the :term:`system`\'s velocity.
 
    process variable
       The term used to describe the output of a :term:`plant` in the context of PID control.
@@ -22,7 +32,12 @@ Controls Glossary
       :term:`Reference <reference>` minus an :term:`output` or :term:`state`.
 
    state
-      A characteristic of a :term:`system` (e.g., velocity) that can be used to determine the :term:`system's <system>` future behavior.
+      A characteristic of a :term:`system` (e.g., velocity) that can be used to determine the :term:`system's <system>` future behavior. In state-space notation, the state of a system is written as a column vector describing it's position in state-space.
+
+            - Ex. A drivetrain system might have the states :math:`\begin{bmatrix}x \\ y \\ \theta \end{bmatrix}` to describe it's position on the field.
+            - Ex. An elevator system might have the states :math:`\begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix}` to describe its current height and velocity.
+
+        A :term:`system's <system>` state is often represented by the variable :math:`\mathbf{x}`, a column vector with one entry per :term:`state`.
 
    system
       A term encompassing a :term:`plant` and it's interaction with a :term:`controller` and :term:`observer`, which is treated as a single entity. Mathematically speaking, a :term:`system` maps :term:`inputs <input>` to :term:`outputs <output>` through a linear combination of :term:`states <state>`.
@@ -34,7 +49,10 @@ Controls Glossary
       Used in position or negative feedback with a :term:`plant` to bring about a desired :term:`system state <state>` by driving the difference between a :term:`reference` signal and the :term:`output` to zero.
 
    observer
-      In control theory, a system that provides an estimate of the internal :term:`state` of a given real :term:`system` from measurements of the :term:`input` and :term:`output` of the real :term:`system`.
+      In control theory, a system that provides an estimate of the internal :term:`state` of a given real :term:`system` from measurements of the :term:`input` and :term:`output` of the real :term:`system`. WPILib includes a Kalman Filter class for observing linear systems, and ExtendedKalmanFilter and UnscentedKalmanFilter classes for nonlinear systems.
+
+   moment of inertia
+      A measurement of a rotating body's resistance to angular acceleration or deceleration. Angular moment of inertia can be thought of as angular mass. See also: `Moment of inertia <https://en.wikipedia.org/wiki/Moment_of_inertia>`__.
 
    control input
       The input of a :term:`plant` used for the purpose of controlling it

source/docs/software/advanced-controls/state-space/filter-comparison-data/iir-filter.csv

@@ -0,0 +1,145 @@
+x,y
+0.013694498069498018, 19.319363293765036
+0.04894124034749031, 59.113053735727135
+0.08789816602316602, 101.7612171917367
+0.12128981660231664, 147.64138455752618
+0.15282637548262545, 192.46914919288702
+0.18621802606177607, 239.64987617747056
+0.21960967664092668, 284.55888330389826
+0.2530013272200772, 331.73961028848225
+0.2863929777992278, 378.69316528724994
+0.31607444498069504, 426.27122396903223
+0.34575591216216217, 472.3888913134283
+0.3754373793436293, 519.3178871785944
+0.40511884652509655, 560.1781457084012
+0.44036558880308885, 609.4903423560372
+0.47375723938223946, 652.6614837909751
+0.5052937982625483, 694.9051670876838
+0.5386854488416989, 743.9032699587924
+0.5720770994208495, 788.3579331135893
+0.6073238416988418, 832.3997396903028
+0.6444256756756757, 878.1186719584375
+0.6815275096525097, 922.6108755031678
+0.7130640685328187, 964.8951252259153
+0.7390353523166024, 1008.0674507619378
+0.7631515444015444, 1048.5176594717873
+0.7891228281853282, 1095.401812990333
+0.8169492036679538, 1138.1685839049596
+0.8429204874517375, 1179.588439836119
+0.8670366795366795, 1225.4907762055434
+0.8874426882239383, 1268.9280772508898
+0.9059936052123554, 1314.2775699807016
+0.926399613899614, 1363.371453472857
+0.9486607142857144, 1406.9411107060778
+0.969066722972973, 1454.8082654748287
+0.9876176399613901, 1501.793396502513
+1.0098787403474905, 1550.1726092068589
+1.0339949324324325, 1591.450373007894
+1.0525458494208495, 1635.8798191951528
+1.07480694980695, 1684.134898635821
+1.104488416988417, 1730.8407791577754
+1.1286046090733592, 1774.2361103980204
+1.1508657094594597, 1825.886599698111
+1.1712717181467183, 1870.2390793148859
+1.1935328185328187, 1915.9384739151283
+1.221359194015444, 1962.4292427400894
+1.2454753861003862, 2006.7251486383893
+1.27144666988417, 2053.2685441782114
+1.2974179536679538, 2092.8264011542033
+1.321534145752896, 2145.0570999856336
+1.3475054295366797, 2183.556173242021
+1.3716216216216217, 2231.6977763287705
+1.3920276303088803, 2279.0878699273085
+1.4105785472972974, 2324.8462722315885
+1.4346947393822396, 2373.6596553335357
+1.462521114864865, 2425.152264489044
+1.4922025820463323, 2464.746850526449
+1.5293044160231661, 2521.8739468822696
+1.5701164333976836, 2550.4107830155153
+1.6109284507722008, 2575.23025938087
+1.6517404681467185, 2587.2248445469977
+1.6925524855212357, 2561.116492092236
+1.733364502895753, 2552.66559853496
+1.7741765202702706, 2555.180916292964
+1.8149885376447878, 2551.5625904339468
+1.8539454633204633, 2590.30752613367
+1.8929023889961392, 2633.7363351409367
+1.9337144063706564, 2641.0842205972003
+1.974526423745174, 2626.685551411297
+2.0153384411196913, 2614.703166074524
+2.0561504584942085, 2594.1708532715998
+2.096962475868726, 2596.12856706442
+2.1377744932432434, 2608.123152230548
+2.1785865106177607, 2601.902674534007
+2.2193985279922783, 2591.035497127601
+2.2602105453667956, 2583.6998115006922
+2.301022562741313, 2568.3718023728165
+2.3418345801158305, 2567.169760362929
+2.3826465974903477, 2570.614418062906
+2.423458614864865, 2550.453841236771
+2.4642706322393826, 2555.0137068671156
+2.5050826496139, 2568.681103928795
+2.545894666988417, 2568.594269849275
+2.5867066843629347, 2576.3138912823274
+2.627518701737452, 2564.889109910738
+2.668330719111969, 2564.802275831218
+2.709142736486487, 2571.034953357114
+2.749954753861004, 2521.1354983878455
+2.7852014961389964, 2480.1695478723605
+2.8204482384169887, 2444.965504997108
+2.861260255791506, 2431.1244397763885
+2.9020722731660236, 2458.7319359676617
+2.942884290540541, 2449.7234384452017
+2.983696307915058, 2444.989904655817
+3.0245083252895757, 2442.1150507503785
+3.065320342664093, 2460.615015510317
+3.10613236003861, 2496.214835202557
+3.146944377413128, 2526.6103512197487
+3.184046211389962, 2575.869036066865
+3.2211480453667956, 2600.6137984446063
+3.2619600627413132, 2586.029261270309
+3.3027720801158305, 2574.0468759335354
+3.3435840974903477, 2568.941606167362
+3.37883083976834, 2549.443408311451
+3.4140775820463323, 2502.4367481731424
+3.453034507722008, 2458.7120574221885
+3.4919914333976836, 2426.250966767947
+3.5328034507722013, 2438.2455519340747
+3.5661951013513518, 2481.4286420254098
+3.5995867519305027, 2522.3016585466976
+3.6403987693050195, 2549.5374187611815
+3.681210786679537, 2586.6241823605787
+3.722022804054055, 2580.961308629221
+3.7628348214285716, 2566.190903466529
+3.8036468388030893, 2554.2085181297557
+3.8407486727799234, 2517.9059723192313
+3.8778505067567575, 2471.711119126373
+3.9186625241312742, 2469.393869186118
+3.959474541505792, 2471.7233189557273
+3.996576375482626, 2516.6657360723475
+4.03367820945946, 2555.020165600314
+4.074490226833976, 2580.583113919247
+4.115302244208495, 2600.3841545979476
+4.156114261583012, 2581.1529177137854
+4.196926278957529, 2563.0368887599907
+4.237738296332047, 2552.3555793419796
+4.278550313706564, 2562.677352612556
+4.319362331081081, 2564.6350664053766
+4.358319256756758, 2527.9361856157384
+4.3917109073359075, 2480.073833034719
+4.4269576496139, 2441.7332678553075
+4.467769666988417, 2424.73244683188
+4.508581684362936, 2430.4075203925927
+4.541973334942085, 2455.9862567259834
+4.5735098938223935, 2498.650208196451
+4.612466819498071, 2528.510654050913
+4.6532788368725875, 2545.895410880484
+4.694090854247104, 2540.046669160732
+4.734902871621623, 2539.4022311160284
+4.77571488899614, 2561.247819667069
+4.816526906370656, 2555.956681912501
+4.857338923745175, 2552.338356053484
+4.898150941119692, 2544.4450664613914
+4.9389629584942085, 2557.183123581098
+4.979774975868727, 2563.9734050721772
+5.0187319015444025, 2590.7298555204497