```python
#we may need some code in the ../python directory and/or matplotlib styles
import sys
import os
sys.path.append('../python/')

#set up matplotlib
os.environ['MPLCONFIGDIR'] = '../mplstyles'
print(os.environ['MPLCONFIGDIR'])
import matplotlib as mpl
from matplotlib import pyplot as plt
#got smarter about the mpl config: see mplstyles/ directory
plt.style.use('standard')
print(mpl.__version__)
print(mpl.get_configdir())

#fonts
# Set the font dictionaries (for plot title and axis titles)
title_font = {'fontname':'Arial', 'size':'16', 'color':'black', 'weight':'normal',
              'verticalalignment':'bottom'} # Bottom vertical alignment for more space
axis_font = {'fontname':'Arial', 'size':'32'}
legend_font = {'fontname':'Arial', 'size':'22'}

#fonts global settings
mpl.rc('font',family=legend_font['fontname'])

#set up numpy
import numpy as np
```

    ../mplstyles
    3.0.3
    /home/phys/villaa/analysis/misc/nrFano_Constraint/mplstyles

# Summary

Fits have been done in notebooks `edelweiss_C.ipynb` and `Qwidth_confirm.ipynb` which give the functional forms of the "C" parameter as a function of energy and the correction to this parameter, called C$^{\prime}$, due to multiple scattering.

The fits in each case have constant components and a slope that are correlated, to find "bounding curves" of these functions given the statistical errors may be easiest with an MCMC fitting approach. That is discussed here.

# Multiple Scatters

For the multiple scatters fitting that was first accomplieshed in the notebook `Qwidth_confirm.ipynb`, the goal would be to expand on the central value for each parameter of the fit, taking into account correlation. From the previous fit we have the following parameters from the `lmfit` output.

Parameter name|Parameter value|Parameter uncertainty
:-|:-|:-
C$_{ms}$|0.0202 | 0.0014 (6.7%)
m | 5.343$\times$10$^{-5}$ | 1.513$\times$10$^{-5}$ (28.3%)
corr(C$_{ms}$,m) | -0.898 | -

The slope parameter has high fractional uncertainty and the parameters are highly correlated. In this situation fluctuating each of them upward by the 1$\sigma$ level and taking the resulting cuve from those parameters will overestimate the true uncertainty on the measured quadrature correction for multiple scatters, C$^{\prime}$, as a function of energy. The _correlation_ parameter is a dimensionless parameter between -1 and 1 where the magnitude describes the size of the correlation and the sign describes whether it is an _anti_correlation (see [[corrNotes][corrNotes]])

[corrNotes]: https://www.unige.ch/sciences/astro/files/5413/8971/4090/2_Segransan_StatClassUnige.pdf "correlation notes"

We therefore seek a fitting method that will yield the two-dimensional distribution of C$_{ms}$ and m. The method we use is a Markov Chain Monte Carlo (MCMC) technique, as implemented by the `emcee` library [[emcee][emcee]].

[emcee]: https://emcee.readthedocs.io/en/v2.2.1/user/line/ "emcee implemntation of MCMC"

We start, below, by getting a version of the high-statistics mulitple-scatter simulation, and the calculation for the single-scatter predicted yield width.

```python
import h5py
filename = 'data/sims.h5'
#remove vars
f = h5py.File(filename,'r')

#save the results for the Edw fit
path='{}/'.format('NR')

xE = np.asarray(f[path+'xE'])
qbootsigs_nr_ms = np.asarray(f[path+'qbootsigs'])
qbootsigerrsu_nr_ms = np.asarray(f[path+'qbootsigerrsu'])
qbootsigerrsl_nr_ms = np.asarray(f[path+'qbootsigerrsl'])

f.close()
```

```python
#get the NR calculations for our yield band
import fano_calc as fc
Enr,signr = fc.RWCalc(filename='data/res_calc.h5',band='NR',alpha=(1/18.0))
```

    GGA3/NR/4.0/5.556E-02/0.0381/000/
    True

```python
#let's make a callable out of the predicted single-scatter resolution
import scipy.interpolate as inter

sigQnr_c = inter.InterpolatedUnivariateSpline(Enr, signr, k=3)
sigQnr_c_v = np.vectorize(sigQnr_c)
```

We take the model we have for the single-scatter yield width, $\sigma_{\mathrm{Q}ss}(E_r)$, and construct a model for the width including all scatters as:

\begin{equation}
\sigma_{\mathrm{Q}} = \sqrt{\sigma_{\mathrm{Q}ss}^2 + C^{\prime2}},
\end{equation}

Where C$^{\prime}$ is modeled as the linear (in recoil energy) function C$_{ms}$ + m$E_r$.

```python
#also construct a residual function for use with lmfit
import lmfit as lmf

def residual(params, x, data, eps_data,sigQfunc):
    Cms = params['Cms']
    m = params['m']

    model = np.sqrt(sigQfunc(x)**2 + (Cms+m*x)**2)

    return (data-model) / eps_data

def residual_const(params, x, data, eps_data,sigQfunc):
    Cms = params['Cms']

    model = np.sqrt(sigQfunc(x)**2 + (Cms)**2)

    return (data-model) / eps_data

#do it with lmfit
params = lmf.Parameters()
params.add('Cms', value=0.02)
lmfout = lmf.minimize(residual_const, params, args=(xE, qbootsigs_nr_ms, qbootsigerrsu_nr_ms,sigQnr_c_v))
#print(lmf.fit_report(lmfout))
print('lmfit result--multiples: constant C')
print(lmf.report_fit(lmfout.params))

Cms_const = lmfout.params['Cms'].value

params = lmf.Parameters()
params.add('Cms', value=0.02)
params.add('m', value=0)
lmfout = lmf.minimize(residual, params, args=(xE, qbootsigs_nr_ms, qbootsigerrsu_nr_ms,sigQnr_c_v))
#print(lmf.fit_report(lmfout))
print('lmfit result--multiples: linear C')
print(lmf.report_fit(lmfout.params))

Cms = lmfout.params['Cms'].value
slope = lmfout.params['m'].value
```

    lmfit result--multiples: constant C
    [[Variables]]
        Cms:  0.02433041 +/- 6.7332e-04 (2.77%) (init = 0.02)
    None
    lmfit result--multiples: linear C
    [[Variables]]
        Cms:  0.02109407 +/- 0.00132253 (6.27%) (init = 0.02)
        m:    4.0414e-05 +/- 1.4538e-05 (35.97%) (init = 0)
    [[Correlations]] (unreported correlations are < 0.100)
        C(Cms, m) = -0.896
    None

```python
print(np.shape(xE))
print(xE)
print(np.shape(qbootsigs_nr_ms))
print(qbootsigs_nr_ms)

#get rid of the firs point because it's spurious
#xE = xE[2:]
#qbootsigs_nr_ms = qbootsigs_nr_ms[2:]
#qbootsigerrsu_nr_ms = qbootsigerrsu_nr_ms[2:]

#print(np.shape(xE))
#print(xE)
#print(np.shape(qbootsigs_nr_ms))
#print(qbootsigs_nr_ms)
#print(np.shape(qbootsigerrsu_nr_ms))
#print(qbootsigerrsu_nr_ms)
```

    (20,)
    [  5.  15.  25.  35.  45.  55.  65.  75.  85.  95. 105. 115. 125. 135.
     145. 155. 165. 175. 185. 195.]
    (20,)
    [3.47598783e+02 8.81845858e-02 5.47623039e-02 4.20855644e-02
     3.68255377e-02 3.40698909e-02 3.31940079e-02 3.15312927e-02
     3.13619783e-02 3.08839191e-02 2.98659882e-02 3.08581960e-02
     3.00476304e-02 3.13050058e-02 3.38317545e-02 3.32995478e-02
     3.33460047e-02 3.10841361e-02 2.99080751e-02 3.22608048e-02]

```python
# from here on basically following https://emcee.readthedocs.io/en/latest/tutorials/line/

#ACTUALLY
#that above link is for the bleeding edge 3.0 ish version, it is not easy to install and appears to be failing tests
#so use the stable:
#https://emcee.readthedocs.io/en/stable/user/line/#marginalization-uncertainty-estimation
#note several interfaces have changed in the 3.0x version so stick to this example
```

```python
def log_likelihood(theta, x, y, yerr,sigfunc):
    Cms, m = theta
    model = np.sqrt(sigfunc(x)**2 + (Cms+m*x)**2)
    #sigma2 = yerr**2 + model**2*np.exp(2*log_f)
    sigma2 = yerr**2
    return -0.5*np.sum((y-model)**2/sigma2 + np.log(sigma2))
```

```python
from scipy.optimize import minimize
np.random.seed(42)
nll = lambda *args: -log_likelihood(*args)
initial = np.array([0.0207, 4.73e-5]) + 0.001*np.random.randn(2)
soln = minimize(nll, initial, args=(xE, qbootsigs_nr_ms, qbootsigerrsu_nr_ms,sigQnr_c_v))
Cms_ml, m_ml = soln.x

print("Maximum likelihood estimates:")
print("m = {0:.4f}".format(Cms_ml))
print("b = {0:.3e}".format(m_ml))

plt.errorbar(xE, qbootsigs_nr_ms, yerr=qbootsigerrsu_nr_ms, fmt=".k", capsize=0)
#plt.plot(x0, m_true*x0+b_true, "k", alpha=0.3, lw=3, label="truth")
#plt.plot(x0, np.dot(np.vander(x0, 2), w), "--k", label="LS")
#plt.plot(x0, np.dot(np.vander(x0, 2), [m_ml, b_ml]), ":k", label="ML")
#plt.legend(fontsize=14)
plt.xlim(10, 200)
plt.ylim(0,0.05)
plt.xlabel("E")
plt.ylabel("$\sigma$");
```

```python
def log_prior(theta):
    Cms, m = theta
    if 0.00 < Cms < 0.04 and -1e-4 < m < 1e-4:
        return 0.0
    return -np.inf
```

```python
def log_probability(theta, x, y, yerr,sigfunc):
    lp = log_prior(theta)
    if not np.isfinite(lp):
        return -np.inf
    return lp + log_likelihood(theta, x, y, yerr,sigfunc)
```

```python
import emcee

ndim, nwalkers = 2, 100
pos = [soln.x + 1e-5*np.random.randn(ndim) for i in range(nwalkers)]
sampler = emcee.EnsembleSampler(nwalkers, ndim, log_probability, \
          args=(xE, qbootsigs_nr_ms, qbootsigerrsu_nr_ms,sigQnr_c_v))

sampler.run_mcmc(pos, 500)

#import emcee
#print(emcee.__version__)
#pos = soln.x + 1e-4*np.random.randn(32, 2)
#nwalkers, ndim = pos.shape

#sampler = emcee.EnsembleSampler(nwalkers, ndim, log_probability, \
#          args=(xE, qbootsigs_nr_ms, qbootsigerrsu_nr_ms,sigQnr_c_v))
#sampler.run_mcmc(pos, 5000, progress=True)
#chain = sampler.run_mcmc(pos, 5000)
```

    (array([[2.12669020e-02, 3.90993582e-05],
            [2.00271873e-02, 5.33474503e-05],
            [2.12292101e-02, 4.24711248e-05],
            [2.09280025e-02, 4.69643558e-05],
            [2.07503350e-02, 3.96253693e-05],
            [2.07364477e-02, 3.94894465e-05],
            [2.03098846e-02, 4.94265697e-05],
            [2.09164416e-02, 4.09037946e-05],
            [2.07903966e-02, 4.67675729e-05],
            [2.13473284e-02, 3.56780651e-05],
            [2.04642533e-02, 4.61900147e-05],
            [2.26387002e-02, 2.62900961e-05],
            [2.16716954e-02, 3.69374246e-05],
            [2.06731629e-02, 5.13670950e-05],
            [2.17048655e-02, 3.80551834e-05],
            [2.11581072e-02, 3.78726516e-05],
            [2.08103746e-02, 4.37165414e-05],
            [2.13867616e-02, 2.91012905e-05],
            [2.02404482e-02, 4.36219499e-05],
            [1.98086494e-02, 5.90786930e-05],
            [2.14069320e-02, 4.23762366e-05],
            [2.17784851e-02, 2.98496592e-05],
            [2.05891928e-02, 4.14766050e-05],
            [2.12802829e-02, 3.99617019e-05],
            [2.20469363e-02, 3.32016663e-05],
            [2.30631399e-02, 2.66432613e-05],
            [2.05678504e-02, 4.51745064e-05],
            [1.97394156e-02, 5.24550731e-05],
            [2.11417797e-02, 4.14132687e-05],
            [2.12309397e-02, 3.85004959e-05],
            [2.06362376e-02, 4.12938709e-05],
            [2.18065513e-02, 3.57605812e-05],
            [2.22821869e-02, 2.54238372e-05],
            [2.14552399e-02, 3.75264507e-05],
            [2.20578975e-02, 2.78077213e-05],
            [2.13224060e-02, 4.35673830e-05],
            [2.14745387e-02, 3.23329181e-05],
            [2.10769314e-02, 4.24243473e-05],
            [2.06738626e-02, 4.50971695e-05],
            [2.16837170e-02, 3.49789834e-05],
            [2.11958261e-02, 4.32783156e-05],
            [2.24690498e-02, 2.72950350e-05],
            [2.05261709e-02, 4.74147463e-05],
            [2.16168612e-02, 3.55068732e-05],
            [2.22673147e-02, 3.06122038e-05],
            [2.15495995e-02, 3.31230238e-05],
            [2.16111003e-02, 3.46714483e-05],
            [2.11980357e-02, 3.04944459e-05],
            [2.16396520e-02, 3.39007606e-05],
            [2.13472106e-02, 4.16654896e-05],
            [2.12847889e-02, 4.56811755e-05],
            [2.06132414e-02, 4.38174585e-05],
            [2.12448067e-02, 3.78390816e-05],
            [2.05838515e-02, 4.20391530e-05],
            [2.06612124e-02, 4.43050604e-05],
            [2.09016073e-02, 4.68620358e-05],
            [2.09427314e-02, 4.06675930e-05],
            [2.07307726e-02, 3.79366910e-05],
            [2.12299541e-02, 4.30511157e-05],
            [2.05549353e-02, 4.73256496e-05],
            [2.14099045e-02, 2.46720845e-05],
            [2.08687647e-02, 4.62655493e-05],
            [2.16100047e-02, 3.38331616e-05],
            [2.05173300e-02, 5.18486073e-05],
            [2.12962168e-02, 3.56570321e-05],
            [2.08452616e-02, 4.06523710e-05],
            [2.06562180e-02, 4.14369663e-05],
            [2.03821183e-02, 4.70100092e-05],
            [2.24220396e-02, 2.53792803e-05],
            [2.05928099e-02, 5.13802971e-05],
            [2.12256333e-02, 4.12525427e-05],
            [2.02098470e-02, 5.49821075e-05],
            [2.12730532e-02, 4.03535120e-05],
            [2.08391824e-02, 3.76669256e-05],
            [2.09760762e-02, 4.23703937e-05],
            [2.11826476e-02, 4.39081511e-05],
            [2.13198949e-02, 3.61772351e-05],
            [2.25814639e-02, 2.62331959e-05],
            [2.24755702e-02, 2.93061146e-05],
            [2.23793851e-02, 2.49428072e-05],
            [2.13314063e-02, 4.03436055e-05],
            [2.18858025e-02, 3.36309611e-05],
            [2.15481182e-02, 4.04809161e-05],
            [2.23020548e-02, 3.17507594e-05],
            [2.12840054e-02, 3.64783626e-05],
            [2.07745224e-02, 3.86219028e-05],
            [2.15191315e-02, 5.03050696e-05],
            [1.95827205e-02, 5.11664304e-05],
            [2.21067901e-02, 2.89984192e-05],
            [2.19302793e-02, 2.96189341e-05],
            [2.27642317e-02, 3.01230655e-05],
            [2.08062344e-02, 4.31942980e-05],
            [2.11693824e-02, 4.41695023e-05],
            [2.04782001e-02, 4.88854911e-05],
            [2.01528692e-02, 4.75077412e-05],
            [1.98159648e-02, 5.36088258e-05],
            [2.10892753e-02, 3.76321358e-05],
            [2.09522079e-02, 4.71037278e-05],
            [2.00084157e-02, 5.67163459e-05],
            [2.07846995e-02, 4.38882537e-05]]),
     array([95.89494553, 94.84056161, 95.52013384, 95.08373898, 95.24759262,
            95.16154119, 95.38965349, 95.84542976, 95.46937171, 95.70571315,
            95.60944476, 93.9709693 , 95.49150054, 94.21066825, 95.17021852,
            95.79796213, 95.85210876, 93.49680154, 94.46003706, 93.40746463,
            94.98097893, 95.07287899, 95.21343526, 95.83146415, 95.03691751,
            91.66827943, 95.70224599, 94.4586672 , 95.84881972, 95.89825945,
            95.31902432, 95.32844775, 94.45873089, 95.80531994, 94.87516589,
            94.89050427, 95.1851196 , 95.80883729, 95.77603782, 95.64409209,
            95.40875447, 94.4095873 , 95.60563555, 95.70591369, 94.71727656,
            95.49980639, 95.69968466, 93.28464587, 95.64710831, 95.3968036 ,
            94.15706145, 95.68407002, 95.86608081, 95.33384486, 95.77300247,
            95.19292256, 95.8540996 , 94.59532023, 95.3731362 , 95.61060701,
            90.57721247, 95.43777368, 95.64049602, 94.53971916, 95.6359443 ,
            95.71067059, 95.41148283, 95.52463587, 94.40558852, 94.49313435,
            95.75565862, 94.14588523, 95.80005542, 94.94788596, 95.89147382,
            95.27097033, 95.74843427, 94.14890011, 94.17549578, 94.36303231,
            95.70466487, 95.3905327 , 95.0582249 , 94.39243313, 95.75284059,
            95.02721837, 88.9733986 , 93.61559043, 95.04709558, 95.15883399,
            92.34738155, 95.8588241 , 95.23339782, 95.43244095, 95.08201054,
            94.65055703, 95.6550923 , 94.958395  , 93.9337774 , 95.84280031]),
     ('MT19937', array([3696728224,  654995086, 3955913520,  619672471, 2314424157,
              502663093, 3979615521, 3774705341, 2768054911, 2335967797,
             3899596348, 3008317835,   20790605, 3635273562, 3023790011,
             2676342923, 1169636684,  443084174,  411423754,  176571181,
             1211817982,  693158203, 3371270204, 3411908005, 2696065200,
             3636226924, 3427933290, 4221911781, 2423649922, 3338328634,
             1858118111, 3608114788,  792739807, 1439377089, 4015029076,
              103121709, 2831685813, 1597166925,  795449159,  454922105,
              740678660, 3449382843, 1762848262, 2985098724, 3014118385,
              504196068,  917433600, 3891228650, 1640299318,  634709392,
             2055417405, 3003919376, 2461836905, 2107866901, 1578703454,
             1115554827, 3484507565, 4180479130, 2188805625, 2779129717,
             3699917159,  414357195, 1386382475, 2779282823, 2592623454,
             4286000841, 3808862347, 3978197179, 2229389199, 3325054317,
             3047752785, 4020299114, 3212420216, 3589349930,  847029630,
              110737376, 1426477865, 2555864399, 4279707184,  801110969,
             2824798218, 1636689931, 4161368964, 3411385831, 4108229955,
             3752649075, 3617649552,  497599446, 3175416763,   37853692,
             3412716961,  647860181,  667544702, 1539726844, 3254289411,
              456328297, 2065239812,  687324091, 1595024808, 4090974569,
              235853563, 1772370733, 2379628706, 2504199625, 3803984114,
             1355127714, 3169721839, 2163452845, 3161609379, 1759931071,
             2953953954,  749446662, 2700131879, 2057454389, 1978987066,
              906091603, 4270738818,  299557002, 1005978551, 3291357855,
             3351498528, 1100272202, 2304530339, 1199802707, 2441586992,
             3254136290,  867470173,  553382786, 4256653224,  889931451,
             2038022817, 4045097226, 3493093178, 1788111135, 2632332501,
             2618489587, 2339692631, 3142453464, 2314143805, 2660718513,
             1907695003, 1000976536,  458433393, 2980601514, 2424271095,
             1439481559, 1703560727, 2674722079, 4007907631, 1006583812,
             1591228375, 2595689204,  595077193, 3656880346, 3364282562,
             3573698476, 3788478793, 1690740726,  871909646, 1727220721,
               76486748, 3447319542, 3520032124, 2786496650, 2813847355,
             3298249092,  413766897, 1016551192, 1683274134, 1478474227,
             2455045227, 3760778135,  893265741, 2841931063, 3922301814,
              442277973, 3823197070, 1225260922,  187161758,  712580128,
             2170216021,  721574624, 2255587285, 1751105661, 2360773681,
             2090276905,  950808223, 1991819004, 2462903883, 2417969695,
             2838255094, 2402132518,  305272717,  998185947, 2977565208,
              178503383, 3552628762, 1357539558, 2617361295, 2090976899,
              309997647, 3925652762,  444568959, 4249889639, 3931849690,
              268353078, 3531448633,  676007155,  818495124,  471750907,
             1109154868,  524971161, 1351545565, 4107259209, 2267507285,
             3267418477, 4222563879, 1959127750, 3745805587, 3011283070,
             2268973567,  126564792,  919769314, 1161055157, 4275936025,
              453740810, 1394061324, 2591990269, 3206302954, 2870585266,
             1047588962, 3027200916,    3856188, 3805744622, 2442921086,
              584899542, 1115844865, 2886524290, 2586270832, 3019188278,
              353820040,  274515829, 2032468209, 3082337706,   57801408,
             3523331460, 1204665247,  670310299, 3888232731,   25054984,
              752562145, 1780837416, 2391363245,  821108655, 3471083447,
             1431559356, 1599010540, 3094788587, 1173806562, 2217572842,
             1354563578, 1356179111, 1687905615, 3367514714,  682540327,
             3145342861,  108817176, 3554251979, 3326695087, 2644079624,
             1364310385, 2007720960,  866917142, 2257730686, 3932122661,
              244282553,  413624963, 3484505687, 3789419582, 4013103756,
             4135919396, 2588856320, 2337386868, 3900248190, 2817813106,
             1616772960, 1113127388, 2519658220,  194062310, 2997868999,
               64017253, 1816631000,  511956170, 3278314015, 3201143329,
             1776267823,   22392976, 2179215880, 3121027465,   83784443,
             2441305183, 1969710214,  406806372, 3217971026,  964787121,
             1624212877, 2706665248, 1704710982, 3565210246, 3815086257,
             2670002153, 2049816088, 4140125304, 3283521250,  581827028,
             3974144527,  834986620,  871885535, 4154843917, 1632122676,
             3597056045, 1361524969, 1424796386, 3089372510, 2027053192,
                5011629, 3605376462, 2161309672, 4093432540,  698590021,
             1038150216, 1553296440, 2016223608,  862917146, 2092062301,
              581175984, 3152777297, 2396085076, 1225510834, 3342174993,
              719832966, 2517614844,   12181935,  718093217, 4068204181,
             1451578649,  521967018, 2651466274, 2022425591, 1762705098,
             3201093030, 2770831852, 2164102242, 3592154242,  561484207,
             3090651646, 2809586173, 2349050178, 1254467831, 2429770558,
              697052931,   89620205, 2624126619, 2380838211, 3114306299,
             3126521960, 2026318269, 4290587821, 2208660408,   70510350,
              634468215,   50996346, 3302331297, 1697251464, 2711206316,
             1510227511, 1905576316, 1024457224, 2578952354, 2309436860,
             2436512192,  342736960, 3706332365,  341901136, 3962858273,
             4219779097,  630303611, 4144068438,  687773742, 3130088397,
             3675344082, 2045601744, 1135075534,  379035208, 1189273316,
             3240088679,  918881149, 4012356259, 1919565973, 3049180882,
             1843754249, 4245147707,  630741600,  732312850, 3992233482,
              177078929, 3984253636, 1490737943,  585327112, 1416392241,
             4060639741, 2941337493, 1607442433, 2335825722, 1206606269,
             1581746812,  612322880, 2809938437, 2839213299, 1043009868,
               92513818, 3784793408, 3673689894, 4111131223,  972225563,
             3852045363, 2780654820,  767348658, 3378901684, 2794589582,
             3139465807, 3524080506, 2343970598, 3060703554, 3868299543,
             3704657766, 1593715371,  524823721, 3170153867, 2740947005,
               76230420, 2496947259, 3393559849,  499168751, 3338818406,
             2609419332, 2574923943, 3219850977, 1480045932,  836378033,
             1657244960,  452387305, 1143673919, 4161284402, 1057011469,
             1978814876, 2183679221, 2947163555, 1191264281,  239036651,
             3588370401, 4190436998, 1239600403, 3812097418, 3299530449,
             1879624625, 1331258882, 2343695427, 3582201032, 2754438470,
             4274107940,  631974469, 3438178140, 1756902894, 2832389151,
             3045741758, 1211227417, 1599042515, 1853339987, 3107729829,
             3064846768, 2462765610,  156860763,  204906312, 3132738021,
             3505636104, 1512252814, 2333139160, 3735102193, 3425192752,
             2192920741,  488048386,   40212093, 2675164808, 3873480045,
             1067534231, 3509741299,   58977960, 4264445527, 1668873842,
             1835696358, 2920292217, 4068408791, 2370700073, 2840853524,
             2396251904, 3355809591, 1282635646, 2209883824, 3739564596,
             2992798688,  414529906, 3886496473, 1125295907, 1303641627,
             1331386827, 3281785742, 1381446245,  222951238, 1300813231,
             1558561967,  986770308,  162617777, 1407797558, 2323658490,
             2149155749, 1825454448, 1729090767,  295671927,  455181956,
             1459265293,  818625825, 1744865814, 2236469152, 2084350722,
             2374578102, 2777302721, 4104518801,  608350098,  733349273,
             3034724371, 2875711235, 3104215898,  611747168, 4038419923,
             3804710949,  863231912, 3976940593, 2891948237, 3673203564,
             3586581366, 2822913093, 2861062937,   33353257,  484280262,
             2644098956, 2709870925, 3139670739, 3725930534, 2280561806,
              878658820,  564340824, 4209881268, 1209875936, 1515262873,
             1470301637, 2264927997,  501608366, 3510577084, 4053085157,
             2652789135, 4157482103, 4148393352, 3987104373, 3504646791,
             1875085434, 2134818321, 3082450880, 1250805540, 2707435678,
              502225379, 3057973456, 1408244095, 2534095348, 3529683769,
             3091846283,  526612557, 4128321333,  495227894, 1264216341,
             2672384974, 1718810354,   66384993, 2725043029, 2907311600,
              814226038, 1306519558, 3489012138, 4216188197,  142676805,
             4260844905, 1041608142, 2718066770, 4039833795, 3548846602,
             4152034517,    9022807, 3441660147,  924128682, 3877701498,
             4021495032,  978907858,  134668833, 4084146191, 3087858990,
             1759916346, 2154269067, 2264161378,  679883129, 3639069097,
              474967449, 2527101333, 3777757410,  418303789], dtype=uint32), 35, 0, 0.0))

```python
fig, axes = plt.subplots(2, figsize=(10, 7), sharex=True)
#samples = sampler.get_chain()
samples = sampler.chain
labels = ["Cms", "m"]
for i in range(ndim):
    ax = axes[i]
    ax.plot(np.transpose(samples[:, :, i]), "k", alpha=0.3)
    ax.set_xlim(0, np.shape(samples)[1])
    ax.set_ylabel(labels[i])
    ax.yaxis.set_label_coords(-0.1, 0.5)

axes[-1].set_xlabel("step number");
```

```python
import corner
print(np.shape(samples))
samples = sampler.chain[:, 100:, :].reshape((-1, ndim))
print(np.shape(samples))
fig = corner.corner(samples, labels=["$Cms$", "$m$"],
                      truths=[Cms, slope])
```

```python
import matplotlib.pyplot as pl
#xl = np.array([10, 200])
xl = np.linspace(10, 200, 500)
#print(xl)
for Cms_em, m_em in samples[np.random.randint(len(samples), size=100)]:
    pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms_em+m_em*xl)**2), color="k", alpha=0.1)
pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2), color="r", lw=2, alpha=0.8)
#pl.errorbar(x, y, yerr=yerr, fmt=".k")
pl.xlim(10, 200)
pl.ylim(0,0.05)
pl.errorbar(xE, qbootsigs_nr_ms, yerr=qbootsigerrsu_nr_ms, fmt=".k", capsize=0)
```

```python
print(len(samples))
```

    40000

```python
sampsize=100
evec = np.zeros((np.shape(xl)[0],sampsize))
#print(samples[np.random.randint(len(samples), size=sampsize)])
i=0
for Cms_em, m_em in samples[np.random.randint(len(samples), size=sampsize)]:
    v = np.sqrt(sigQnr_c_v(xl)**2+(Cms_em+m_em*xl)**2)
    #print(np.std(v))
    #print(np.mean(v))
    cent = np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2)
    evec[:,i] = v
    #print(i)
    #print(np.mean(v)+np.std(v))
    #print(np.mean(v)-np.std(v))
    i=i+1

upvec = np.mean(evec,axis=1)+np.std(evec,axis=1)
dnvec = np.mean(evec,axis=1)-np.std(evec,axis=1)
```

```python
pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2), color="r", lw=2, alpha=0.8)
pl.plot(xl, upvec, color="r",linestyle="--", lw=2, alpha=0.8)
pl.plot(xl, dnvec, color="r",linestyle="--", lw=2, alpha=0.8)
#pl.errorbar(x, y, yerr=yerr, fmt=".k")
pl.xlim(10, 200)
pl.ylim(0,0.05)
pl.errorbar(xE, qbootsigs_nr_ms, yerr=qbootsigerrsu_nr_ms, fmt=".k", capsize=0)
```

```python
import h5py

#save the results for the MS fit
path='{}/{}/'.format('mcmc','multiples')

filename = 'data/mcmc_fits.h5'

#remove vars
f = h5py.File(filename,'a')
exCms = path+'Cms' in f
exm = path+'m' in f
exsamp = path+'samples' in f
exsampsize = path+'sampsize' in f
exEr = path+'Er' in f
exCsig_u = path+'Csig_u' in f
exCsig_l = path+'Csig_l' in f
exSigss = path+'Sigss' in f

if exCms:
  del f[path+'Cms']
if exm:
  del f[path+'m']
if exsamp:
  del f[path+'samples']
if exsampsize:
  del f[path+'sampsize']
if exEr:
  del f[path+'Er']
if exCsig_u:
  del f[path+'Csig_u']
if exCsig_l:
  del f[path+'Csig_l']
if exSigss:
  del f[path+'Sigss']

dset = f.create_dataset(path+'Cms',np.shape(Cms),dtype=np.dtype('float64').type)
dset[...] = Cms
dset = f.create_dataset(path+'m',np.shape(slope),dtype=np.dtype('float64').type)
dset[...] = slope
dset = f.create_dataset(path+'samples',np.shape(samples),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = samples
dset = f.create_dataset(path+'sampsize',np.shape(sampsize),dtype=np.dtype('float64').type)
dset[...] = sampsize
dset = f.create_dataset(path+'Er',np.shape(xl),dtype=np.dtype('float64').type, \
compression="gzip")
dset[...] = xl
dset = f.create_dataset(path+'Csig_u',np.shape(upvec),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = upvec
dset = f.create_dataset(path+'Csig_l',np.shape(dnvec),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = dnvec
dset = f.create_dataset(path+'Sigss',np.shape(xl),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = sigQnr_c_v(xl)

f.close()
```

# Experimental Yield Widening (EDELWEISS Fit)

This procedure can also be done for the fit (originally in the notebook `edelweiss_C.ipynb`) that extracts the uncertainty and central value for the experimentally observed widening parameter, C.

```python
# import data from Edelweiss
import pandas as pds
res_data = pds.read_csv("data/edelweiss_NRwidth_GGA3_data.txt", skiprows=1, \
                       names=['E_recoil', 'sig_NR', 'E_recoil_err', 'sig_NR_err'], \
                       delim_whitespace=True)

resER_data = pds.read_csv("data/edelweiss_ERwidth_GGA3_data.txt", skiprows=1, \
                         names=['E_recoil', 'sig_ER', 'sig_ER_err'], \
                         delim_whitespace=True)

resER_data = resER_data.sort_values(by='E_recoil')

print (res_data.head(10))
E_recoil = res_data["E_recoil"]
sig_NR = res_data["sig_NR"]
sig_NR_err = res_data['sig_NR_err']
```

       E_recoil    sig_NR  E_recoil_err  sig_NR_err
    0   16.1946  0.062345      0.946176    0.001157
    1   16.4428  0.062345      0.945278    0.001157
    2   44.2627  0.046528      0.992477    0.001543
    3   24.5012  0.059397      0.992477    0.001185
    4   97.7172  0.044847      1.033260    0.002783
    5   58.4014  0.050082      0.991830    0.002288
    6   34.2156  0.053417      1.033260    0.001102

```python
#some of the fit points are known to be "bad" because of the presence of inelastic scattering
#to fix that ignore the first two points

#do it with lmfit
params = lmf.Parameters()
params.add('Cms', value=0.04)
lmfout = lmf.minimize(residual_const, params, args=(E_recoil[2::], sig_NR[2::], sig_NR_err[2::],sigQnr_c_v))
#print(lmf.fit_report(lmfout))
print('lmfit result--multiples: constant C')
print(lmf.report_fit(lmfout.params))

Cms_const = lmfout.params['Cms'].value

params = lmf.Parameters()
params.add('Cms', value=0.04)
params.add('m', value=0)
lmfout = lmf.minimize(residual, params, args=(E_recoil[2::], sig_NR[2::], sig_NR_err[2::],sigQnr_c_v))
#print(lmf.fit_report(lmfout))
print('lmfit result--multiples: linear C')
print(lmf.report_fit(lmfout.params))

Cms = lmfout.params['Cms'].value
slope = lmfout.params['m'].value
```

    lmfit result--multiples: constant C
    [[Variables]]
        Cms:  0.03691795 +/- 0.00196614 (5.33%) (init = 0.04)
    None
    lmfit result--multiples: linear C
    [[Variables]]
        Cms:  0.03132155 +/- 0.00398807 (12.73%) (init = 0.04)
        m:    1.3000e-04 +/- 8.2395e-05 (63.38%) (init = 0)
    [[Correlations]] (unreported correlations are < 0.100)
        C(Cms, m) = -0.905
    None

```python
def new_log_prior(theta):
    Cms, m = theta
    if 0.02 < Cms < 0.05 and -1e-3 < m < 1e-3:
        return 0.0
    return -np.inf
```

```python
def new_log_probability(theta, x, y, yerr,sigfunc):
    lp = new_log_prior(theta)
    if not np.isfinite(lp):
        return -np.inf
    return lp + log_likelihood(theta, x, y, yerr,sigfunc)
```

```python
ndim, nwalkers = 2, 100
pos = [[Cms,slope] + 1e-5*np.random.randn(ndim) for i in range(nwalkers)]
sampler = emcee.EnsembleSampler(nwalkers, ndim, new_log_probability, \
          args=(E_recoil[2::], sig_NR[2::], sig_NR_err[2::],sigQnr_c_v))

sampler.run_mcmc(pos, 500)
```

    (array([[2.88179182e-02, 2.13545889e-04],
            [3.03993109e-02, 1.62366842e-04],
            [3.48266266e-02, 1.88435940e-05],
            [3.13503045e-02, 1.60763516e-04],
            [3.39077528e-02, 9.69228882e-05],
            [3.25859661e-02, 1.07701416e-04],
            [2.95016938e-02, 1.62006889e-04],
            [3.16972843e-02, 1.21888955e-04],
            [3.30752518e-02, 9.96105972e-05],
            [2.80524645e-02, 2.23121160e-04],
            [2.90288864e-02, 1.82491777e-04],
            [3.08259483e-02, 1.40537211e-04],
            [2.73539147e-02, 1.82590203e-04],
            [3.05050122e-02, 1.39467118e-04],
            [3.30866768e-02, 1.03257579e-04],
            [3.17672993e-02, 1.13000579e-04],
            [3.13715643e-02, 1.10888961e-04],
            [2.90118425e-02, 1.94399691e-04],
            [3.11098958e-02, 1.57826722e-04],
            [2.98415663e-02, 1.74889670e-04],
            [3.03557920e-02, 1.28423597e-04],
            [2.78716665e-02, 1.87298828e-04],
            [3.14523203e-02, 1.48151575e-04],
            [3.12709250e-02, 1.39566646e-04],
            [2.94766774e-02, 1.62520354e-04],
            [3.29039660e-02, 1.11029324e-04],
            [2.78614045e-02, 1.81320931e-04],
            [3.14801821e-02, 1.37165905e-04],
            [3.37097388e-02, 9.13237193e-05],
            [3.14477887e-02, 1.27573727e-04],
            [2.93856866e-02, 1.80119494e-04],
            [3.21778081e-02, 1.20482967e-04],
            [3.09253387e-02, 1.41234374e-04],
            [2.89037006e-02, 1.73351345e-04],
            [2.63603464e-02, 2.40557193e-04],
            [2.97531292e-02, 1.49838303e-04],
            [3.09406782e-02, 1.20147748e-04],
            [3.34269522e-02, 9.64184388e-05],
            [3.21702444e-02, 1.07523317e-04],
            [3.31884986e-02, 1.13888715e-04],
            [3.13634170e-02, 1.50539477e-04],
            [3.15433426e-02, 9.78224420e-05],
            [3.37264300e-02, 4.58819115e-05],
            [3.05134628e-02, 1.62163171e-04],
            [3.01990945e-02, 1.45595282e-04],
            [3.24193878e-02, 1.07040686e-04],
            [2.89361267e-02, 1.68982945e-04],
            [3.14644782e-02, 1.21687800e-04],
            [3.00928787e-02, 1.25225983e-04],
            [2.97172322e-02, 1.58535727e-04],
            [3.16525292e-02, 1.59825175e-04],
            [2.73133702e-02, 2.03033967e-04],
            [3.07012036e-02, 1.41624569e-04],
            [3.14037807e-02, 1.56153449e-04],
            [3.68941515e-02, 1.39717748e-05],
            [3.35195714e-02, 9.74448037e-05],
            [3.44766330e-02, 1.10075179e-04],
            [2.96521571e-02, 2.00544246e-04],
            [3.40437951e-02, 8.34663474e-05],
            [3.18787889e-02, 1.12408021e-04],
            [2.91018266e-02, 1.83205442e-04],
            [3.42752978e-02, 1.09834213e-04],
            [2.99147614e-02, 1.64588162e-04],
            [2.69411536e-02, 2.22056101e-04],
            [2.92384537e-02, 1.75857908e-04],
            [3.21497785e-02, 1.18536561e-04],
            [3.20368825e-02, 1.27615270e-04],
            [3.05481807e-02, 1.31411952e-04],
            [2.80671188e-02, 1.71808102e-04],
            [2.96667234e-02, 1.67225399e-04],
            [2.93501758e-02, 1.64861176e-04],
            [3.17621016e-02, 1.28950237e-04],
            [2.70245545e-02, 1.91540782e-04],
            [2.95737237e-02, 1.79733199e-04],
            [3.03960805e-02, 1.16566738e-04],
            [3.13360514e-02, 1.33077717e-04],
            [3.30154933e-02, 9.10127579e-05],
            [3.16792716e-02, 1.13576429e-04],
            [3.26114170e-02, 9.20815074e-05],
            [3.11348619e-02, 1.49516283e-04],
            [3.29058388e-02, 1.13296699e-04],
            [2.82928829e-02, 2.10688050e-04],
            [2.73916104e-02, 1.68613280e-04],
            [3.20345449e-02, 1.09580234e-04],
            [3.27864968e-02, 9.96904496e-05],
            [3.18321415e-02, 1.16487267e-04],
            [3.12894016e-02, 1.12140553e-04],
            [3.24296053e-02, 1.10626740e-04],
            [3.40294184e-02, 8.18386107e-05],
            [3.18478848e-02, 1.45856917e-04],
            [2.94398934e-02, 1.76937833e-04],
            [3.31926873e-02, 1.04409871e-04],
            [3.15933311e-02, 1.31747773e-04],
            [3.07172269e-02, 1.03464438e-04],
            [3.55747889e-02, 1.96620581e-05],
            [2.92958584e-02, 1.84935203e-04],
            [3.17684047e-02, 1.44769615e-04],
            [3.53566660e-02, 5.25826627e-05],
            [3.54608141e-02, 8.58596332e-05],
            [3.16629751e-02, 1.35231504e-04]]),
     array([24.7439626 , 26.79364649, 23.30621735, 25.88544187, 26.16516212,
            27.01394324, 26.83947621, 27.16550936, 26.85511915, 24.71923833,
            26.52105937, 27.15423597, 25.03154214, 27.06983254, 26.80507408,
            27.06375912, 26.75308124, 26.03802921, 26.42593564, 26.56606067,
            26.58913388, 25.91928702, 26.62054837, 27.08380522, 26.8304138 ,
            26.77038184, 25.75689091, 27.04403273, 26.53894323, 27.17986656,
            26.54127507, 27.04977078, 27.14626776, 26.5840375 , 24.24529354,
            26.81613757, 26.7933097 , 26.67516044, 27.05133283, 26.34441375,
            26.58213261, 26.16232585, 24.63998805, 26.7321408 , 27.01557656,
            27.05012009, 26.56552985, 27.13804685, 26.03940172, 26.91666735,
            25.44946429, 25.55795536, 27.14212241, 26.16767556, 23.79733361,
            26.57935618, 24.0558227 , 24.84364425, 26.38354823, 27.08304227,
            26.49573574, 24.59490383, 26.88842542, 25.09144139, 26.67468812,
            27.08913517, 26.9689296 , 26.90001437, 25.66433374, 26.84920639,
            26.78142729, 27.09341332, 24.90480615, 26.48431498, 25.87988504,
            27.16678603, 26.81867543, 27.04538565, 26.76592905, 26.8404071 ,
            26.69730679, 25.47037639, 24.13104541, 27.06444678, 26.94998506,
            27.13444124, 26.73975312, 27.05233036, 26.41082743, 26.2787913 ,
            26.63626223, 26.69839912, 27.11257119, 25.30682266, 24.14324917,
            26.37593843, 26.45404679, 25.48525069, 23.84274373, 26.99186463]),
     ('MT19937', array([2272821728,  855770064,  720798844, 2780792156, 1486914826,
             3650355937, 1328070557, 1318400635, 1736019877, 1429985833,
               48032620,  796816110, 1032495831, 2491838610, 1519277624,
             3189284807,  484389594, 2178720037, 3286384117,  500639894,
             3526867372, 2012742518, 2460992478, 1171701516, 3909667668,
             2228970849, 1768532439, 1470406543, 3196843240,  162088758,
              510595711,  454005131,   10787105, 1240234538,  960657403,
             2271992100, 3476248196, 3273991307, 4145696639, 2549434819,
              409471508, 2155272397,  357554663, 2877476731, 2375707784,
             4230559576, 2900246781, 2823551926, 2688376485, 2787870900,
             3272750161, 2678124295, 1934375694, 1238208536, 2904177243,
             2340167430,  516361993, 2390210466,   49400244, 4083783613,
              111861114, 2974709401, 4214125072, 2026627690, 1812152106,
              999719239, 1219349342,  231572937, 2324495134,   48513930,
             2992299177, 1221435820,  842496681,  951329903, 3288170589,
             4048621504, 2252396866, 1157471809, 1229439129,  125757986,
             2767025898, 2070924562, 2363780521, 3013282650, 1881068537,
             2370169961,  490844682, 2056526572, 1080324925, 2681797289,
             3074291066, 1676495728, 2888324500, 4050671359,  563588046,
             2787548080,  824747172, 1455856116,  865701207, 3191446520,
             2294867629, 1888843963, 4165968915,  736410661, 2563973911,
             3905712770, 2585754732, 2230666088,  942178601,  812572559,
             2921424084, 2231186694, 3522494975, 4247963666, 3938054092,
             3364016595,  111593144, 4041926637, 3839544957,  183699870,
             2068512099, 1362880725, 1600292546, 2014987721, 1147637885,
             3720835854, 2461376038, 3954942744, 2694177605, 2284950538,
             3017201090, 2269635442, 2440450157, 2866309630, 2508763903,
             1267445671, 1704727249, 2395752236,   23233286, 4010838645,
              937363217, 1159664455,  435630452, 3764275211,  354365428,
              219566878, 1056692346, 2645786186,  281831709,   60402544,
              477710277,  841863027, 2182113141, 2158739308, 1854385527,
             3799273364, 4262929313, 1221713764, 3297477229, 1504673689,
              488718462, 3316602587, 3475140844, 2221453559, 3454166380,
             3218272162, 4012709332, 1560425278, 3036073115,  171075923,
             2562235104, 2081868812,  745361076, 3106923182,  470147734,
             1436217822, 1624487260, 4046483899, 2020969093, 1039491165,
             3296453989, 3049612610,  779979739, 1877246110, 2371513687,
              759117193, 3751431596,  988101127,  489533693, 3861777682,
             2150298386, 3409046841, 1436060227, 1611007144, 3666429761,
                5413311, 3045458480, 4191515412, 3045961888, 3500141366,
             3939785223, 2163383912, 2745994588, 2414767888, 2143827190,
             3162426642, 2025470647, 3419365616, 1822974046, 4045802953,
             3390133410, 1756706406, 1631920509, 3335171616, 3835098762,
             2616137327, 3493111767, 3691542231,    4998193,  476777869,
             3006635881, 2445806270, 3722332957, 1335990696,  465900999,
             2265276898,  796225621, 2908071898, 3265156191, 1757449066,
              672674435, 1807771209,  285262911, 1095478446, 3330989079,
             1986821990,  829795625, 2027527440, 3874184827, 2019573272,
             2613177823, 1745095681,  968996147, 2386275511, 3056843084,
             3196043164, 1914311385, 1958435840, 3772913540, 3352068813,
             1044874397,  522109421,  864703675, 3213452302, 2156935925,
             3905751338, 1861522813, 2180728817, 3944467200, 1905564262,
             3022070730, 4072220970, 3182633997, 1992318548, 2507603054,
             2988750389, 1780533347, 3901665556,  118528500, 1250853413,
              670021531, 2903786340,   12358271,  695452593, 4294092090,
             3728875381, 2016900868, 3706685549, 3859699367,  463792760,
             2540545933, 2794403401,  103088509, 2892270797, 3061340979,
             1563314533, 3210317151, 2933835055, 4208838178, 2266837646,
             2652770884,  533474080,  634763490,  598029419, 1773839293,
             1942598800,  509040453,  590930765, 3492151706, 2495077947,
             2845005019, 1977095463, 2698499604,  930046116, 2020945622,
             1548451543, 1613467429, 1433463371, 2799477331, 2311295660,
             4294204759, 3578934745,   87839973, 3303640952,  342444770,
             2346649326, 3454123203, 2039666249, 2210486106, 2331380346,
              731135431, 3006100370, 1984825306, 3300776999, 3642732083,
             1377396882, 1170046845, 2949831608, 3734757222, 3679556584,
              745721069, 3637368155,  662768310, 4163197372, 1531189646,
             1238675776, 1609689529, 2988232407, 3028540594, 4115789588,
             3195769707, 3197867848, 4117279850, 1895065153,  126900299,
             3241612407, 2609823468, 1459441349, 3282756452, 1305924606,
              684312133, 2631252764, 2919181522, 1494913868, 1724161000,
             2976919616, 2105020858,  525063280, 2103648296, 2753659713,
              229483642, 3848250444, 4278176594,  627699787, 3485361245,
             3009587570, 3150704886, 1852809258, 3857736989, 2028232857,
             1263067498, 4125028164, 2022486232, 3899530141,  877236650,
             2423137570, 2227642719, 3910212503, 2479218584, 2840079915,
              426182848, 1446773523, 1313539523,  710995907,  350216957,
              225585336, 3843088424, 1116193264, 1086150853, 1025332656,
              460840310, 3115067317, 2952503523, 3095995065, 2015498860,
             1973925261, 4292348431, 2527512642,  323979722, 2364331712,
               17608791, 2561472551, 3263129047, 2902295900,  894416048,
              315471413,  628171966, 2393816377, 1128847385, 3488511667,
             3856955238, 3487571281,  550769468,  606539941, 1832161732,
             1655448916, 3338058053,  111859035, 1200438694, 2892310859,
             1372069352, 3031373882, 1102214530, 1148545842, 1379343155,
             4206418254, 3862576545, 4055163188, 2533021155, 4059692449,
             3806434194,  303799525, 1820496518,  456534791, 3887678320,
             1542667478, 1514746750, 3729450484,  878694664, 3869749658,
             2031373582, 3964632239, 3746895089, 4269217855, 1878686192,
             1916358074, 3805387950, 3548908420, 4196242864, 1202743972,
             1611899117,  147493346, 1549326895,  231180002, 3619305961,
              139005578, 1159914265, 1179880973, 2295460714, 2296474289,
             3547568962, 4280544244,   19870797, 3159210278, 3521381716,
             2635761017,  255086316, 3218103760, 3613538383, 2616324436,
             2488388992, 1366725759,  414820550, 1587432866, 1231897775,
             2323663267,  115680739, 4216655469, 2807103001,  170402032,
             4071408239, 2450813596,  643870045, 3531402779, 2505505633,
             3674520059, 1690165709, 3638654161, 3659484221, 1478815741,
             3972357985, 2302062980, 2015832555, 4000345941, 2619045890,
             2137525364,  302518918, 2375219927, 2137775397, 3145795928,
             3984793852, 2569780658,  557333161, 2511054721, 2307389311,
             2196493171, 1731903722, 1331717560,  675057316, 1410158447,
             2829499735,  242018510,  355017123,  895461764, 2447284104,
             2497889746,   24720517,  343673353, 1938779273, 1731394712,
             3200838289,  173796036,  999092396,  892935457, 2035224976,
              639140622, 3011301067,  212768705,  635065628, 1499078467,
             3656080310, 3867131079, 2103308765, 1860931452, 4091497532,
             2847343362, 1064371043, 3062681907, 4084346749,  531493290,
              646057866, 2027942963, 1012668499,  374913732, 1777178420,
             1250035001, 2632726940, 3125327228, 3674998912, 4036610404,
             3135899938, 1285533329, 1602182111, 3852071840,  716521507,
             1850849859, 3688544923, 1897764046, 1460654567, 2006429562,
              330239136, 3915209061,  964132725, 3680079570, 3474220765,
              817767429, 1717767419, 3537206444, 2823779393, 2816080428,
              997680695,  153125388, 2134992514, 1593845314,  972249391,
             3974158850, 1880463311, 3840040928, 3260329753, 1354957854,
             2407873891, 3935351381, 1499155732, 2950515525, 3342409717,
             1379697433, 1211717939, 3205006436, 2221855480,  174952879,
             1597942084, 3634957370, 1130144094, 3265519522, 2358773848,
             1496232399,  472921099, 3362466712, 4096119866,  332314258,
             2470137819, 2077827576, 1079266776, 1888273769, 2207910600,
             2208748033, 2429856681, 2916558962, 1066245126, 1245948596,
             2647264343, 1778659754, 2668048043, 1428333960,  563173395,
             2653531477, 3409815370, 3493773536, 4006866873, 1926989143,
              269552666, 3905871468, 3787235479, 4234431231], dtype=uint32), 203, 0, 0.0))

```python
fig, axes = plt.subplots(2, figsize=(10, 7), sharex=True)
#samples = sampler.get_chain()
samples = sampler.chain
labels = ["Cms", "m"]
for i in range(ndim):
    ax = axes[i]
    ax.plot(np.transpose(samples[:, :, i]), "k", alpha=0.3)
    ax.set_xlim(0, np.shape(samples)[1])
    ax.set_ylabel(labels[i])
    ax.yaxis.set_label_coords(-0.1, 0.5)

axes[-1].set_xlabel("step number");
```

```python
print(np.shape(samples))
samples = sampler.chain[:, 100:, :].reshape((-1, ndim))
print(np.shape(samples))
fig = corner.corner(samples, labels=["$Cms$", "$m$"],
                      truths=[Cms, slope])
```

```python
xl = np.linspace(10, 200, 500)
#print(xl)
for Cms_em, m_em in samples[np.random.randint(len(samples), size=100)]:
    pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms_em+m_em*xl)**2), color="k", alpha=0.1)
pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2), color="r", lw=2, alpha=0.8)
#pl.errorbar(x, y, yerr=yerr, fmt=".k")
pl.xlim(10, 200)
pl.ylim(0.03,0.12)
pl.errorbar(E_recoil[2::], sig_NR[2::], yerr=sig_NR_err[2::], fmt=".k", capsize=0)
```

```python
sampsize=100
evec = np.zeros((np.shape(xl)[0],sampsize))
#print(samples[np.random.randint(len(samples), size=sampsize)])
i=0
for Cms_em, m_em in samples[np.random.randint(len(samples), size=sampsize)]:
    v = np.sqrt(sigQnr_c_v(xl)**2+(Cms_em+m_em*xl)**2)
    #print(np.std(v))
    #print(np.mean(v))
    cent = np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2)
    evec[:,i] = v
    #print(i)
    #print(np.mean(v)+np.std(v))
    #print(np.mean(v)-np.std(v))
    i=i+1

upvec = np.mean(evec,axis=1)+np.std(evec,axis=1)
dnvec = np.mean(evec,axis=1)-np.std(evec,axis=1)
```

```python
pl.plot(xl, np.sqrt(sigQnr_c_v(xl)**2+(Cms+slope*xl)**2), color="r", lw=2, alpha=0.8)
pl.plot(xl, upvec, color="r",linestyle="--", lw=2, alpha=0.8)
pl.plot(xl, dnvec, color="r",linestyle="--", lw=2, alpha=0.8)
#pl.errorbar(x, y, yerr=yerr, fmt=".k")
pl.xlim(10, 200)
pl.ylim(0.03,0.12)
#args=(E_recoil[2::], sig_NR[2::], sig_NR_err[2::],sigQnr_c_v))
pl.errorbar(E_recoil[2::], sig_NR[2::], yerr=sig_NR_err[2::], fmt=".k", capsize=0)
```

```python
#save the results for the MS fit
path='{}/{}/'.format('mcmc','edwdata')

filename = 'data/mcmc_fits.h5'

#remove vars
f = h5py.File(filename,'a')
exCms = path+'Cms' in f
exm = path+'m' in f
exsamp = path+'samples' in f
exsampsize = path+'sampsize' in f
exEr = path+'Er' in f
exCsig_u = path+'Csig_u' in f
exCsig_l = path+'Csig_l' in f
exSigss = path+'Sigss' in f

if exCms:
  del f[path+'Cms']
if exm:
  del f[path+'m']
if exsamp:
  del f[path+'samples']
if exsampsize:
  del f[path+'sampsize']
if exEr:
  del f[path+'Er']
if exCsig_u:
  del f[path+'Csig_u']
if exCsig_l:
  del f[path+'Csig_l']
if exSigss:
  del f[path+'Sigss']

dset = f.create_dataset(path+'Cms',np.shape(Cms),dtype=np.dtype('float64').type)
dset[...] = Cms
dset = f.create_dataset(path+'m',np.shape(slope),dtype=np.dtype('float64').type)
dset[...] = slope
dset = f.create_dataset(path+'samples',np.shape(samples),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = samples
dset = f.create_dataset(path+'sampsize',np.shape(sampsize),dtype=np.dtype('float64').type)
dset[...] = sampsize
dset = f.create_dataset(path+'Er',np.shape(xl),dtype=np.dtype('float64').type, \
compression="gzip")
dset[...] = xl
dset = f.create_dataset(path+'Csig_u',np.shape(upvec),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = upvec
dset = f.create_dataset(path+'Csig_l',np.shape(dnvec),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = dnvec
dset = f.create_dataset(path+'Sigss',np.shape(xl),dtype=np.dtype('float64').type, \
compression="gzip",compression_opts=9)
dset[...] = sigQnr_c_v(xl)

f.close()
```

```python

```


# Section IV. DYNAMICS AND CONTROL

# Chapter 13. What are Dynamics and Control?

The purpose of dynamics is to study how time and force act on a
mechanism, while the purpose of controls is to study how a system should
respond to errors and disturbances. At this point, we have described how
to reason about the positions of robots and how to generate continuous
paths. But actually executing those paths requires us to think much more
carefully about the physics of robot mechanisms, and the role of time
and velocity. Even the strongest robots cannot instantaneously change
velocities, and driving and flying robots cannot move sideways.

It is through the use of control that an industrial robot can move to a
position with sub-millimeter accuracy, and an aircraft can fly for
thousands of kilometers but land on an airstrip a few meters wide. It is
also a means for understanding locomotion and reflexes in the biological
sensorimotor system. It is important to note that both dynamics and
control are deep fields of study that are more than one hundred years
old, and yet they are still undergoing significant change! Classical
approaches rely heavily on mathematical analysis, while more modern
approaches to control rely on computation as a key tool. Due to this
depth, to master these fields requires years of specialized
investigation, and this part of the book can only survey the main points
as they relate to robotics. We will see some of both the historical and
modern approaches in the next few chapters.

In the topic of dynamics we will cover 1) basic terminology of dynamical
systems, 2) simple dynamical systems from physics, 3) the dynamics of
articulated robots, and 4) contact mechanics. In controls will describe
methods for 1) analyzing stability of controlled dynamical systems, 2)
controlling articulated robots with high accuracy, and 3) generating
feasible and optimal control strategies.

Basic terminology
-----------------

A *dynamical system* is one in which the state of the system changes
continuously over time. The notion of *state* is similar to that of a
configuration, although it can also include terms like joint velocities.
In this section, we let $x \in \mathbb{R}^n$ be the quantity defining
the *state* of the system. Robots are able to apply forces and otherwise
*alter the rate of change of the state* using their actuators. We define
the *control* (aka control input) as $u \in \mathbb{R}^m$, where $m$ is
the number of independently chosen variables.

For example, in a 6-joint industrial robot, the state of the robot is
typically considered as $x=(q,v) \in \mathbb{R}^{12}$. The inclusion of
a velocity term allows us to express how the robot's momentum affects
its future movement, and how joint forces affect velocities. The control
variable $u$ can take on many forms, depending on how the controller is
designed. For example, if the controller takes desired joint velocities
as inputs, the control variable is $u=(v_{d1},\ldots,v_{d6})$ where
$v_{di}$ indicates the desired velocity of joint $i$. On the other hand,
if it takes joint torques as inputs, the control variable is
$u=(\tau_{1},\ldots,\tau_{6})$.

The standard terminology for modeling a dynamical system is an
expression relating the state and control to the derivative of the
state. In the case that we do not have the ability to control a system,
we have an *uncontrolled dynamics equation* of the form
$$\dot{x} = f(x).
\label{eq:UncontrolledDynamicEquation}$$ If the system can indeed be
controlled by a control $u$, we have a *controlled dynamics equation*:
$$\dot{x} = f(x,u)
\label{eq:DynamicEquation}$$ where $x$ is the state, $u$ is the control,
and $\dot{x}$ is the time derivative of the state $\frac{dx}{dt}$. The function $f$ is
known as the dynamics of the system. These equations are also known as
the *equations of motion*.

It is important to note that $x$ and $u$ are actually *functions of
time*. If we need to explicitly represent the dependence on time we
shall write $x(t)$ and $u(t)$. Hence, the dot notation is simply the
time derivative $\dot{x} = \frac{d}{dt}x$. (Or more explicitly,
$\dot{x}(t) = \frac{d}{dt}x(t)$.) Also note that from this chapter
onward, all variables except for time will be vector quantities unless
stated otherwise.

It should be noted that we have introduced the terms "dynamical" and
"dynamics" which should be taken to be *almost* synonyms. Being quite
pedantic, we will say something is dynamic when it changes over time,
while something is dynamical if it *regards* dynamics. When we say
"dynamical system" it means that the system regards a dynamic quantity
(the state) but the system itself is not changing over time. We shall
also sometimes say "dynamic equation" which is a synonym with "dynamics
equation" and is chosen according to author's preference. But why don't we
call it a "dynamical equation"? Let's just move on, and let the grammar
Nazis squabble over terminology\...

### Open-loop and closed-loop control

Given a dynamics function $f$, our job is to decide upon the control $u$
in order to accomplish some desired task. There are two primary types of
controls: 1) *open-loop* control, in which case $u \equiv u(t)$ only
depends on time, and 2) closed-loop control, in which case
$u \equiv u(x)$ depends on state. (It may also depend on time, in which
case we write $u \equiv u(x,t)$.)

The significance of closed-loop control is that the control function can
"observe" the state of the system and change accordingly in order to
achieve the desired task. The control function in this case is also
known as a *control policy*. This allows a robot to adapt to
disturbances to achieve high accuracy and to prevent veering off-course.
However, for purposes of planning, it will often be easier to compute an
open-loop trajectory. Later, we shall see how to convert an open loop
plan into a closed-loop one via the approach of model predictive
control.

### Discrete-time systems

In many cases it is convenient to talk about *discrete-time* systems in
which time is no longer a continuous variable but a discrete quantity
$t=0,1,2,\ldots$, and the dynamics are specified in the form
$$x_{t+1} = f(x_t,u_t).
\label{eq:DiscreteTimeDynamicEquation}$$
Here, the control is allowed to
change only at discrete points in time, and the state is only observed
at discrete points in time. This more accurately characterizes digital
control systems which operate on a given clock frequency. However, in
many situations the *control frequency* is so high that the
continuous-time
model ($\ref{eq:DynamicEquation}$) is appropriate.

### Converting higher-order dynamic systems into first-order systems

Often, we shall see systems of the form

$$\ddot{x} = f(x,\dot{x},u)
\label{eq:SecondOrderSystem}
$$

which relate state and controls to *accelerations* of the state $\ddot{x} = \frac{d^2 x}{dt^2}$.  This does not seem to satisfy our definition of a dynamic system, since we've never seen a double time derivative.  However, we can employ a *stacking trick* to define a first order system, but of twice the dimension.  Let us define the stacked state vector

$$y \equiv \begin{bmatrix} x \\ \dot{x} \end{bmatrix}.$$

Then, we can rewrite ($\ref{eq:SecondOrderSystem}$) in a first-order form as:

$$\dot{y} = g(y,u)$$

where $g(y,u) \equiv f(x,\dot{x},u)$ simply "unstacks" the state and velocity from $y$.  Now all of the machinery of first-order systems can be applied to the second order system.  This can also be done for dynamic systems of order 3 and higher, wherein all derivatives are stacked into a single vector.

(Note that to define an initial state $y_0$, we will need to specify the initial position $x_0$ as well as the velocity $\dot{x}_0$.)

ODE integration
--------------------------

Consider a controlled, continuous time dynamic system $\dot{x}= f(x,u)$, with $x\in \mathbb{R}^n$
and $u\in \mathbb{R}^m$.  Suppose we are given an _initial state_ $x_0$ encountered at $t=0$, and a control $u(x,t)$ defined for $t \geq 0$.  Solving for the state trajectory requires solving an **initial value problem** of an **ordinary differential equation** (ODE):

$$\text{Find }x(t) \text{ for } t > 0 \text{ subject to }\dot{x}(t) = g(x(t),t) \text{ and } x(0)=x_0. $$

where $g(x,t) \equiv f(x,u(x,t))$ is a time-varying dynamics function.  (Note that we have applied the simple trick of pushing the control $u$ inside $g$, which turns the controlled system into an uncontrolled system.)

For some limited classes of dynamic systems and control trajectories we can solve the ODE analytically.  We shall see some of these solutions for the [Dubins car](#Dubins-car) and [linear time invariant systems](#Linear-Time-Invariant-Systems).  However, in the general case, we shall need to resort to numerical methods.  This problem is known as **ODE integration** (also known as **simulation**).

### Euler's method

The simplest numerical integration technique is known as **Euler's method**, which divides time into a sequence of small steps of $\Delta t$ in which the dynamics are assumed constant.  Each subsequent movement simply displaces the state by the first-order approximation $\Delta t g(x(t),t)$.  What emerges is a sequence of states $x_0,\ldots,x_N$ given by:

$$x_1 = x_0 + \Delta t \cdot g(x_0,\Delta t\cdot 0)$$

$$x_2 = x_1 + \Delta t \cdot g(x_1,\Delta t\cdot 1)$$

$$...$$

$$x_N = x_{N-1} + \Delta t \cdot g(x_{N-1},\Delta t\cdot(N-1))$$

This is a widely-used technique due to its straightforward implementation, and it is also easy to analyze.  Code for this method is given below.

```python
import numpy as np

def integrate_euler(f,x0,N,dt,t0=0):
    """Approximates the trajectory resulting from the initial value problem x'=f(x,t)
    using euler's method.

    Arguments:
    - f(x,t): a function of state and time giving the derivative dx
    - x0: the initial state at time t0, x(t0)=x0
    - N: the number of steps to take
    - t0: the initial time

    Return value: a trajectory ([t0,t1,...,tN],[x0,x1,...,xN])
    """
    t = t0
    x = x0
    ts = [t0]
    xs = [x0]
    for i in range(N):
        dx = f(x,t)
        x = x + dt*dx
        t = t + dt
        ts.append(t)
        xs.append(x)
    return (ts,xs)
```

The below code plots the result of Euler's method applied to a simple 2D particle under a gravity field, with the control u giving an external acceleration (here, $u(t)=0$ for all $t$).

```python
# Code for plotting Euler integration of a 2D particle
%matplotlib inline
%config InlineBackend.figure_format = 'svg'
import matplotlib.pyplot as plt
import math

g = -9.8  #the gravitational constant
#the 4D state is [px,py,vx,vy]
def zero_control(x,t):
    return np.zeros(2)
#you might try replacing zero_control with sin_control below and seeing what happens...
def sin_control(x,t):
    return np.array([5.0*math.sin(t*15),0])
def f_grav(x,t):
    u = zero_control(x,t)
    return np.hstack((x[2:4],u + np.array([0,g])))
#initial px,py,vx,vy (at origin, with forward velocity 1, upwards velocity 10)
x0 = np.array([0.0,0.0,1.0,10.0])
#integrate for total time T
T = 2.0
#compare several time steps
dts = [0.025,0.05,0.1,0.2]
for dt in dts:
    N = int(T/dt)
    times,points = integrate_euler(f_grav,x0,N,dt)
    times = np.array(times)
    points = np.array(points)
    plt.plot(points[:,0],points[:,1],label='Euler, dt='+str(dt))
times = np.linspace(0,T,50)
ground_truth = np.vstack((x0[2]*times,x0[3]*times+0.5*g*times**2)).T
plt.plot(ground_truth[:,0],ground_truth[:,1],label='Exact')
plt.xlabel('x')
plt.ylabel('y')

plt.legend()
plt.show()
```

Note that the accuracy of the integration depends heavily on the timestep chosen.  In general, the smaller
the timestep, the more accurate the integration will be.  More formally, define the *integration error*
as $\epsilon(t) = x(t) - x_{\lfloor t/ \Delta t\rfloor}$.
Higher errors result as

* The spatial variation of the dynamics function is large.  More precisely, the error will grow if the Jacobian of f (in either x or u) are large.

* The time $t$ is large (i.e., the error generally gets worse over time).

* $\Delta t$ is large.

### Higher order integrators

A great deal of work has investigated ODE integration techniques that are more accurate than Euler integration.  Rather than approximate the dynamics function as a first order Taylor expansion, they may use higher order terms to achieve lower approximation error.  A popular class of higher order methods are the **Runge-Kutta methods**, which use multiple evaluations of the dynamics function to achieve far lower error than standard Euler integration.
More advanced methods may also use **adaptive step size**, which take smaller steps where the dynamics function is found to be more highly varying.

Many numerical libraries have a variety of integrators to choose from.  For example, the below plot shows an integrator used in Scipy library, which is, in fact, exact for this dynamics function.

```python
# Code for the plot using scipy ODE integration
def integrate_scipy(f,x0,N,dt,t0=0):
    """Same arguments and return type as euler, but using the integrators in the Scipy library"""
    from scipy.integrate import ode
    r = ode(lambda t,x:f(x,t))  #need to swap the order of arguments for scipy's ode function
    r.set_integrator('dopri5')   #lots of options here... see function documentation
    r.set_initial_value(x0, t0)
    t = t0
    ts = [t0]
    xs = [x0]
    for i in range(N):
        x = r.integrate(t+dt)
        t += dt
        ts.append(t)
        xs.append(x)
    return (ts,xs)

dt = 0.1
times,points = integrate_scipy(f_grav,x0,int(T/dt),dt)
times = np.array(times)
points = np.array(points)
plt.plot(points[:,0],points[:,1],label='Scipy, dt='+str(dt))
plt.plot(ground_truth[:,0],ground_truth[:,1],label='Exact')
plt.xlabel('x')
plt.ylabel('y')

plt.legend()
plt.show()
```

### Stability, convergence, and divergence

A dynamic system is said to be:

* **Stable** for some class of initial states if its solution trajectories do not
grow without bound,

* **Unstable** (or **divergent**) if the trajectories grow without bound, and

* **Convergent** if the solution trajectories approach a single point.

A *stable point* is a state $x$ such that for some neighborhood
of $x$, the ODE is convergent toward $x$. A necessary condition for a
point to be stable is that it is an *equilibrium point*.

> **Equilibrium point**. For a continuous time dynamical system, a state $x$ such that $\dot{x} = f(x) = 0$.  For a discrete time dynamical system, a state that satisfies $x = f(x)$.

All stable points are equilibria, but the converse is not true. For example, in the system $\dot{x} = x$, $x=0$ is an equilibrium point, but it is not stable.  Through inspection, observe that $x(t) = c e^t$  is a solution to this differential equation.  Hence, a tiny perturbation $x(0) = \epsilon$ would yield the solution trajectory $x(t) = \epsilon e^t$, which grows without bound as $t$ grows.

The trajectories derived from Euler integration can be divergent even when the underlying system itself is stable or convergent.  As an example, consider the damped harmonic oscillator system $$\ddot{x} = -10x - \dot{x}$$.

With the initial condition $x(0)=1$, $\dot{x}(0)=0$, the solution trajectory is $x(t) = e^{-t/2}\cos(\omega t)$ with $\omega=\sqrt{10-1/2^2}$.  But see what happens when this is integrated using Euler's method:

```python
# Code for integration of a damped harmonic oscillator with Euler's method
def f_harmonic_oscillator(x,t):
    return np.array([x[1],-10*x[0]-x[1]])

#initial x,dx
x0 = np.array([1.0,0.0])
#integrate for total time T
T = 4.0
#compare several time steps
dts = [0.025,0.1,0.2]
for dt in dts:
    N = int(T/dt)
    times,points = integrate_euler(f_harmonic_oscillator,x0,N,dt)
    #times,points = integrate_scipy(f_harmonic_oscillator,x0,N,dt)
    times = np.array(times)
    points = np.array(points)
    #plt.plot(points[:,0],points[:,1],label='Euler, dt='+str(dt))
    plt.plot(times,points[:,0],label='Euler, dt='+str(dt))
times = np.linspace(0,T,100)
d = 0.5
w = math.sqrt(10-d**2)
ground_truth = np.vstack((np.multiply(np.exp(-d*times),np.cos(times*w)),
                          -d*np.multiply(np.exp(-d*times),w*np.sin(times*w)))).T
#plt.plot(ground_truth[:,0],ground_truth[:,1],label='Exact')
plt.plot(times,ground_truth[:,0],label='Exact')
plt.xlabel('t')
plt.ylabel('x')

plt.legend()
plt.show()
```

When the time step is small, the integrated trajectory does indeed converge torward 0, like the exact solution. However, at $\Delta t=0.1$, the solution is oscillatory between $[-1,1]$ and never converges.  At $\Delta t = 0.2$, the solution "blows up" toward infinity!  This is a serious problem for simulation, since we would like to avoid the computational expense of taking tiny steps, but while also integrating accurately.

In fact there are systems that are stable everywhere for which Euler's
method is unstable everywhere! An example is the oscillator:
$$\begin{bmatrix}\dot{x}  \\ \dot{y} \end{bmatrix}  = \begin{bmatrix}0 & -1 \\ 1& 0\end{bmatrix} \begin{bmatrix}x  \\ y \end{bmatrix}.$$
Here, the flow vector at a point is always perpendicular and CCW to the
vector from the origin to that point. The solution trajectories are
circles $(r \cos (t - \theta), r \sin (t - \theta))$, where $(r,\theta)$
are the polar coordinates of the initial point. If we were to
approximate this using Euler integration, each integration step brings the state
further and further from the origin, spiraling outward without bound. Taking
smaller time steps helps a little, but cannot completely remedy the problem.

```python
#Code for plotting the phase space of a pure oscillator
def f_oscillator(x,t):
    return np.array([-x[1],x[0]])
X, Y = np.meshgrid(np.arange(-3, 3, .5), np.arange(-3, 3, .5))
UV = np.array([f_oscillator([x,y],0) for x,y in zip(X,Y)])
U = UV[:,0]
V = UV[:,1]
plt.quiver(X, Y, U, V)

#compare several time steps
T = 8.0
dts = [0.025,0.1,0.25]
x0 = np.array([1,0])
for dt in dts:
    N = int(T/dt)
    times,points = integrate_euler(f_oscillator,x0,N,dt)
    #times,points = integrate_scipy(f_harmonic_oscillator,x0,N,dt)
    times = np.array(times)
    points = np.array(points)
    plt.plot(points[:,0],points[:,1],label='Euler, dt='+str(dt))
plt.legend()
plt.show()
```

Simple dynamic systems
-----------------------------------
### Basic physics: mass, force, and torque

Newton's laws

F = m a

Torques and moment arms

### Particle driven by forces
A 1D particle with mass $m$, position $p$ and velocities $v$, controlled by forces $u$, follows Newton's laws under the second-order controlled dynamics:
$$\ddot{p} = u / m$$

This problem can be modeled
with a state $x = (p,v) \in \mathbb{R}^2$ and control
$u = f \in \mathbb{R}$ with the dynamics equation

\begin{equation}
\dot{x} \equiv \begin{bmatrix} \dot{p}\\ \dot{v} \end{bmatrix} = f(x,u) = \begin{bmatrix}v \\ f/m \end{bmatrix}. \label{eq:PointMass}
\end{equation}

This function $f$ can be thought of as a *vector
field* that maps each 2D point to a 2D vector. If we plot this vector
field on the $(p,v)$ plane for various values of $f$, we observe a few
things. First, it is invariant to $p$. Second, the value of $f$ varies
the length and direction of the vectors in the $v$ direction.

For any initial state $x_0=(p_0,v_0)$ under a constant forcing
$u(t) = f$, the velocity of the solution trajectory $x(t)$ can be
determined through simple integration:
$$v(t) = v_0+\int_0^t f/m dt = v_0 + t f/m.
\label{eq:PointMassVelocity}$$ Continuing with the position, we see that
$$p(t) = p_0+\int_0^t v(t) dt = p_0+\int_0^t (v_0 + t f/m) dt =p_0 + t v_0 + \frac{1}{2}t^2 f/m.
\label{eq:PointMassPosition}$$ This means that the velocity of the
particle increases or decreases over time according to a linear function
with slope depending on $f$, and its position takes on a parabola
trajectory over time. Note that this model generalizes to any point
particle in $n$-D space, except that position, velocity, and force
become vector quantities. The state is then a $2n$-D vector and control
is $n$-D.

Now, let us suppose we wish to drive the particle from some position to
another (say, from 0 to 1) while starting and stopping at 0 velocity.
Can we use a constant force to do so? We start with $x(0)=(0,0)$ and
wish to achieve $x(T)=(1,0)$ at some future time $T$. Well,
by (\ref{eq:PointMassPosition}) we would need $T^2 f/m = 1$, but
by (\ref{eq:PointMassVelocity}), we would need $T f / m = 0$. This is
a contradiction, so we could not reach this other state via a constant
force.

Can we use a linear interpolation instead? If we define $u=t/T$ as the
interpolation parameter, such a trajectory would have
$v(t) = 0\cdot (1-u) + 0\cdot u = 0$ and
$p(t) = 0\cdot (1-u) + 1\cdot u = t/T$. However, this trajectory does
not satisfy dynamic constraints for any value of $t>0$ and any value of
$f$!

There are a couple ways to solve this problem. One is to make $f$ a
closed-loop control, such as the PD controller
$f(t) \equiv u(x(t)) = -k_P (p-1) - k_D v$. We will show when we discuss [PID control](Control.ipynb) that for certain constants $k_P$ and $k_D$,
this choice can be shown to force the system to converge toward the
target $(1,0)$. Another is to design a clever open loop control that
satisfies the endpoint constraints and the dynamic constraints, such as
$T = 2$, $f(t) = 1$ for $t\leq 1$ and $f(t) = -1$ for $1 < t \leq 2$.
This control accelerates the system to the point $(p,v)=(0.5,1)$ at
$t=1$, and then decelerates to $(1,0)$ at $t=2$. We shall see more
general ways of designing such control functions using the optimal
control methods presented in [later chapters](OptimalControl.ipynb).

### Pendulum swing-up

The pendulum swing up problem asks to control an actuator with limited
torque to drive a pendulum with progressively larger and larger
momentum, until it can then reach and stabilize about the vertical
position. The pendulum is assumed to with a point mass at the end of a
bar of length $L$, with the other end fixed to rotate about the origin.
The system has a state space of $x=(\theta,
\omega)$, with $\theta$ the CCW angle of the mass with respect to the
$x$ axis, and $\omega$ its angular velocity. The start state is
$x=(3\pi/2,0)$ and the goal state is $x=(\pi/2,0)$.

************

<div class="figcaption"><a name="fig:PendulumStrongMotor">Figure 1.</a>
Illustrating the dynamics of a controlled pendulum moving from the
down ($\theta = 3\pi/2 \approx 4.71$) to the up
($\theta = \pi/2 \approx 1.57$) position. If the motor is strong enough,
it can proceed almost directly toward the goal state. The legend
displays torque requirement needed to implement such a controller.
</div>

************

The actuator $u$ applies a torque about the origin, and is usually
assumed bounded $|u|\leq u_{max}$. The force of gravity produces a
torque of magnitude $mg L \cos \theta$ about the origin. Since the
moment of inertia of the point mass is $mL$, the overall acceleration of
the system is: $$\ddot{\theta} = g \cos \theta + u/mL.$$ Writing this in
canonical form, we have
$$\dot{x} \equiv \begin{bmatrix}\dot{\theta}\\\ddot{\omega}\end{bmatrix} = f(x,u) = \begin{bmatrix}{\omega}\\{g \cos \theta}\end{bmatrix}  + u \begin{bmatrix}1 \\ 1/mL \end{bmatrix}.$$
This is a nonlinear equation without an analytical solution.

With $u_{max}$ sufficiently large ($u_{max} > mLg$) the motor has enough
strength to hold the pendulum steady horizontally, and it is possible to
drive it monotonically to the goal
([Fig. 1](#fig:PendulumStrongMotor)). But if the maximum torque is
lowered beyond some amount, the motor can no longer achieve sufficient
inertia to raise the pendulum without "pumping", like a child on a
swing, to increase the kinetic energy of the system. As we shall see when we discuss [bang-bang control](OptimalControl.ipynb), the optimal controller will then
alternate between extreme controls to build up enough kinetic energy to
reach the goal. This implies that the time evolution of the system will
switch between the flow fields shown in
[Fig. 2](#fig:PendulumWeakMotor).

************

|Max CW|Max CCW|
|----|----|
|  |  |

<div class="figcaption"><a name="fig:PendulumWeakMotor">Figure 2.</a>
The flow fields corresponding to minimum (left) and maximum (right)
controls for a pendulum swing-up problem with unit mass, unit length,
and torque bounded at
$|u| \leq 5$ N$\cdot$m.
    </div>

************

### Cart-pole

The cart-pole problem is a toy underactuated system in which a cart that
can translate in $x$ direction needs to swing up and/or balance a pole
attached to it with a pin joint
([Fig. 3](#fig:Cartpole)). Its control has been studied quite
extensively, and it has similar dynamics to the Segway mobility
scooters.

************

<div class="figcaption"><a name="fig:Cartpole">Figure 3.</a>
    Illustration of the cart-pole problem.
</div>

************

In this problem, the system's configuration has two parameters
$q=(q_1,q_2)$ which denote the $x$ translation of the cart and the angle
of the pole, respectively. In the below convention we treat the
cart-pole as a PR robot, so that $q_2$ is the CCW angle of the pole from
the $x$ axis. In the balancing task, we wish to design a controller to
maintain the state near the unstable equilibrium point $q_2=\pi/2$ under
disturbances. In the swing-up task, we wish to go from $q_2=-\pi/2$ to
$\pi/2$. (Keep in mind that the topology of $q_2$ is SO(2), so the pole
can swing either left or right.)

This is a highly dynamic system where the cart's motors can apply forces
$u_1$ in the positive and negative $x$ direction. Optionally, the pole
could apply torques $u_2$, but it is typical to enforce $u_2=0$ so that
the pole swings passively. The cart and pole have masses $m_1$ and $m_2$
respectively, and the pole is assumed to have all of its mass
concentrated at a point distance $L$ away from the pin.

In [Chapter 14](RobotDynamics.ipynb), we shall derive the equations
of motion for the cart-pole system to be the second-order system of
equations:

$$\begin{aligned}
(m_1+m_2) \ddot{q_1} -\frac{m_2 L}{2} \ddot{q}_2  \sin q_2 - \frac{m_2 L}{2} \dot{q}_2^2  \cos q_2 = u_1 \\
-\frac{m_2 L}{2} \ddot{q}_1  \sin q_2 + \frac{m_2 L^2}{4} \ddot{q}_2 + m_2 g \cos q_2 = u_2
\end{aligned}$$

where $g$ is the gravitational constant. Notice here
that the accelerations $\ddot{q}_1$ and $\ddot{q}_2$ are coupled, in
that they appear in both equations. Solving this system of equations, we
obtain a solution:

$$\begin{bmatrix}{\ddot{q}_1}\\{\ddot{q}_2}\end{bmatrix} =
\frac{1}{d} \begin{bmatrix}
\frac{m_2 L^2}{4} & \frac{m_2 L}{2} \sin q_2 \\
\frac{m_2 L}{2} \sin q_2 & m_1+m_2
\end{bmatrix}
\begin{bmatrix}{u_1 + \frac{m_2 L}{2} \dot{q}_2^2 \cos q_2}\\{u_2-m_2 g \cos q_2}\end{bmatrix}$$

with $d= \frac{m_1 m_2 L^2}{4}+\frac{m_2^2 L^2}{4} \cos^2 q_2$. For any
given choice of $u_1$ and $u_2$, this can then be integrated to obtain
solution trajectories.

The cart-pole system is highly sensitive to the behavior of the cart.
[Fig. 4](#fig:CartpoleSpin) displays the behavior of the swing-up
problem under 1.5 sinusoidal movements of the cart with amplitude 0.5.
Each plot shows a slighly different period. In this setup, the pole
swings over the upright position only for periods in approximately the
range $[1.12,1.29]$. There is another range of periods where the pole is
swung about the upright position in the range $[1.32,1.39]$.

*************

|Period 1.288s | Period 1.5s |
|----|----|
|  |  |

<div class="figcaption"><a name="fig:CartpoleSpin">Figure 4</a>
    Behavior of the cart pole problem as a function of time. Slightly
changing the period of the cart's movement from 1.288 s to 1.5 s fails
to swing the pendulum past the upright position. A good swing-up
controller might use a period of 1.288 and then switch to a stabilizing
controller around
$t=2$.
    </div>

*************

### Dubins car

A Dubins car model approximates the mobility of a standard 2-axle car
moving on a flat surface, ignoring accelerations. In this model,
$(p_x,p_y)$ is the center of its rear axle, $\theta$ is its heading, and
$L$ is the distance between the front and rear axles. The control
$u=(v,\phi)$ specifies the velocity $v$ and the steering angle of the
front wheels $\phi$. The dynamics of this system are given as follows:
$$\dot{x} \equiv \begin{bmatrix}{\dot{p}_x}\\{\dot{p}_y}\\{\dot{\theta}}\end{bmatrix} = f(x,u) = \begin{bmatrix}{v \cos \theta}\\{v \sin \theta}\\{\frac{v}{L}\tan \phi}\end{bmatrix}$$
Note that the velocity vector is always parallel to the heading
$(\cos \theta,\sin \theta)$, and the turning rate $\dot{\theta}$ depends
on both the steering angle and the velocity. For constant $u$, the
position $(p_x,p_y)$ traces out straight lines (with $\phi=0$) or arcs
(with $\phi\neq 0$).

Typically, the control is subject to bounds
$v_{min} \leq v \leq v_{max}$ and $|\phi| \leq \phi_{max}$. With these
limits, the vehicle has a minimum turning radius of
$\frac{1}{L \tan \phi_{max}}$. The vehicle cannot move sideways, and
must instead perform "parallel parking" maneuvers in order to move in
the state-space direction $(-\sin \theta,\cos \theta,0)$.

Linear time invariant systems
-----------------------------

In general, the $f$ function may be nonlinear in its arguments. However,
a widely studied class of dynamical system is the *linear,
time-invariant* (LTI) system. In an LTI system, the dynamics equation
takes on the form $$\dot{x} = Ax + Bu$$ where $A$ and $B$ are constant
matrices of size $n \times n$ and $n \times m$, respectively. This type
of system is easily analyzed using results from linear algebra and can
represent a wide range of dynamic behavior.

For example, the 1D point
mass system (\ref{eq:PointMass}) can be represented as an LTI system with:
$$\dot{x} \equiv \begin{bmatrix}\dot{p} \\ \dot{v} \end{bmatrix} = \begin{bmatrix}0&1\\0&0\end{bmatrix} \begin{bmatrix}p \\ v \end{bmatrix} + \begin{bmatrix} 0 \\ 1/m \end{bmatrix}u$$

In the discrete-time
form (\ref{eq:DiscreteTimeDynamicEquation}), an LTI system takes the
form $$x_{t+1} = A x_t + B u_t.$$ A continuous-time LTI system can be
converted to an equivalent discrete-time LTI system through integration.

For example, the point mass system with time step $\Delta t$ and
constant control can be represented in discrete time as
$$\begin{aligned}
x_{t+1} &\equiv \begin{bmatrix}{p(t+\Delta t)}\\{v(t+\Delta t)}\end{bmatrix} = \begin{bmatrix}{p(t) + \Delta t v(t) + \frac{1}{2} f /m\Delta t^2}\\{v(t)+\Delta t f/m}\end{bmatrix} \\
& = \begin{bmatrix}1 & \Delta t \\ 0 & 1\end{bmatrix} x_t + \begin{bmatrix}{\frac{1}{2}\Delta t^2 / m}\\{\Delta t/m}\end{bmatrix} u_t
\end{aligned}$$

Moreover, nonlinear systems can be approximated by an LTI system about
any stable point in state space using linearization. Consider
linearizing a system of the form $\dot{x} = f(x) + g(x)u$ about state
$x_0$ and control $u_0$. Also assume that $u_0$ applied at $x_0$ leads
to no derivative (i.e., $f(x_0)+g(x_0) u_0=0$). Perform a change of
variables to $(\Delta x, \Delta u)$ such that $x = x_0 + \Delta x$ and
$u = u_0 + \Delta u$. Then $$\begin{aligned}
\dot{x} & = \dot {\Delta x} = (f(x_0)+g(x_0) u_0) + \left(\frac{\partial f}{\partial x}(x_0) + \frac{\partial g}{\partial x}(x_0)u_0\right) \Delta x + g(x_0) \Delta u \\
 & = \left(\frac{\partial f}{\partial x}(x_0) + \frac{\partial g}{\partial x}(x_0)u_0\right) \Delta x + g(x_0) \Delta u
\end{aligned}$$ This is LTI in $(\Delta x,\Delta u)$ with
$A=\frac{\partial f}{\partial x}(x_0) + \frac{\partial g}{\partial x}(x_0)u_0$
and $B=g(x_0)$.

Noise, uncertainty, disturbances, errors
----------------------------------------

Besides handling the differential constraint of the dynamics function,
the purpose of control is to handle deviations from an idealized state
or trajectory. These deviations are in various contexts called noise,
bias, uncertainty, disturbances, or errors. When they do occur, a
variety of problems could happen: the robot could fail to reach a goal,
hit an obstacle, reach an unrecoverable state, or even run into a
person! A *robust* planner or controller is designed to produce
high-quality behavior even when such deviations exist. It is important
to recognize *errors are a fact of life* for all robots outside of
tightly controlled industrial environments.

Generally speaking, errors can be characterized as being either *noisy*
or *systematic*. A noisy error is one obeys no obvious pattern each time
it is measured. A systematic error is one that does obey a pattern. We
shall also see that for the purposes of control, these deviations fall
under two fundamental classes, which we call *motion uncertainty* and
*state uncertainty*.

*Disturbances* are a form of motion uncertainty that cause the state to
be moved in unexpected ways at future points in time. For example, wind
gusts are very hard to predict in advance, and can move a drone from a
desired path.

*Actuation error* occurs when a desired control is not executed
faithfully. An example would be a controller that outputs desired
torques for a robot, but where these are not followed exactly by the
low-level motor controller. These errors can be treated as motion
uncertainty.

*Measurement error* is a type of state uncertainty where due to sensor
noise the state is observed incorrectly. Understanding measurement error
is critical for closed-loop controllers which base their behavior on the
measured state.

*Partial observability* means that only certain aspects of the state
*can possibly be measured* by the available sensors. For example, a
mobile robot with a GPS sensor can only measure position, whereas it may
need to model velocity as part of its state. State estimation techniques,
such as Kalman filtering and particle filtering,
can be used to extrapolate the unobserved components of state to provide
reasonable state estimates. With those estimates, there will be some
remaining *localization error* that the controller will still need to
handle.

*Modeling error*, or *parameter uncertainty* means that the true
dynamics function differs from what is *known* to the robot. This is
sometimes considered a third class of uncertainty, but could also be
treated as state uncertainty as we shall see below.

Motion uncertainty can be modeled as a disturbance to the dynamics
$$\dot{x} = f(x,u) + \epsilon_d$$ where $\epsilon_d(t) \in E_d$ is some
error. Here $E_d$ is a set of possible disturbances, or a probability
distribution over disturbances. Motion uncertainty will cause an
open-loop system to "drift" from its intended trajectory over time. A
properly designed closed-loop controller can regulate the disturbances
by choosing controls that drive the system back to intended trajectory.

State uncertainty can be modeled as a discrepancy between the estimated
state $\hat{x}$ and the "true" state of the system $x$, such that
$\hat{x} = x + \epsilon_x$. This means that in open-loop trajectory
planning, we will start a plan from the estimated state $\hat{x}$. Then,
even if there was no motion uncertainty and we planned the best control
sequence possible $u(t)$ starting from $\hat{x}$, bad things could still
happen when it is executed. For closed-loop control, the control policy
$u(\hat{x})$ is *always chosen based on an incorrect estimate*. This
makes it much more difficult to ensure that it is correcting for true
deviations from the intended trajectory, rather than phantom errors
caused by uncertainty.

To design a robust controller, we might try to characterize $E_d$ and
$E_x$ by observing likely disturbance values. If we observe systematic
errors like a constant *bias*, then perhaps we can improve our models to
be more accurate and cancel out the systematic error (called
*calibration*). On the other hand, noisy errors are much harder to
cancel out. To make any theoretical guarantees about a system's behavior
in the case of motion uncertainty, it is usually necessary to ensure
that noise in $E_x$ and $E_d$ are relatively small.

Finally, let us note that modeling error can often be treated as state
uncertainty on a different dynamical system on an *augmented state*
vector. Suppose that we are controlling a 1D point mass, but we do not
observe the true mass $m$. Instead, we observe $\hat{m}$ which is
disturbed from the true value by $\epsilon_m$ such that
$\hat{m} = m + \epsilon_m$. If we construct the augmented state vector
$(p,v,m)\in \mathbb{R}^3$, then the state follows dynamics
$$\dot{x} \equiv \begin{bmatrix}\dot{p} \\ \dot{v} \\ \dot{m} \end{bmatrix} = f(x,u) = \begin{bmatrix} v \\ f/m \\ 0 \end{bmatrix}.$$
Hence, the modeling error is equivalent to the state uncertainty vector
$$\epsilon_x = \begin{bmatrix} 0 \\ 0 \\ \hat{m}-m \end{bmatrix}.$$

Trajectories with timing
-------------------------------------

It is important to discuss the difference between trajectories of a
dynamic system vs. the geometric paths that we worked with in kinematic
motion planning. In a dynamic system, the trajectory in state space
$x(t):[0,T]\rightarrow \mathbb{R}^n$ is parameterized by time. The state
space of a robotic system typically includes both configuration and
velocity components. By contrast, a geometric path moves in
configuration space and has no inherent notion of time.

Moreover, a geometric path can move in any direction as long as it does
not touch an obstacle, whereas a valid dynamic trajectory can only move
in directions that can be generated by feasible controls. Hence we must
consider both time and dynamic constraints when representing valid
trajectories.

### Trajectory representation

One basic representation is to store a trajectory as a sequence of
states sampled along the trajectory $(x_0,\ldots,x_n)$ along with the
initial time $t_0$ (often assumed to be 0) and the time step $\Delta t$
between each point. An approximate interpolation between each point can
be performed piecewise-linearly or with splines. For example, the
piecewise linear approximation has
$$x(t) = x_k + \frac{t-t_0-k\Delta t}{\Delta t}(x_{k+1} - x_k)$$ defined
over $t \in [t_0,t_0+n\Delta t]$, where
$k = \lfloor \frac{t-t_0}{\Delta t} \rfloor$ is the index of the
trajectory segment corresponding to the time $t$.

More generally, the
trajectory could store both states $(x_0,\ldots,x_n)$ and times
$(t_0,\ldots,t_n)$, with a slightly modified interpolation function
$$x(t) = x_k + \frac{t-t_k}{t_{k+1}-t_k}(x_{k+1} - x_k)$$ defined over
the range $[t_0,t_n]$ and $k$ determined to be the point in time so that
$t_k \leq t \leq t_{k+1}$.

If we are given an *integrator* (i.e., a *simulator*) for the dynamics
function, trajectories can be encoded in a control-space representation
$(x_0,u)$, which captures the initial state $x_0$ and an arbitrary control trajectory $u(t)$.
From these items, the integrator *generates* the state trajectory
$x(t)$. Specifically, we assume the existence of a function
$Sim(f,x_0,u,t)$ that integrates the dynamics $f$ forward over time $t$,
starting from $x_0$ and using the control trajectory $u$. The control
$u$ can be stored using arbitrary path representations, like
piecewise-constant functions, piecewise-linear functions, polynomials,
and splines. Then, we can regenerate the state-space trajectory
$x(t) \equiv Sim(f,x_0,u,t)$ as needed.

### Path to trajectory conversion

It is almost trivial to convert trajectories to paths: simply
construct a state space path and dropping the time component.
The converse --- creating a timed, dynamically-feasible trajectory from
a path --- can in some cases be quite challenging or even impossible. The reason is that the speed at which a robot should execute a path requires foresight into future twists and turns, like a race car driver slowing down ahead of a hairpin turn.

If a piecewise linear path were to be executed at a constant rate, then the timed trajectory would instantaneously change velocity at each milestone.  But infinite forces are needed to execute instantaneous changes of velocity, so sending such trajectories to motors would lead to overshooting corners.  We will examine
better methods for industrial robots to start and stop smoothly at milestones
when we discuss [motion generation](RobotControl.ipynb#Motion-queues-(motion-generation)).  The basic idea is to speed up and slow down gradually, while choosing the point in time when the robot slows so that the robot ends exactly at the next milestone.

The more general case is known as a *time-scaling* problem. Mathematically, we describe such a problem as being given a geometric path $p(s)$ as input, and we wish to find a timed path $x(t)$ such that:

* The trajectory follows the path: for all $t$, there exists an $s$ such that $x(t) = p(s)$
* First-order dynamic constraints satisfied: $g(t,\dot{x}(t)) \leq 0$ for all $t$
* Second-order dynamic constraints satisfied: $h(t,\dot{x}(t),\ddot{x}(t)) \leq 0$ for all $t$
* Possibly higher-order constraints as well...

This is formulated as finding a smooth, monotonically increasing 1D function $t(s)$ that defines the timing along the path.  At one end of the domain, there is a boundary constraint $t(0)=0$.  Since $t(s)$ is monotonically increasing, it has a well-defined inverse $s(t)$, so that the trajectory is defined via $x(t) = p(s(t))$.  and we can define the trajectory velocity, acceleration, and higher order derivatives using the chain rule:

* $\dot{x}(t) = p^\prime(s(t)) s^\prime(t)$
* $\ddot{x}(t) = p^{\prime\prime}(s(t)) s^\prime(t)^2 + p^\prime(s(t)) s^{\prime\prime}(t)$
* ...

Then, a dynamic constraint of order $k$ can then be rewritten in terms of $p$ (which is known), $s$, and their derivatives up to order $k$. Choosing $s(t)$ then becomes a constrained trajectory optimization problem, which we will discuss when we visit the topic of [optimal control](OptimalControl.ipynb).

Summary
-------

* Continuous-time dynamic systems are represented by a dynamics equation in the canonical form $\dot{x}(t) = f(x(t),u(t))$, where $x$ is the state trajectory and $u$ is the control trajectory.  Discrete-time systems are represented by the form $x_{t+1} = f(x_t,u_t)$.
* Integration (or simulation) is needed to determine the trajectory that the state will follow under a given control.  Numerical instability can result with a time step that is too large.
* Dynamic systems can be convergent, stable, or divergent under a given controller.

Exercises
---------

```python

```