# Parameter Selection for the Functionally Assembled Terrestrial Ecosystem Simulator

__Summary__

Numerical, process-based models that simulate tropical forest ecosystem dynamics, such as the Functionally Assembled Terrestrial Ecosystem Simulator (FATES), have been proposed as a way to improve climate change projections. However, parameterizing these complex models is challenging due to their numerous and interconnected non-linear relationships. Here, we to identify three high-performing parameter sets for use in future FATES simulations by quantitatively evaluating the performance of nearly 300 simulations, each run with a unique parameter set, against observations at a tropical forest test site.

__Motivation__

Greenhouse gas emissions from human activities are warming the Earth with potentially catastrophic implications for human health. Projections of twenty-first century warming are critical for climate change mitigation and adaptation efforts. Unfortunately, the numerical models used to project future climate differ in their predictions of warming even for the same greenhouse gas emissions scenario. This variability in projections across models stems largely from uncertainty about how Earth's vegetation will respond to climate change. In particular, predictions of tropical forest responses to climate change must be improved, as these forests exert strong control over global climate.  Numerical, process-based models of vegetated ecosystem dynamics, such as the Functionally Assembled Terrestrial Ecosystem Simulator (FATES; Fisher et al., 2015, 2018), have been proposed as a way of improving projections of tropical forests and thus future climate. However, parameterizing these complex models remains challenging due to their numerous parameters and interconnected, non-linear relationships.

__Goal__

The goal of this analysis is to identify model parameter sets that allows the FATES model to best match observations of tropical forest structure and functioning at a test site.

__Methods__

This analysis identifies high-performing parameter sets for use in future experiments by quantitatively evaluating the performance of nearly 300 different FATES parameter sets against observations at a tropical forest test site, Barro Colorado Island, Panama.

_Parameter ensemble simulations_

Prior to this analysis, we ran an ensemble of FATES simulations at our test site, Barro Colorado Island, Panama. Each simulation in the ensemble was initialized with a unique parameter set but was otherwise set-up identically to all other simulations. The 287 parameter sets we tested differed in 12 key plant trait parameters, which were sampled from observed distributions when possible (following Koven et al., _in prep_). All simulations were forced with repeating meteorological data for Barro Colorado Island, Panama, from the years 2003 to 2016 (Faybishenko et al., 2018). See Kovenock (2019) for further details of the parameter ensemble simulations.

Plant structure and functioning are sensitive to atmospheric carbon dioxide concentration, which increased over the observational time period. We therefore repeat all parameter ensemble simulations and our analysis below for two carbon dioxide concentrations that approximately bookend the observational time period (367ppm and 400ppm carbon dioxide).

_Parameter set evaluation and selection_

This code uses the above ensemble of simulations to quantitatively evaluate the performance of each parameter set against observations of six variables at our tropical forest test site. These variables characterize ecosystem structure (leaf area index, above-ground biomass, basal area) and functioning (gross primary productivity, latent heat flux, sensible heat flux). Observations come from the following sources:  leaf area index from Detto et al. (2018); above-ground biomass from Meakem et al. (2018), Feeley et al. (2007), and Baraloto et al. (2013); basal area from Condit et al. (2017, 2012), Condit (1998), and Hubbell et al. (1999); and gross primary productivity, latent heat flux, and sensible heat flux from Koven et al. (in prep). As some of these data sets require permission to use or are not yet publicly available, observational data sets are not included for download here.

We use two performance metrics $-$ error rate and normalized root mean square error $-$ to quantify each parameter set's performance. The error rate measures how frequently the model output falls within the observed range for each variable. The normalized root mean square error measures the distance between the model output and the observed mean, relative to the observed range for each variable. The expectation is that high-performing parameter sets will result in model output that falls within the range of observations (low error rate) and near the observed mean (low normalized root mean square error). After evaluating a parameter set's performance for each individual variable, we calculate the weighted average performance across all variables for that parameter set. To ensure that our selection of a high-performing parameter set is robust to weighting method, we consider three different weighting approaches:  even weighting, weighting favoring structural variables, and weighting accounting for correlation between individual variable performance.

Lastly, we identify parameter sets for use in future experiments by assigning an overall rank to each parameter set based on its performance across both performance metrics, three weighted averaging approaches, and two background carbon dioxide concentrations.

__Results__

This analysis identifies three high-performing parameter sets for use in future FATES simulations. We recommend the highest-performing parameter set for use in primary experiments and the next two highest-performing parameter sets for use in parameter sensitivity tests. These high-performing parameter sets are publicly available through the University of Washington ResearchWorks digital repository at http://hdl.handle.net/1773/43779. The performance of these parameter sets is reported in further detail in the [Results](#Results) section below and in Kovenock (2019).

## Analysis
## Step 1: Load libraries

```python
import netCDF4 as nc4
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import scipy
from scipy import stats
```

## Step 2: Load and preprocess data

Here we load the data and calculate time series of annual mean values for six ecosystem characteristics for all the simulations and observations. This code section returns two multidimensional arrays, one for model output and one for observations, that contain annual mean values for each variable organized by parameter set and background carbon dioxide concentration.

The six ecosystem characteristics included in these arrays are:

- Leaf area index,
- Above-ground biomass,
- Basal area,
- Gross primary productivity,
- Latent heat flux, and
- Sensible heat flux.

### 2.1  Model output

This part of the code loads the model output for all FATES simulations in our parameter ensemble. Then it calculates time series of annual mean values for the six variables we use to evaluate the performance of each parameter set.

```python
def annmeants(filepath,var,varfiletype,nyrs,conv_factor):
    ''' Calculate time series of annual means for a model output variable.
    :param filepath (str): file path to data file
    :param var (str): name of variable to call from filename
    :param nyrs (int, float): number of years to analyze
    :param conv_factor (float): conversion factor specific to variable specified by var
    :return: 2-D array containing annual mean time series (ensemble member, nyrs)
    '''

    # If model output is stored as monthly average for all tree sizes,
    # need to calculate annual mean.
    if varfiletype == 0:

        # Load monthly time series
        # For all cases except latent heat flux (FLH):
        if var != 'FLH':
            mthts_temp = nc4.Dataset(filepath).variables[var][:,:,0]

        # For the special case of latent heat flux:
        elif var == 'FLH':
            # Sum of three terms:
            mthts_temp = (nc4.Dataset(filepath).variables['FCTR'][:,:,0]
                          + nc4.Dataset(filepath).variables['FGEV'][:,:,0]
                          + nc4.Dataset(filepath).variables['FCEV'][:,:,0])

        # Calculate annual mean time series for nyrs and convert units if necessary
        annmeants = np.nanmean(np.reshape((mthts_temp[:,int(-1*nyrs*12):] * conv_factor),
                                          (mthts_temp.shape[0],-1,12)),axis=2)

    # Else if model output is stored as annual mean but structured by tree size,
    # need to sum across tree sizes.
    elif varfiletype == 1:
        # Calculate annual mean time series for entire ecosystem by summing across tree sizes
        annmeants = np.squeeze(np.nansum((
                        nc4.Dataset(filepath).variables[var + '_SCLS'][:,int(-1*nyrs):,:]),
                        axis=2))

    mthts_temp = None

    return annmeants
```

First, we will specify the information required to load and calculate annual mean time series of model output for each simulation, including file paths and names, variables to analyze, and conversion factors.

```python
# Filepath
model_filepath = 'data/'

# Filenames
# {1} = carbon dioxide concentration specified by CO2level;
# {2} = variable file type specified by varfiletype.
model_filenames =[
    'fates_clm5_fullmodel_bci_parameter_ensemble_1pft_slaprofile_{}_v001.I2000Clm50FatesGs.Cdf9b02d-Fb178808.2018-07-27.h{}.ensemble.sofar.nc',
    'fates_clm5_fullmodel_bci_parameter_ensemble_1pft_slaprofile_{}_v001.I2000Clm50FatesGs.Cdf9b02d-Fb178808.2018-07-27.h{}.ensemble.sofar.nc']

# Background carbon dioxide (CO2) concentration
CO2levels = ['367ppm', '400ppm']

# Variable list for model output
varlist = ['TLAI','AGB','BA','GPP','FLH','FSH']

# Data structure for each variable in varlist:
# 0 = monthly data for entire ecosystem;
# 1 = annual data structured by tree size structure.
varfiletype = [0,1,1,0,0,0]

# Conversion factor for each variable in varlist:
varconv = [1, 1, 1, 86400*365, 1, 1]

# Variable units after applying conversion factor for each variable in varlist:
varunits = ['$m^2/m^2$','$kgC/m^2$','$m^2/ha$','$gC/m^2/yr$','$W/m^2$','$W/m^2$']

# Number of years of model output to analyze
nyrs = 50

# Number of parameter sets in ensemble
nens = nc4.Dataset(model_filepath + model_filenames[0].format(CO2levels[0],varfiletype[0])).variables[varlist[0]].shape[0]
```

Next, we create a multidimensional array that contains a time series of annual means for each variable, parameter set and background carbon dioxide concentration.

```python
# Return model_data (float): a 4-D array of annual mean values for
    # each variable with dimensions
    # (CO2levels, varlist, nens, nyrs)
    # with the following indexing:
    # CO2levels:
    #         0 = 367ppm CO2;
    #         1 = 400ppm CO2.
    # varlist:
    #         0 = Leaf area index;
    #         1 = Above ground biomass;
    #         2 = Basal area;
    #         3 = Gross primary productivity;
    #         4 = Latent heat flux;
    #         5 = Sensible heat flux.
    # nens:
    #         0:286 = parameter set index.

# Initialize array
model_data = np.zeros([len(CO2levels), len(varlist), nens, nyrs])

for c in range(len(CO2levels)):
    for v in range(len(varlist)):

        filepath = model_filepath + model_filenames[c].format(CO2levels[c],varfiletype[v])

        model_data[c, v, :, :] = annmeants(filepath, varlist[v], varfiletype[v], nyrs, varconv[v])

        filepath = None
```

### 2.2 Observations

This code section loads data for the observations we will use to evaluate the performance of each parameter set in our FATES parameter ensemble. It then calculates annual mean values when necessary.

#### Leaf area index

Leaf area index observations come from Detto et al. (2018) and were made using hemispherical photographs taken approximately monthly from January 2015 to August 2017 at 188 locations at our test site, Barro Colorado Island, Panama. We calculate annual mean values from the monthly means reported by Detto et al. (2018). (Note that monthly data consists of spatial means across photograph locations, rather than temporal means.) In order to use all the data available, we calculate two time series of annual means - one starting in from January and the second starting from September. Data was captured from Detto et al. (2018) Figure 7a using GraphClick software.

```python
# Return obs_data_lai (float): 2-D array of annual mean leaf area index
#    (sample number, years) using the following index coding for
#    sample number:
#        0 = sample months starting from January;
#        1 = sample months starting from September.

# File path
filepath = 'data/LAI_Detto2018Obs.csv'

# Monthly spatial means
lai_mthts = np.asarray([col[2] for col in (pd.read_csv(filepath)).values])

# Specify start months for observations
startmonth_list = np.array([1,9])

# Number of annual means per sample
nyears_lai = round(len(lai_mthts)/12-0.5)

# Initialize array
obs_data_lai = np.zeros([len(startmonth_list), nyears_lai])

# Calculate annual means and fill array
for x in range(len(startmonth_list)):
    obs_data_lai[x,:] = np.nanmean(np.reshape(lai_mthts[startmonth_list[x]-1:24+startmonth_list[x]-1],(nyears_lai,12)),1)
```

#### Above-ground carbon biomass

Above-ground carbon biomass estimates were calculated from a 1995 census survey at our test site, Barro Colorado Island, by Meakem et al. (2018). They estimate above-ground biomass using two different methods (the standard and Chave allometric formulations). We use values from these two methods to represent uncertainty in the observational estimate.

Alternatively, we can approximate above-ground carbon biomass from estimates of total biomass (rather than just carbon biomass) from census survey data reproted in Baraloto et al. (2013) and Feeley et al. (2007) for the following years:  1985, 1990,1995, 2000, and 2005. This alternative method yields similar results and can be implemented by setting use_alt_agb_obs to 1 in the code below.

```python
# Return obs_data_agb (float): vector of above-ground
#    carbon biomass (KgC/m2) indexed by allometric
#    formulation:
#        0 = standard;
#        1 = Chave.

use_alt_agb_obs = 0;

filepath = 'data/BCI_biomass.csv'

if use_alt_agb_obs == 0:
    # Above-ground carbon biomass from Meakem et al. 2018 (MgC/ha)
    cbiomass_obs_Mgha = np.asarray([col[2] for col in (pd.read_csv(filepath)).values])[-2:,]
    # Convert from MgC/ha to KgC/m2
    ha_to_m2 = 1/10000
    Mg_to_kg = 1000
    obs_data_agb = cbiomass_obs_Mgha * ha_to_m2 * Mg_to_kg

elif use_alt_agb_obs == 1:
    # Total aboveground biomass (Mg biomass/ha) from
    # Baraloto et al. (2013) and Feeley et al. (2007)
    agb_biomass_obs = np.asarray([col[1] for col in (pd.read_csv(filepath)).values])[:-2,]
    # Estimate of carbon biomass from total biomass using
    # following Meakem et al. 2018
    obs_data_agb_v2 = agb_biomass_obs*0.47
    obs_data_agb = obs_data_agb_v2
```

#### Basal area

We use estimates of the median basal area for our test site Barro Colorado Island, Panama, from census surveys conducted in 1999, 2001, 2006, and 2011 by Condit (1998), Condit et al. (2012, 2017), and Hubbell et al. (1999).

```python
# Return obs_data_ba (float): vector containing basal area (m^2/ha)
#    indexed by census year in chronological order

filepath = 'data/census_bmks_bci_171208.nc'

# Load basal area median values for the last 5 census dates
# Data structured as follows:
# [census number, tree diameter size class,...
# distribution percentiles (0.05,0.5,0.95)]
basalarea_bysize = nc4.Dataset(filepath).variables['basal_area_by_size_census'][-5:,:,1]

# Sum across tree size classes
obs_data_ba = np.nansum(basalarea_bysize,1)
```

#### Gross primary productivity, latent heat flux, and sensible heat flux

Estimates of gross primary productivity, latent heat fluxes, and sensible heat fluxes were calculated from fluxtower eddy covariance measurements made from July 2012 to August 2017 at Barro Colorado Islana by Koven et al. (in prep). To use all available data in our analysis, we calculate two versions of the annual means time series, one beginning in July and the second beginning in September.

```python
def annmeants_fluxobs(mthts,startmth):
    ''' Calculate time series of annual means from monthly fluxtower estimates.
    :param mthts (float): 2-D array containing fluxtower observations (years, months)
    :param startmth (int): number corresponding to start month for this annual mean time series
                            (e.g. 7 = start with July, 9 = start with Sept)
    :return: vector containing annual mean time series of size (nyrs)
    '''
    # Discard number of months specified by dif
    mthts_dif = np.reshape(mthts,(1,-1))[:,startmth-1:startmth-1-12]

    # Calculate annual mean time series
    annmeants = np.nanmean(np.reshape(mthts_dif,(5,12)),axis=1)

    return annmeants
```

```python
# Return obs_data_flux (float): 3-D array containing annual mean values
#    indexed as (sample number, variable, year).
#    sample number:
#        0 = sample months starting from July;
#        1 = sample months starting from September.
#    variable:
#        0 = gross primary productivity;
#        1 = latent heat flux;
#        2 = sensible heat flux.

# Load observations
GPP_data = np.load('data/fluxdata_GPP.npy')
LH_data  = np.load('data/fluxdata_LH.npy')
SH_data  = np.load('data/fluxdata_SH.npy')
fluxdata_mask= np.load('data/fluxdata_mask.npy')

# Apply mask to arrays
GPP_monthyear = np.ma.masked_array(GPP_data, mask=fluxdata_mask)
LH_monthyear = np.ma.masked_array(LH_data, mask=fluxdata_mask)
SH_monthyear = np.ma.masked_array(SH_data, mask=fluxdata_mask)

# Specify start months for observations
startmonth_list = np.array([7,9])

# Number of years
nyrs_obsflux = len(annmeants_fluxobs(GPP_monthyear,startmonth_list[0]))

# Initialize array
obs_data_flux = np.zeros([len(startmonth_list), 3, nyrs_obsflux])

# Fill array
for x in range(len(startmonth_list)):
    obs_data_flux[x,0,:] = annmeants_fluxobs(GPP_monthyear,startmonth_list[x])
    obs_data_flux[x,1,:] = annmeants_fluxobs(LH_monthyear,startmonth_list[x])
    obs_data_flux[x,2,:] = annmeants_fluxobs(SH_monthyear,startmonth_list[x])
```

## Step 3: Quanitfy performance of each parameter set

In this section we evaluate the model performance against observations for each parameter set and background carbon dioxide level. As we would like to identify parameter sets that robsutly perform well regardless of performance metric we use two metrics to evaluate performance:  error rate and normalized root mean square error (NRMSE). We calculate both metrics for each variable. Then, we take a weighted average across all variables for each metric and parameter set.

### Performance Metric #1: Error Rate

The error rate measures the percent of model annual means that fall within the observed range for each variable and ensemble member. The observed range is defined as the difference between the maximum and minimum observed values. To account for relatively small sample sizes and potential measurment error within the observations we extend the observational range by 10% in both directions.

```python
def error_rate(model_ts,obs_ts,dg):
    '''Function calculates the error rate for each simulation
    as the percentage of model output annual means that fall
    within the observed range for the variable.
    param model_ts (float): a 2-D array containing the time series of annual means
       for a given variable indexed by (parameter set, years)
    param obs_ts (float): a vecotr or 2-D array containing the observed time series for
        the given variable indexed as (years) or (sample number, years)
    param dg (float): a scalar specifying the degradation level for observed range
        as a fraction
    return error_rate (float): a vector containing the error rates indexed by parameter set
        (nens)'''

    # Number of ensemble members
    nens = model_ts.shape[0]

    # Empty array to fill
    error_rate = np.zeros([nens])

    # Observed minimum and maximum
    obs_min = np.nanmin(obs_ts)
    obs_max = np.nanmax(obs_ts)

    error_rate = 100*np.nansum(np.where((model_ts <= obs_min*(1-dg)) | (model_ts >= obs_max*(1+dg)),1,0),1)/model_ts.shape[1]
    return error_rate
```

```python
# Return error_rate_array (float): a 3-D array containing error rates
#    indexed by (CO2 level, varlist, parameter set)

# Specify observed data arrays in order corresponding to varlist
obs_data_list = [obs_data_lai,obs_data_agb,obs_data_ba,
                 obs_data_flux[:,0,:],obs_data_flux[:,1,:],
                 obs_data_flux[:,2,:]]

# Degradation level for the observational range
# as fraction, not percent
dg = np.array([0.10])

# Calculate error rate
error_rate_array = np.zeros([len(CO2levels),len(varlist), nens])
for i in range(len(CO2levels)):
        for j in range(len(varlist)):
            error_rate_array[i,j,:] = error_rate(model_data[i,j,:,:], obs_data_list[j], dg)
```

### Performance Metric #2:  Normalized root mean square error (NRMSE)

The normalized root mean square error (NRMSE) measures the distance between the model output for each parameter set and the observed mean value. We normalize the root mean square error by the observed range for each variable so that we can compare NRMSE values across variables. In other words, normalizing the error tells us whether the distance from the observation is large compared to the spread in observations for each varialbe. This becomes especially useful when we take a weighted average NRMSE across all variables in the next code section. We caluclate this metric for each variable, parameter set, and background carbon dioxide level. The NRMSE is calculated as follows:

\begin{equation}\Large
NRMSE = \frac{ \sqrt{ \sum_{k=1}^n \frac{(x_{model,k} - \bar{X}_{obs})^2}{n}}} {x_{obs,max} - x_{obs,min}}
\end{equation}

where $NRMSE$ is the normalized root mean square error for a single variable (e.g. leaf area index) and parameter set, $n$ is the number of years of model output, $x_{model,k}$ is the model annual mean for year $k$, $\bar{X}_{obs}$ is the overall annual mean of the observed values, and $x_{obs,max}$ and $x_{obs,min}$ are the maximum and minimum observed values, respectively. When mulitple annual mean time series were sampled for an observed variable (e.g. observations for leaf area index spanned a partial year), we calculate the difference between the observed mean and model output using the time series that minimizes this difference.

```python
def nrmse(model_ts,obs_ts):
    '''Function calculates the normalized root mean square error(NRMSE)
    for each model ensemble member. When multiple observation time series
    are available, this function calculates the NRMSE for each time series
    and then selects the lowest of those NRMSE values.
    param model_ts (float): a 2-D array containing the time series of annual means
       for a given variable for all model ensemble members
       (ensemble member, years)
    param obs_ts (float): a vecotr or 2-D array containing the observed time series for
        the given variable indexed as (years) or (sample number, years)
    return nrmse (float): a vector containing the normalized root mean square error
        for each ensemble member indexed by (parameter set)'''

    # Number of ensemble members
    nens = model_ts.shape[0]

    # If multiple observation time series,
    # take the lowest NRMSE for each ensemble member
    try:
        if obs_ts.shape[1]>0:
            # Number of observation time series
            nobs = obs_ts.shape[0]
            obs_min = np.nanmin(obs_ts,axis=1)
            obs_max = np.nanmax(obs_ts, axis=1)
            obs_mean = np.nanmean(obs_ts,axis=1)

            temp_nrmse = np.zeros([nobs,nens])

            for obsnum in range(nobs):
                temp_nrmse[obsnum,:] = np.sqrt(np.nansum((model_ts[:,:] - obs_mean[obsnum])**2,axis=1) / model_ts.shape[1]) / (obs_max[obsnum]-obs_min[obsnum])

            nrmse = np.nanmin(temp_nrmse,axis=0)

            temp_nrmse = None

    # Otherwise, simply calculate NRMSE
    except IndexError:
        obs_min = np.nanmin(obs_ts,axis=0)
        obs_max = np.nanmax(obs_ts,axis=0)
        obs_mean = np.nanmean(obs_ts,axis=0)

        nrmse = np.sqrt(np.nansum((model_ts[:,:] - obs_mean)**2,axis=1) / model_ts.shape[1]) / (obs_max-obs_min)

    return nrmse
```

```python
# Return nrsmse_array (float): a 3-D array containing the NRMSE
#    indexed by [CO2level, varlist, parameer set]

nrmse_array = np.zeros([len(CO2levels),len(varlist), nens])

for i in range(len(CO2levels)):
        for j in range(len(varlist)):
            nrmse_array[i,j,:] = nrmse(model_data[i,j,:,:], obs_data_list[j])
```

### Weighted average permformance metrics across variables

We calculate weighted average performance metrics across variables for both the normalized root mean square error and the error rate. We calculate and consider three different weighting approaches to ensure that our selection of high-performing parameter sets is robust to weighting method. The weighting approaches we use are:

1. Even:  All variables are evenly weighted.

2. Structure:  This weighting favors structural ecosystem properties (leaf area index, above-ground biomass, and basal area). This weighting scheme reflects the likelihood that structural variables at our test site include less measurment uncertainty than flux variables.

3. Correlation: This weighting scheme is informed by correlations between individual variable performance metrics. The ability of a parameter set to mach observations of flux variables (gross primary productivity, sensible heat, and latent heat) was correlated with ability to match observations of leaf area index,  as well as other flux variables. As leaf area index observations likely include smaller measurement uncertainty, we choose to give a greater weighting to leaf area index at the expense of flux variables. We also reduced the weightings of basal area and above-ground biomass performance to account for their correlation with one another.

#### Weighted average error rate

```python
# Even weighting across all variables
er_wavg_even = np.nansum(error_rate_array,1) / error_rate_array.shape[1]

# Weighted average favoring structural properties
w = 0.3
er_wavg_strct = (w*(error_rate_array[:,0,:])
            + w*(error_rate_array[:,1,:])
            + w*(error_rate_array[:,2,:])
            + (1-3*w)*((error_rate_array[:,3,:])
                +(error_rate_array[:,4,:])
                +(error_rate_array[:,5,:]))/3)

# Weighting based on correlations between performance metric for each variable
w1 = 0.4
w2 = 0.25
w3 = 0.1
er_wavg_corr = ( w1*(error_rate_array[:,0,:])
            + w2*(error_rate_array[:,1,:])
            + w2*(error_rate_array[:,2,:])
            + w3*((error_rate_array[:,3,:])
                +(error_rate_array[:,4,:])
                +(error_rate_array[:,5,:]))/3)
```

#### Weighted average NRMSE

We quantify the distance of model output from the mean observations in multivariate space by calculating the weighted Euclidean distance as follows:

\begin{equation}
NRMSE_{avg} = \sqrt{ \sum_{i=1}^m (\omega_{i} \cdot NRMSE_{i})^2}
\end{equation}

where $NRMSE_{avg}$ is the weighted average normalized root mean square error across variables, $m$ is the number of variables we consider, $NRMSE_{i}$ is the normalized root mean square error for each individual variable, and $\omega_{i}$ is the weighting for each variable.

```python
# Even weighting across all variables
w = 1/6
nrmse_wavg_even = np.sqrt(w*(nrmse_array[:,0,:])**2
                                  + w*(nrmse_array[:,1,:])**2
                                  + w*(nrmse_array[:,2,:])**2
                                  + w*(nrmse_array[:,3,:])**2
                                  + w*(nrmse_array[:,4,:])**2
                                  + w*(nrmse_array[:,5,:])**2)

# Weighted average favoring structural properties
w = 0.3
nrmse_wavg_strct = np.sqrt(w*(nrmse_array[:,0,:])**2
                        + w*(nrmse_array[:,1,:])**2
                        + w*(nrmse_array[:,2,:])**2
                        + (1-3*w)*((nrmse_array[:,3,:])**2
                                    +(nrmse_array[:,4,:])**2
                                    +(nrmse_array[:,5,:])**2)/3)

# Weighting based on correlations between performance metric for each variable
w1 = 0.4
w2 = 0.25
w3 = 0.1
nrmse_wavg_corr = np.sqrt(w1*(nrmse_array[:,0,:])**2
                          + w2*(nrmse_array[:,1,:])**2
                          + w2*(nrmse_array[:,2,:])**2
                          + w3*((nrmse_array[:,3,:])**2
                                  +(nrmse_array[:,4,:])**2
                                  +(nrmse_array[:,5,:])**2)/3)
```

## Step 4: Rank parameter sets by performance
Here we assign an overall rank to each parameter set based on its performance across both performance metrics (error rate and NRMSE), three weighting schemes (even, structure, and correlated), and two cases (low and high atmospheric carbon dioxide concentration). The goal of this analysis is to identify parameter sets that robsutly perform well at our test site.

```python
all_avg_array = np.stack([er_wavg_even,nrmse_wavg_even,
                             er_wavg_strct,nrmse_wavg_strct,
                             er_wavg_corr,nrmse_wavg_corr])

rank_array = scipy.stats.mstats.rankdata(all_avg_array,axis=2)

# Sum ranks across cases and ranking methods
sum_rank_array = np.nansum(np.nansum(rank_array,axis=0),axis=0)

# Sort the index number for each ensemble member by their summed rank (best to worst performance)
sum_rank_index = np.argsort(sum_rank_array)

#Print Index # for highest-performing parameter sets
highperform_num = np.transpose(sum_rank_index)[:10,]+1
highperform_indx = np.transpose(sum_rank_index)[:10,]
print("Indices for the highest-performing parameter sets: ", highperform_num[0:3])
```

    Indices for the highest-performing parameter sets:  [ 86 260 151]

__Result:__ Parameter sets 86, 260, and 151 resulted in the highest model performance (in descending order) at our test site.

## Step 5: Plot performance metrics for each parameter set

In this section we visualize the model performance for each parameter set to gain insight into how the highest-performing parameter sets performed in individual variables and in comparison to all other parameter sets. More specifically, we plot heat maps of each parameter set's performance by performance metric and background carbon dioxide concentration.

```python
def heatsubplotfxn(heatdata, CO2indx, minval, maxval, plotnum, metriclabel,
                   highperform_indx, heat_var_labels, ens10label, enslabel):

    '''Function creates a heatmap for each performance metric
    for high-performing ensemble members and then all ensemble members.

    param heatdata (float): 3-D array containing a performance metric (CO2levels, variable, nens)
    param CO2indx(int, float): index for background CO2 level (0 = 367 ppm; 1 = 400ppm)
    param minval (int): minimum value for heatmap colorbar
    param maxval (int): maximum value for heatmap colorbar
    param plotnum (int): subplot number
    param metriclabel (str): label for performance metric
    param highperform_indx (int, float): vector of index numbers for
        high-performing parameter sets
    param heat_var_labels (str): list of variable labels indexed by (variable)
    param ens10label (str): list of high-performing parameter set numbers
        to be used as plot labels
    param enslabel (str): list parameter set numbers to label in plot of
        all parameter sets' performance metrics

    return heatmap subplot for one performance metric
    '''

    #Subplot indexing paramter
    i = 2

    # Highest Performing Ensemble Members
    ax1 = plt.subplot(3,i,plotnum)
    im1 = ax1.imshow(
        heatdata[CO2indx,:,highperform_indx],
        vmin = minval, vmax = maxval,
        cmap="viridis_r",aspect='auto')

    ax1.set_xticks(np.arange(len(heat_var_labels)))
    ax1.xaxis.tick_top()
    ax1.set_xticklabels(heat_var_labels)
    ax1.xaxis.set_label_position('top')

    ax1.set_ylabel('High Performing Parameter Sets (#)')
    ax1.set_yticks(np.arange(len(ens10)))
    ax1.set_yticklabels(ens10)

    # All Ensemble Members
    ax2 = plt.subplot(3,i,(i+plotnum,i*2+plotnum))
    im2 = ax2.imshow(
        np.transpose(heatdata[CO2indx,:,:]),
        vmin = minval, vmax = maxval,
        cmap="viridis_r",aspect='auto')

    # Colorbar
    cbar = ax1.figure.colorbar(
        im2, ax=ax2, orientation="horizontal",
        pad=0.025)
    # Labels
    cbar.ax.set_xlabel(metriclabel, fontsize = 16,
                       fontweight ='bold')
    ax2.set_xticks([]) # hide xticks/labels
    ax2.set_ylabel('All Parameter Sets (#)')
    ax2.set_yticks(ens)
    ax2.set_yticklabels(enslist)
```

```python
# Return error_heatdata: a 3-D array containing error rates
#    indexed by (CO2level, variable, nens)
# Return nrmse_heatdata: a 3-D array containing NRMSE
#    indexed by (CO2level, variable, parameter set)
#        Variable indexing as follows:
#            0-5 = variables in order of varlist,
#            6-8 = weighted averages across variables
#                    using even,
#                    structure, and correlation weights,
#                    respectively

# Concatenate data for error rate heatmap
error_rate_wavg_array = np.stack([er_wavg_even,er_wavg_strct,er_wavg_corr],axis=1)
error_heatdata = np.concatenate([error_rate_array,error_rate_wavg_array],axis=1)

# Concatenate data for NRMSE heatmap
nrmse_wavg_array = np.stack([nrmse_wavg_even,nrmse_wavg_strct,nrmse_wavg_corr],axis=1)
nrmse_heatdata = np.concatenate([nrmse_array,nrmse_wavg_array],axis=1)
```

```python
# Additional information for figures

# Metric/Variable labels
heat_var_labels = ["LAI","AGB","BA",
                "GPP","LH","SH","Av$_{E}$","Av$_{S}$","Av$_{C}$"]

# Ensemble member labels
# 10 highest performing
ens10 = [str(int(x)) for x in highperform_num]
# All (label every 25th ensemble member)
ens = np.array(range(25,300,25))
enslist = [str(x) for x in ens]
```

### Plot heat maps of performance by parameter set, performance metric, and background carbon dioxide concentration

```python
fig1 = plt.figure(figsize=(12,12))

# Set CO2levels index number
casenum = 0

# Plot Error Rate
plotnum = 1
heatsubplotfxn(error_heatdata, casenum, 0, 100, plotnum, 'A.  Error Rate (%)',
               highperform_indx, heat_var_labels, ens10, enslist)

# Plot NRMSE
plotnum = plotnum+1
heatsubplotfxn(nrmse_heatdata, casenum, 0, 10, plotnum, 'B.  NRMSE',
               highperform_indx, heat_var_labels, ens10, enslist)

plt.tight_layout()
```

__Figure 1.__ FATES model performance as measured by (A) error rate and (B) normalized root mean square error (NRMSE) for simulations run with 367 ppm atmospheric carbon dioxide. Performance metrics are shown for leaf area index (LAI), above-ground biomass (AGB), basal area (BA), gross primary productivity (GPP), latent heat flux (LH), sensible heat flux (SH) and weighted averages across variables using three weighting approaches:  even (Av$_E$), favoring structural variables (Av$_S$), and considering correlations (Av$_C$). Top panels highlight the performance of the top 10 highest-perfoming parameter sets. Bottom panels show the performance of all 287 parameter sets we tested.

```python
fig2 = plt.figure(figsize=(12,12))

# Set CO2levels index number
casenum = 1

# Plot Error Rate
plotnum = 1
heatsubplotfxn(error_heatdata, casenum, 0, 100, plotnum, 'A.  Error Rate (%)',
              highperform_indx, heat_var_labels, ens10, enslist)

# Plot NRMSE
plotnum = plotnum+1
heatsubplotfxn(nrmse_heatdata, casenum, 0, 10, plotnum, 'B.  NRMSE',
              highperform_indx, heat_var_labels, ens10, enslist)

plt.tight_layout()
```

__Figure 2.__ FATES model performance as measured by (A) error rate and (B) normalized root mean square error (NRMSE) for simulations run with 400 ppm atmospheric carbon dioxide. Performance metrics are shown for leaf area index (LAI), above-ground biomass (AGB), basal area (BA), gross primary productivity (GPP), latent heat flux (LH), sensible heat flux (SH) and weighted averages across variables using three weighting approaches:  even (Av$_E$), favoring structural variables (Av$_S$), and considering correlations (Av$_C$). Top panels highlight the performance of the top 10 highest-perfoming parameter sets. Bottom panels show the performance of all 287 parameter sets we tested.

## Results
This analysis identifies three high-performing parameter sets for use in future FATES simulations. We recommend the highest-performing parameter set (parameter set 86; Figures 1 and 2) as the primary parameter set for future experiments. Two other high-performing parameter sets (151 and 260; Figures 1 and 2) may be of interest for testing the sensitivity of simulation results to parameterization. Parameter set 260 is similar in parameter values and performance to the highest-performing parameter set (number 86). Parameter set 151 has the third highest performance but, has greater differences in key parameter values and results in a different simulated ecosystem. Specifically, parameter set 151 has higher performance in the above-ground biomass and basal area structural properties but, lower performance in leaf area index and gross primary productivity (Figures 1 and 2). These three high-performing parameter sets are publicly available through the University of Washington ResearchWorks digital repository at http://hdl.handle.net/1773/43779.

## Further Information
__Details of the parameter ensemble and analysis herein:__

Kovenock, M. (2019). Ecosystem and large-scale climate impacts of plant leaf dynamics (Doctoral dissertation). Chapter 4: "Within-canopy gradient of specific leaf area improves simulation of tropical forest structure and functioning in a demographic vegetation model." http://hdl.handle.net/1773/44061

__Details of the Functionally Assembled Ecosystem Simulator (FATES):__

https://github.com/NGEET/fates

Fisher, R. A., Koven, C. D., Anderegg, W. R., Christoffersen, B. O., Dietze, M. C., Farrior, C. E., et al. (2018). Vegetation demographics in Earth System Models: A review of progress and priorities. Global Change Biology, 24(1), 35–54. https://doi.org/10.1111/gcb.13910

Fisher, R. A., Muszala, S., Verteinstein, M., Lawrence, P., Xu, C., McDowell, N. G., et al. (2015). Taking off the training wheels: the properties of a dynamic vegetation model without climate envelopes. _Geoscientific Model Development, 8_(4), 3293–3357. https://doi.org/10.5194/gmdd-8-3293-2015

## References

Baraloto, C., Molto, Q., Rabaud, S., Hérault, B., Valencia, R., Blanc, L., et al. (2013). Rapid simultaneous estimation of aboveground biomass and tree diversity across Neotropical forests: a comparison of field inventory methods. _Biotropica, 45_(3), 288–298. https://doi.org/10.1111/btp.12006

Condit, R. (1998). Tropical forest census plots. Berlin, Germany, and Georgetown, Texas: Springer-Verlag and R. G. Landes Company.

Condit, R. S., Aguilar, S., Perez, R., Lao, S., Hubbell, S. P., & Foster, R. B. (2017). Barro Colorado 50-ha Plot Taxonomy as of 2017. https://doi.org/10.25570/stri/10088/32990

Condit, R., Lao, S., Pérez, R., Dolins, S. B., Foster, R., & Hubbell, S. (2012). Barro Colorado forest census plot data (version 2012). Center for Tropical Forest Science Databases. https://doi.org/10.5479/data.bci.20130603

Detto, M., Wright, S. J., Calderón, O., & Muller-Landau, H. C. (2018). Resource acquisition and reproductive strategies of tropical forest in response to the El Niño$-$Southern Oscillation. _Nature Communications, 9_(1), 913. https://doi.org/10.1038/s41467-018-03306-9

Faybishenko, B., Paton, S., Powell, T., Knox, R., Pastorello, G., Varadharajan, C., et al. (2018). QA/QC-ed BCI meteorological drivers. United States: Next-Generation Ecosystem Experiments Tropics; STRI; LBNL. https://doi.org/doi:10.15486/ngt/1423307

Feeley, K. J., Davies, S. J., Ashton, P. S., Bunyavejchewin, S., Supardi, M. N., Kassim, A. R., et al. (2007). The role of gap phase processes in the biomass dynamics of tropical forests. _Proceedings of the Royal Society B: Biological Sciences, 274_(1627), 2857–2864. https://doi.org/10.1098/rspb.2007.0954

Fisher, R. A., Koven, C. D., Anderegg, W. R., Christoffersen, B. O., Dietze, M. C., Farrior, C. E., et al. (2018). Vegetation demographics in Earth System Models: A review of progress and priorities. _Global Change Biology, 24_(1), 35–54. https://doi.org/10.1111/gcb.13910

Fisher, R. A., Muszala, S., Verteinstein, M., Lawrence, P., Xu, C., McDowell, N. G., et al. (2015). Taking off the training wheels: the properties of a dynamic vegetation model without climate envelopes. _Geoscientific Model Development, 8_(4), 3293–3357. https://doi.org/10.5194/gmdd-8-3293-2015

Hubbell, S. P., Foster, R. B., O'Brien, S. T., Harms, K. E., Condit, R., Wechsler, B., et al. (1999). Light-gap disturbances, recruitment limitation, and tree diversity in a neotropical forest. _Science, 283_(5401), 554–557. https://doi.org/10.1126/science.283.5401.554

Koven, C. D., et al. (_in prep_). Benchmarking and parameter sensitivity of physiological and vegetation dynamics using the Functionally Assembled Terrestrial Ecosystem Simulator (FATES) at Barro Colorado Island, Panama.

Kovenock, M. (2019).  Ecosystem and large-scale climate impacts of plant leaf dynamics (Doctoral dissertation). http://hdl.handle.net/1773/44061

Meakem, V., Tepley, A. J., Gonzalez-Akre, E. B., Herrmann, V., Muller-Landau, H. C., Wright, S. J., et al. (2018). Role of tree size in moist tropical forest carbon cycling and water deficit responses. _New Phytologist, 219_, 947–958. https://doi.org/doi:10.1111/nph.14633

```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{dx}{dt}(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 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,t)$$

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

$$...$$

$$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
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 $f(x) = 0$, and points that satisfy this criteria
are known as *equilibrium points*. All stable points are equilibria, but
the converse is not true.

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 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)$
and $B=\frac{\partial g}{\partial x}(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

```