# Introduction to the Kalman filter

```python
# imports

import pandas as pd
from matplotlib import pyplot as plt
import scipy.linalg as la
import numpy as np
import scipy.interpolate as interp
import scipy.stats as stats
from abc import ABC, abstractmethod
from copy import deepcopy
%matplotlib inline
```

## Dynamic systems

A dynamic system is, roughly speaking, any system that evolves in time. This evolution in time is based on its initial state, the system input, and some underlying equations. The classic example of a dynamic system is a cart on rails, whose initial state is known to be an $x$ position and $x$ velocity of zero, and that moves based on an applied force, which is the system input. Once a force is applied, the cart starts to move according to Newton's laws:

$\begin{eqnarray*}
x = x_0 + v_0 t + \frac{f}{2m} t^2\\
v = v_0 + \frac{f}{m}t \end{eqnarray*}$

These are the state-transition rules of the cart.

We can make predictions about the state of a dynamic system based on its state-transition rules and knowing some initial state. However, when these dynamic systems are implemented in practice, the state transitions do not perfectly match the predictions. The predictions do not account for environmental disturbances, which cannot be accurately estimated upfront. In the case of the cart, there may be a friction force preventing the cart from moving perfectly according to its state-transition rules. Therefore there is some uncertainty surrounding the prediction as to how close it is to the actual state.

The purpose of a Kalman filter is to combine the predicted system state (including uncertainty) with measurements of the system that are noisy and therefore have their own uncertainty, in order to get a better estimate of the system state over time.

A succinct way to describe what a Kalman filter does is that it combines *physics* and *reality*: we build a physical model to obtain some expectation of what a system should do at any given time, and we combine that with what we know is really happening

## State-space representation of dynamic systems

Dynamic systems can be represented in *state space* by defining a *state vector* of the form $\vec{x} = [\begin{matrix} x_1 & x_2 & \dots & x_n \end{matrix}]^T$ that contains variables describing the system.

A dynamic system can be described based on the time-evolution of the system state as well as how the system output $\vec{y}$ relates to the control input $\vec{u}$ and state. This is done in continuous time by specifying a set of first-order differential equations and combining them into one equation using matrices,

$\begin{eqnarray*}
    \vec{\dot{x}}(t) = A'\vec{x}(t) + B'\vec{u}(t)\\
    \vec{y}(t) = C'\vec{x}(t) + D'\vec{u}(t)
\end{eqnarray*}$

Typically, the matrix $A'$ is referred to as the *state-transition matrix* and the first equation is the *state-transition equation*. To verbally describe the other matrices, you could call $B'$ the *state-transition input gain*, $C'$ the *state-to-output matrix* and $D'$ the *input-to-output gain*, but only $A$ has a generally accepted verbal description.

Dynamic systems can have many possible equivalent state-space representations depending on the choices of state variables, but for real-life systems that are easily described by a finite number of parameters, it is easiest to describe the system in that representation. For linear systems, it is possible to translate a state-space representation to a *transfer function* representation and from there transform it into an alternative state-space representation that is more useful for controller or observer design (described [here](https://www.engr.mun.ca/~millan/Eng6825/canonicals.pdf)). For Kalman filtering it isn't necessary to do this, but it may make it easier to reason about systems by changing the representation.

In discrete time, the setup is similar to continuous time, but with a *difference equation*,

$\begin{align}
    \vec{x}((k+1) \Delta t) & =  A\vec{x}(k \Delta t) + B\vec{u}(k \Delta t)\\
    \vec{y}(k \Delta t) & =  C\vec{x}(k \Delta t) + D\vec{u}(k \Delta t)
\end{align}$

The discrete-time matrices A and B relate to the continuous-time matrices A' and B' [^1] according to

$\begin{align*}
    & A(\Delta t) =  e^{A'\Delta t}\\
    & B(\Delta t) = \int_0^{\Delta t} e^{A'\lambda} d\lambda B'
\end{align*}$

For a derivation of this, see http://www.engr.iupui.edu/~skoskie/ECE595_f05/handouts/discretization.pdf.

The state-space representation allows us to model dynamic systems as black boxes: we provide them an input $u$ and get out an output $y$. Based on this we can design feedback systems to observe or control the system.

[^1] (other conventions like (A,B) for continuous time and (G,H) for discrete time interfere with later Kalman notation, so this is the convention we'll use)

For example, with a state $[\begin{matrix} x & \dot{x} \end{matrix}]^T$, the continuous-time state-space representation would have the following time-evolution equation:

$\left[\begin{matrix} \dot{x} \\ \ddot{x} \end{matrix}\right] = \left[\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}\right] \left[\begin{matrix} x \\ \dot{x} \end{matrix}\right] + \left[\begin{matrix} 0 \\ \frac{1}{m} \\  \end{matrix}\right] f(t)$

This is just a matrix representation of $F = ma$ coupled with the definitions of derivatives (note that the first row is just $\dot x = \dot x$; this is quite common in state-space systems, where the state variables are derivatives of one another.) If we wanted to discretize this, we would follow the above equations:

```python
# simple demonstration of the CT-to-DT conversion

dt = 0.1
la.expm(dt*np.array([[0, 1], [0, 0]]))
```

    array([[1. , 0.1],
           [0. , 1. ]])

This intuitively makes sense as the discrete-time state-transition matrix; through a small time, the new position is the old position plus the product of velocity and the timestep, $x[k + 1] = x[k] + v[k] \Delta t$, and the new velocity is the old velocity. This is true as long as there is no force; to figure out the effect of the force, let's calculate the state-transition input gain.

$\begin{eqnarray*}
B(\Delta t) & = \int_0^{\Delta t} A(\lambda) B' d\lambda \\ & = \int_0^{\Delta t} \left[\begin{matrix} 1 & \lambda \\ 0 & 1\end{matrix}\right] \left[\begin{matrix} 0 \\ \frac{1}{m} \end{matrix}\right] d\lambda \\ & = \int_0^{\Delta t} \left[\begin{matrix} \frac{\lambda}{m} \\ \frac{1}{m}\end{matrix}\right] d\lambda \\ & = \left[\begin{matrix} \frac{\Delta t^2}{2m} \\ \frac{\Delta t}{m} \end{matrix}\right]
\end{eqnarray*}$

We can intuitively make sense of this too; the first element is the acceleration component in the usual kinematic equation $x = x_0 + v_0 t + \frac{1}{2} at^2$, and the second is the acceleration component in the other common equation $v = v_0 + at$.

With a sufficiently small timestep, we can accurately simulate continuous-time systems by discretizing them in this way and advancing them through state transitions over time.

## System observers and controllers

The general control problem is to design a control system to regulate the behaviour of any dynamic system, usually through a feedback loop. Control usually requires access to the full state, which is not always available; all that is guaranteed is access to the system output. Therefore, a system called an *observer* is used to obtain the state from the output, and from there a controller acts to provide the optimal system input such that the system operates as desired. This is usually quantified by certain performance metrics like overshoot from a desired state, error in the steady state (as time goes to infinity), the rise time (delay between a control input and the corresponding desired output), and so on.

An $n$-dimensional system is called *observable* if $n$ measurements of the output $y$ are sufficient to uniquely determine $x[0]$, the initial system state. A system is observable if the *observability matrix* $\left[\begin{matrix} C & CA & \dots & CA^{n-1} \end{matrix}\right]^T$ has full rank.

An $n$-dimensional system is called *controllable* if it is possible to find a sequence of control inputs $u[0], \dots, u[n-1]$ such that any initial state $x[0]$ can be taken to any desired $x_f = x[n]$. A system is controllable if the *controllability matrix* $\left[\begin{matrix} B & AB & \dots & A^{n-1}B \end{matrix}\right]$ has full rank.

A Kalman filter is an observer; given a set of system measurements, it returns the best estimate of the state that produced those measurements. It can be proven that the Kalman filter is the optimal linear observer, in the sense that its state estimates have the minimum mean-squared error (MMSE).

Let a system (called the *plant*, i.e. the system to be controlled) have dynamics $\hat{\dot{x}} = A'\hat{x} + B'u$ and $\hat{y} = C'\hat{x}$, and let there be a measurement $y$ of the system. The error between the estimated and actual measurements is then $y - \hat{y} = y - C'\hat{x}$. We can update the state equations of the plant to construct those of the observer, by applying a proportional gain to this error and adding this scaled error in to the state-transition equation:

$\begin{eqnarray*}
    \hat{\dot{x}} = A'\hat{x} + B'u + K(y - \hat{y})\\
    \hat{y} = C'\hat{x}
\end{eqnarray*}$

In discrete time, the error dynamics are more simply described by the equation

$x[k] = \hat{x}[k] + K(y[k] - C\hat{x}[k])$

The problem of observer design is to find the optimal $K$ to minimize the magnitude of the error $y - C\hat{x}$ over time. Simple linear systems can do this in the Laplace domain by designing $K$ such that the transfer representation has a certain desired transfer function, simply by matching terms and solving a linear system based on that. The Kalman filter approach works entirely in state-space; instead of a desired transfer function, it builds an error gain matrix (often called the *Kalman gain*) based on the physical model and the uncertainties in both the physics and measurements.

Building the Kalman gain requires an understanding of how we model process noise (the noise associated with the *physics* side of the Kalman filter) and measurement noise (the noise associated with the *reality* side).

## The normal distribution

In Kalman filter models, noise in the predicted or measured state is considered to be Gaussian, i.e. it follows the normal distribution:

$$p(x) = \frac{1}{\sigma \sqrt{2\pi}} exp\left({\frac{-(x - \mu)^2}{2\sigma^2}}\right)$$

where variance is defined as the expected value of a deviation from the mean squared, $\sigma^2 = E[(x - \mu)^2]$.

In NumPy, you can get normally distributed variables with a specific mean $\mu$ and standard deviation $\sigma$ using `np.random.normal`.

```python
np.random.normal(0, 2, 10) # expect most/all of the data to be between -2 and 2.
```

    array([-2.8826191 , -0.5713489 ,  3.6183021 ,  1.49279356,  2.13750928,
           -3.41582899,  3.1426489 , -2.1099841 , -0.58226979, -2.73678364])

We can verify that the mean and SD are about what we expect:

```python
np.mean(np.random.normal(0, 2, 10000))
```

    -0.013034891213234331

```python
np.std(np.random.normal(0, 2, 10000))
```

    2.000430570677955

We can visualize what this looks like:

```python
data = np.random.normal(0, 5, 1000)
data.sort()
pdf = stats.norm.pdf(data, np.mean(data), np.std(data))
plt.plot(data, pdf)
```

This kind of distribution makes physical sense; around an expected value, there is a high chance of a small deviation in the actual state, and a low chance of a larger deviation. If there are more complicated processes causing noise, they can be modelled as their own dynamic systems and incorporated into the state-transition and output equations. For example, if there is skewed noise, an expectation of that noise can augment the state, and any deviations around that can be considered Gaussian.

## Covariance

The multidimensional version of variance is called covariance. Covariance describes how much one variable changes with another. Explicitly, it is given by

$\text{cov}(x, y) = E\left[[x - E[x]] [y - E[y]]\right]$

where $E$ is an expected value operator. It can be verified that the covariance of a variable with itself is just its variance.

Covariance is useful for Kalman filtering because even if the state variable update rules are different, they are likely to be correlated, and this correlation can usually be understood physically. For example, a higher cart velocity would correlate to a higher predicted next position value, so there is likely a nonzero positive covariance between the cart's velocity and position.

The covariance of a predicted state can be represented in a square matrix $P$, where $P_{ij} = \text{cov}(x_i, x_j)$. The variances of each variable are down the diagonal. Additionally, because covariance is independent of the order of the variables, the matrix $P$ is symmetric.

Covariance changes under a matrix multiplication (like in a state-transition step) according to $x \to Ax, P \to APA^T$.

## Sensor measurements and noise

We can observe the state of a dynamic system based on sensor measurements that may be noisy. Let the matrix C translate between a state at some specific time and what its measurements should be at that time. These can then be compared to the actual measurements at that time, $\vec{y}$.

The measurements have their own covariance that is associated with the sensor being unreliable (take any sensor, such as an accelerometer or pressure sensor, hold it in one place, and watch it fluctuate) so measurements aren't a perfect representation of what a dynamic system is doing.

## Deriving the Kalman Filter governing equations

### Predict

The Kalman prediction step involves advancing the state through a single timestep based on the previous state and the control input at that time. With this, the covariance matrix also has to be updated and state noise associated with environmental disturbances added to it. Let the state noise matrix be $Q$; then the prediction equations are

$\begin{eqnarray*}
x^-(t + \Delta t) = Ax(t) + Bu(t)\\
P^-(t + \Delta t) = AP(t)A^T + Q
\end{eqnarray*}
$

Mathematically, it is fine for any of these to vary in time, but dynamic systems can usually be modelled fairly accurately with constant $A$ and $B$ matrices and a constant noise model. The state and covariance have a negative superscript because they have not yet been updated, as will be described below; however, for simulations of dynamic systems, it is sufficient to just predict as time advances.

```python
# example of Kalman prediction
```

### Update

The part of the Kalman filter that ends up reducing the system state error is the update step. Here, we want to take our estimate of the error (in some sources, this is optimistically called the innovation),

$e(t) = \vec{y}(t) - C\vec{x^-}(t)$

and multiply this element-wise by the Kalman gain $K$ to get an updated state that is hopefully closer to reality:

$\vec{x} = \vec{x^-} + Ke = \vec{x^-} + K(\vec{y} - C\vec{x^-}) = [I - KC]\vec{x^-} + K\vec{y}$

where $\vec{x^-}$ is the *a priori* state, i.e. the state before the Kalman update, and $\vec{x}$ is the *a posteriori* state, i.e. that after the update. The covariance also updates with this step. The new covariance has components from the noise associated with $\vec{x^-}$ being multiplied by $[I - KC]$ and $\vec{y}$ being multiplied by $K$; the covariance $P^-$ is updated by the first, and the measurement noise $R$ is incorporated into the new covariance by the second.

$P = [I - KC]P^- [I - KC]^T + KRK^T$

We can find the Kalman gain by attempting to minimize the covariance of the *a posteriori* state. We set the following:

$\frac{\partial \text{Tr} P}{\partial K} = 0 \implies \frac{\partial \text{Tr} P^-}{\partial K} - 2 \frac{\partial \text{Tr} KCP^-}{\partial K} + \frac{\partial \text{Tr} KCP^- C^T K^T}{\partial K} + \frac{\partial \text{Tr} KRK^T}{\partial K} = 0$

Applying trace differentiation properties (namely that $\frac{\partial \text{Tr} ABA^T}{\partial A} = 2AB$ for symmetric $B$, and $\frac{\partial \text{Tr} AC}{\partial A} = C^T$), we get

$-2P^-C^T + 2KCP^-C^T + 2KR = 0$

which allows us to find $K$,

$K = P^-C^T [CP^-C^T + R]^{-1}$

Therefore, the Kalman update step is fully defined by first finding $K$ according to this formula, then updating the state based on the above equation. This is the optimal linear state estimate, based on which a controller can operate

## Implementation

The general dynamic system simulator is in the next cell. It is implemented as a Python abstract class (a class that can't directly be instantiated) that other classes can inherit from. Derived classes have to:

- implement their own state-transition matrices and external input ($A$, $B$, $u$) in the methods `evolve` and `ext_input`.
- define their own state and simulation end condition (`self.state`, `self.simend`) and update `self.simend` in `evolve`.
- define a `reset` method that returns the object state to what it was before simulation. (Ideally you'd do this entirely in the state, but for the rocket example below I found it was easier to have some class attributes other than the state to keep algebra simpler.)
- define
 - the initial state covariance matrix P
 - the state-transition noise matrix Q
 - the state-to-measurement matrix H
 - the measurement covariance matrix R.

Assuming these are defined for each instance of a `DynamicSystem` child class, any `DynamicSystem` can run simulations (i.e. just transition states over time without the Kalman update step), or filter its own state based on noisy sensor data.

```python
class DynamicSystem(ABC):
    '''
    An abstract dynamic system with time-dependent state evolution, time-independent state to measurement mapping,
    and initial measurement covariances, constructed to facilitate Kalman filtering.

    Required parameters:
        simend (bool)
        state (ndarray)
        P (ndarray)
        Q (ndarray)
        H (ndarray)
        R (ndarray)
        STATE_SIZE (int)
        INPUT_SIZE (int)
    Required methods:
        evolve
        ext_input
        reset
    '''

    @abstractmethod
    def evolve(self, t, dt):
        # Given t and dt, returns the dynamic system's A and B matrices. Also sets simend based on state.
        pass

    @abstractmethod
    def ext_input(self, t):
        # Given t, returns u(t).
        pass

    @abstractmethod
    def reset(self):
        # restores all internal variables that may have changed during a simulation run.
        pass

    def predict(self, t, dt):
        # predicts system state at time t+dt based on system state at time t
        A, B = self.evolve(t, dt)
        u, u_status = self.ext_input(t)
        state_predicted = A.dot(self.state) + B.dot(u)
        P_predicted = A.dot(self.P.dot(A.T)) + self.Q
        return (u, u_status, state_predicted, P_predicted)

    def sim_results(self, t, k, states, inputs, terminate):
        self.reset()
        print("Simulation ended at t =", t, "s due to", terminate)
        processed_states = np.zeros([self.STATE_SIZE, k])
        processed_inputs = np.zeros([self.INPUT_SIZE, k])
        states = states.T
        inputs = inputs.T
        for i, state in enumerate(states):
            processed_states[i] = state[:k]
        for i, input in enumerate(inputs):
            processed_inputs[i] = input[:k]
        return (np.linspace(0,t,k+1)[:k], processed_states, processed_inputs)

    def simulate(self, dt=0.01, timeout=30, verbose=False, kalman=None):
        interrupt = False
        t, k = 0, 0
        if kalman is not None:
            m = 0
            measure_times = kalman[0]
        states = np.zeros([int(np.ceil(timeout/dt))+1, self.STATE_SIZE])
        inputs = np.zeros([int(np.ceil(timeout/dt))+1, self.INPUT_SIZE])
        terminate = "error."
        try:
            while t < timeout:
                if verbose and hasattr(self, "compact_status") and k % 100 == 0:
                    self.compact_status(t)
                states[k] = self.state
                inputs[k], input_status, state_predicted, P_predicted = self.predict(t, dt)
                if kalman is not None and m < measure_times.size and np.isclose(t, measure_times[m], atol=dt/2):
                    self.state, self.P = self.update(state_predicted, P_predicted, kalman[1][m])
                    m += 1
                else:
                    self.state = state_predicted
                    self.P = P_predicted
                if verbose and input_status is not None and k % 100 == 0:
                    print(input_status)
                if self.simend:
                    terminate = "end condition."
                    break
                t += dt
                t = np.round(t, -int(np.log10(dt)))
                k += 1
        except KeyboardInterrupt:
            print("\nSteps completed:", k)
            terminate = "interrupt."
        if t >= timeout:
            terminate = "timeout."
        return self.sim_results(t, k, states, inputs, terminate)

    def measure(self, state):
        return self.H.dot(state)

    def update(self, state_predicted, P_predicted, measurement):
        error = measurement - self.measure(state_predicted)
        K = P_predicted.dot(self.H.T.dot(np.linalg.inv(self.H.dot(P_predicted.dot(self.H.T)) + self.R)))
        state_updated = state_predicted + K.dot(error)
        P_updated = P_predicted - K.dot(self.H.dot(P_predicted))
        return (state_updated, P_updated)
```

## The rocket dynamic model

We'll demonstrate Kalman filtering on data collected from a rocket launch. The rocket is called Sub-Arktos. It was made by the UC Berkeley high-powered rocketry team, CalSTAR, as a subscale launch vehicle for the NASA Student Launch competition in 2017.

High-powered rockets usually fly using commercial off-the-shelf motors, which provide on the order of 10 Gs in thrust over a very short time. Once a rocket reaches apogee (the highest point of flight), it deploys parachutes so that it comes down gradually rather than at its terminal velocity, so that it can be recovered without damage. The condition for parachute deployment is usually reaching apogee, a specific time delay after apogee, or reaching a specific altitude during descent. Sub-Arktos flew with two parachutes, which is reasonably standard for an apogee on the order of a mile. The first was a small "drogue" chute deployed at apogee, so as to make the rocket fall gradually, but still fast enough that it does not drift too much during descent. The second was a larger "main" chute deployed at an altitude of 800 feet, to ensure the rocket is falling slowly enough that impact is soft and there is no damage to the airframe.

To deploy parachutes at the correct time, it is necessary to have a good idea of the rocket's state at any given time. However, knowing the rocket's state is dependent on altimeter and accelerometer data, which may be noisy. But since we know from simulation approximately what the rocket state at any given time should be, determining when the rocket is likely to be at apogee or at some specific altitude during descent is a perfect Kalman filtering problem.

If you're interested in how the rocket simulation was developed, I'd love to discuss it in detail in person or in a different notebook; for here I'll just use it to demonstrate filtering without explaining much of what's under the hood. Essentially, it's just solving for the rocket's position, velocity, and acceleration based on those values at the previous timestep and the force on the rocket over time. Accurately modelling the force was the difficult part; after that, it's essentially just like the example above.

The state-transition equations can be discretized according to the formulas given previously to get

$\begin{bmatrix} x(t + dt) \\ \dot{x}(t + dt) \\ \ddot{x}(t + dt) \end{bmatrix} = \begin{bmatrix} 1 & dt & \frac{dt^2}{2} \\ 0 & 1 & \frac{dt}{2} \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} x(t) \\ \dot{x}(t) \\ \ddot{x}(t) \end{bmatrix} + \begin{bmatrix}0 \\ \frac{dt}{2m(t)} \\ \frac{1}{m(t)} \end{bmatrix} f(t)$

Ordinarily, the 2D state $\begin{bmatrix} x(t) & \dot{x}(t) \end{bmatrix}^T$ would suffice, but I included acceleration in the state because one of the sensors is an accelerometer, and for Kalman filtering to work, the measurements have to depend only on the state.

It should also be noted that this is an approximation, using second-order (velocity verlet) integration instead of something like a fourth-order Runge-Kutta method. You could do that in the form $x(t + dt) = Ax(t) + Bu(t)$ as well, but $A$ and $B$ would vary with time (more accurately, they would vary with the state) which makes it computationally difficult to filter in real time.

Note that there's a slight oversimplification that both my simulation and the reference simulation make, to do with main parachute deployment. Both model it as an instantaneous process, with the chute providing zero drag until it's time for it to be deployed and its full drag thereafter. This essentially creates a Dirac delta-type spike in the acceleration that's slightly offset between the reference and reality, and likely isn't what you'd see on a real-life accelerometer. Therefore, for this example, we're going to assume that Sub-Arktos only has a drogue chute and no main  chute. This reflects what actually happened to the rocket at its last launch, so it seems reasonable to simulate. If you want to see a simulation with the main parachute included, take a look at 'subarktos/subarktos.py' under this repo.

```python
# Feel free to skip way past this, till I figure out how to hide a code cell in Jupyter.

class Parachute:

    CHUTE_OPEN_DELAY = 0.2
    '''
    diameter : scalar
        Diameter of the parachute.
    cd : scalar
        Drag coefficient of the parachute.
    alt : scalar
        Deployment altitude of the parachute in feet converted to meters.
        If None, considered to be at apogee.
    deployed : bool
        Indicates whether or not the chute has deployed.
    name : string
        Name of the parachute, for reading status.
    t_deployed : scalar
        Time when the parachute was deployed.
    '''
    def __init__(self, diameter, cd, name, alt=None):
        self.b_drag = 1.275 * np.pi * cd * diameter**2/8
        if alt is not None:
            self.alt = alt * 0.3048
        else:
            self.alt = float('inf')
        self.deployed = False
        self.name = name
        self.t_deployed = None

    def drag(self, velocity, t):
        # velocity in m/s
        if not self.deployed or t - self.t_deployed < self.CHUTE_OPEN_DELAY:
            return 0
        return self.b_drag * velocity**2

    def deploy(self, t):
        if not self.deployed:
            print("Deploying", self.name, "at t =", str(t).strip(),"s")
            self.deployed = True
            self.t_deployed = t

class Motor:
    # credit for thrust and mass functions: Aled Cuda
    '''
    mass_init : scalar
        Initial mass of the motor.
    mass_final : scalar
        Final mass of the motor.
    time_burnout : scalar
        Time at which motor burns out in seconds.
    thrust_curve : string
        Name of text file containing thrust curve data.
    time_delay : scalar
        Time between launch and ignition of this motor in seconds.
    '''
    def __init__(self, mass_init, mass_final, thrust_curve, time_delay=0):
        self.mass_init = mass_init
        self.mass_final = mass_final
        self.thrust_data = np.loadtxt(thrust_curve)
        # adding thrust(t = 0) = 0 to help interpolation
        self.thrust_data = np.concatenate((np.array([[0,self.thrust_data[::,1][1]/2]]), self.thrust_data), axis=0)
        self.max_thrust = np.amax(self.thrust_data,0)[1]
        assert time_delay >= 0, "Cannot have a negative delay."
        self.time_delay = time_delay
        # adjust for time delay in thrust data
        self.thrust_data = self.thrust_data + np.vstack([np.array([time_delay,0])]*self.thrust_data.shape[0])
        self.time_burnout = np.max(self.thrust_data[::,0]) + time_delay

    def thrust(self, t):
        # If we ask for a time before or after the range we assume thrust is zero
        if t > self.time_burnout or t < self.time_delay:
            return 0
        # Otherwise we use the interpolate function
        return interp.interp1d(self.thrust_data[::,0], self.thrust_data[::,1])(t)

    def mass(self, t):
        if t < self.time_delay:
            return self.mass_init
        elif t > self.time_burnout:
            return self.mass_final
        return ((self.mass_final - self.mass_init)/(self.time_burnout - self.time_delay))*t + self.mass_init

class Sensor:
    '''
    Object that reads in a rocket state variable.
    select : int
        Indicates which state variable is being read: 0 for altitude, 1 for velocity (not expected to be used),
        2 for acceleration.
    var : scalar
        Variance of the sensor.
    data : ndarray
        Collection of n row vectors with two elements: time and sensor reading.
    convert : scalar
        Multiplicative factor from the state variable to the sensor reading. Mostly m to ft conversion.
    '''

    def __init__(self, select, var, data, convert=3.28):
        self.select = select
        self.var = var
        self.data = data
        self.convert = convert

class Rocket(DynamicSystem):

    '''
    A rocket with an altimeter and accelerometer.

    dry_mass : scalar
        Takes in time, returns rocket mass.
    parachutes : list
        A list of Parachute objects.
    motors : list
        A list of Motor objects.
    sensors : list
        A list of Sensor objects. Setting this to the empty list will set Kalman filtering to simulation without updates.
    b_drag : scalar
        Coefficient on v^2 in drag equation. (Not the same as cd.)
        Currently being determined via curve fitting on OpenRocket sim data.
    state : ndarray
        1x3 row vector containing position, velocity, and acceleration in the best units.
        State represented as a row vector, to be transposed if it's important for matrix operations.
    apogee : bool
        Boolean to indicate if apogee has been reached yet, to update parachute states.
    '''
    STATE_SIZE = 3
    INPUT_SIZE = 1

    def __init__(self, dry_mass, parachutes, motors, sensors, b_drag):
        self.dry_mass = dry_mass
        self.parachutes = parachutes
        self.motors = motors
        self.sensors = sensors
        self.b_drag = b_drag
        self.H = np.zeros((len(sensors), 3))
        self.R = np.zeros((len(sensors), len(sensors)))
        for i,s in enumerate(sensors):
            self.H[i][s.select] = s.convert
            self.R[i][i] = s.var
        self.P = np.zeros([self.STATE_SIZE, self.STATE_SIZE])
        q = 2
        d = 0.001 # sampling time difference of one of the sensors. Just in here for the noise model, to be updated.
        # constant acceleration approximation: change this later
        self.Q = q * np.array([[d**4/4, d**3/3, d**2/2], [d**3/3, d**2/2, d], [d**2/2, d, 1]])
        # for now these are hardcoded in, but find a way to remove them and still detect apogee:
        self.stdev_alt = np.sqrt(5.54)
        self.stdev_acc = np.sqrt(62.953)
        self.apogee = False
        self.simend = False
        self.max_thrust = sum([m.max_thrust for m in self.motors])
        self.state = np.array([0, 0, 0])
        self.state[2] = self.get_thrust(0)/self.get_mass(0)
        print(self.state)

    def get_mass(self, t):
        return self.dry_mass + sum([m.mass(t) for m in self.motors])

    def get_thrust(self, t):
        # to do: burnout detection.
        return sum([m.thrust(t) for m in self.motors])

    def get_rocket_drag(self):
        magnitude = self.b_drag * self.state[1]**2
        if not self.apogee:
            return -magnitude
        else:
            return magnitude

    def get_chute_drag(self, t):
        return sum([p.drag(self.state[1], t) for p in self.parachutes])

    def ext_input(self, t, mass=None):
        if mass is None:
            mass = self.get_mass(t)
        thrust = self.get_thrust(t)
        if thrust == 0 and t < 0.2:
            # to do: better way of detecting 'still on the pad'
            gravity = 0
        else:
            gravity = -9.8 * mass
        rocket_drag = self.get_rocket_drag()
        if thrust > 0.05*self.max_thrust:
            assert thrust > np.abs(rocket_drag), "Excessive drag"
        chute_drag = self.get_chute_drag(t)
        return (thrust + gravity + rocket_drag + chute_drag, str([thrust, gravity, rocket_drag, chute_drag]) + '\n')

    def evolve(self, t, dt, order=2):
        # order: of integration.
        m = self.get_mass(t)
        if order == 1:
            A = np.array([[1, dt, dt**2/2],[0, 1, dt],[0, 0, 0]])
            B = np.array([0, 0, 1/m])
        elif order == 2:
            A = np.array([[1, dt, dt**2/2], [0, 1, dt/2], [0, 0, 0]])
            B = np.array([0, dt/(2*m), 1/m])
        # Orders > 2 to be implemented based on https://math.stackexchange.com/questions/2946737/solving-a-matrix-differential-equation-using-runge-kutta-methods, plus some slightly jank workarounds that allow the 'a' row to still be all zero.
        # Check for apogee
        if not self.apogee:
            try:
                assert self.state[0] > -self.stdev_alt, "you will not go to space today"
            except AssertionError as e:
                e.args += ("Time", t, "State", self.state)
                raise
            if self.state[1] < -self.stdev_acc and self.state[2] < -self.stdev_acc:
                # this is kind of a jank apogee check, especially because I'm comparing velocity to a deviation in acceleration
                try:
                    assert self.get_thrust(t) == 0, "If you're at apogee, the motor can't still be burning"
                except AssertionError as e:
                    e.args += ("Time", t, "thrust", self.get_thrust(t), "state:", self.state)
                    raise
                self.apogee = True
                print("Hit apogee at altitude",str(np.round(3.28*self.state[0], 2)),"ft")
                for p in self.parachutes:
                    if p.alt is None:
                        p.deploy(t)
        # Check for altitude-based chute deployment
        # To do: nicer way of doing this than checking at every timestep.
        if self.apogee and not all([p.deployed for p in self.parachutes]):
            for p in self.parachutes:
                if p.alt > self.state[0]:
                    p.deploy(t)

        # Check for ground hit
        if self.apogee and self.state[0] < self.stdev_alt and A.dot(self.state)[0] < -2*self.stdev_alt:
            self.simend = True
        return (A, B)

    def reset(self):
        # for some reason copy.deepcopy isn't working so here
        for p in self.parachutes:
            p.deployed = False
            p.t_deployed = None
        self.apogee = False
        self.state = np.array([0,0,0])
        self.simend = False

    def status(self):
        print("Altitude:", self.state[0])
        print("Velocity:", self.state[1])
        print("Acceleration:", self.state[2])
        print("Apogee hit:", self.apogee)
        for p in self.parachutes:
            print(p.name, "deployed:", p.deployed)
        print("Ground hit:", self.simend)

    def compact_status(self, t):
        print(t, self.state, self.apogee, [p.deployed for p in self.parachutes], self.simend)
```

```python
'''subarktos_sim_no_main.csv has data from the reference simulator. We're going to consider this to be our sensor data,
because then we know exactly what the noise is. We initialize sensor data before the rocket because the rocket
needs information on how noisy its sensors are.'''

%matplotlib inline
sim = pd.read_csv("subarktos/subarktos_sim_no_main.csv", comment='#')
read_t = np.array(sim['Time (s)'])
read_alt = np.array(sim['Altitude (ft)'])
read_vel = np.array(sim['Vertical velocity (ft/s)']) # not a sensor
read_acc = np.array(sim['Vertical acceleration (ft/s^2)'])
t_apogee = 16.208 # I read this off the csv, ideally I'd parse the comments
read_drag = np.array(sim['Drag force (N)'])
read_thrust = np.array(sim['Thrust (N)'])
read_mass = np.array(sim['Mass (oz)'])
read_gravity = np.array(sim['Gravitational acceleration (ft/s^2)']) * read_mass * 0.008641

def force(t):
    n = np.where(read_t == t)[0][0]
    if t < t_apogee:
        return read_thrust[n] - read_drag[n] - read_gravity[n]
    else:
        return read_drag[n] - read_gravity[n]

read_force = (np.vectorize(force))(read_t)

# To demonstrate filtering, we apply Gaussian noise, which we're going to try and remove.

sigma_alt = np.max(read_alt)/64
sigma_acc = np.max(read_acc)/64
alt_noise = np.random.normal(0, np.sqrt(sigma_alt), np.size(read_alt))
acc_noise = np.random.normal(0, np.sqrt(sigma_acc), np.size(read_acc))
```

```python
# to get the drag coefficient for subarktos, we curve-fit drag to velocity squared over the ascent.
n_ascent = np.where(np.isclose(read_t, t_apogee, atol=1e-2))[0][0]
v2 = read_vel**2
plt.plot(v2[np.arange(0,n_ascent,1)], read_drag[np.arange(0,n_ascent,1)], label="Data")
slope, intercept, _, _, _ = stats.linregress(v2[np.arange(0,n_ascent,1)],read_drag[np.arange(0,n_ascent,1)])
plt.plot(read_vel**2, slope*(read_vel**2) + intercept, label="Curve fit")
plt.legend(loc='lower right')
```

```python
'''Initializing Sub-Arktos. A couple of things are done weirdly because I'm pulling from simulation data, some of which
is being considered simulation data and some of which will be used as sensor data. Usually the motor's final mass
would be nonzero and would be measured beforehand without the motor in it, here I've just shifted mass around a bit.
Also, sigma_alt and sigma_acc are manufactured numbers; to test those you could just have both sensors read while at
rest and find their variances. Finally, the weirdest part is drag is determined by curve-fitting (and it's pulling
the _wrong way_ for some reason).
'''

mass = read_mass[-1] * 0.0283495 #kg
motor = Motor(1.223 - 0.713, 0, "subarktos/J450.txt") # motor starts with a mass 1.223 kg and ends with 0.713 kg.
# These are standard numbers you can look up for any COTS motor.
drogue = Parachute(0.3, 1.8, "Drogue")  # drogue parachute diameter in m, and Cd.
#main = Parachute(0.9, 2.2, "Main", alt=800)
altimeter = Sensor(0, sigma_alt, None) # sensor is reading state variable 0 (altitude) and has the given variance
accelerometer = Sensor(2, sigma_acc, None) # reading state variable 2 (acceleration)
drag = slope # determined via curve fitting to drag = -bv*2
subarktos = Rocket(mass, [drogue], [motor], [altimeter, accelerometer], drag)

def test_acc_differences(subarktos, read_t, read_acc):
    def new_ext_input(t):
        if t < np.min(read_t) or t > np.max(read_t):
            return (0, "OpenRocket sim data")
        return (interp.interp1d(read_t, read_acc)(t)/3.28 * subarktos.get_mass(t), "OpenRocket sim data")

    subarktos.ext_input = new_ext_input
    return subarktos

#subarktos = test_acc_differences(subarktos, read_t, read_acc)

```

```python
test_time = np.linspace(0,0.01,100)
plt.plot(test_time, np.vectorize(lambda x: [print(subarktos.ext_input(x)[1]), subarktos.ext_input(x)[0]][1])(test_time))
```

```python
test_time = np.linspace(0,0.01,100)
plt.plot(test_time, np.vectorize(interp.interp1d(read_t, read_force))(test_time))
```

```python
model_mass = np.vectorize(subarktos.get_mass)(read_t)
plt.plot(read_t, (np.array(sim['Mass (oz)']) * 0.0283495 - model_mass))
```

```python
plt.plot(read_t, read_thrust - np.vectorize(subarktos.get_thrust)(read_t))
```

```python
#plt.plot(np.linspace(0,120,10000), np.vectorize(lambda x: subarktos.ext_input(x)[0])(np.linspace(0,120,10000)))
```

```python
# Simulating the system
subarktos.reset()
times, states, inputs = subarktos.simulate(dt=0.001, timeout=120, verbose=False)
```

```python
# let's see how that compares to the reference simulation.

plt.figure()
plt.plot(times, 3.28*states[0], label='subarktos.simulate()')
plt.plot(read_t, read_alt, label='OpenRocket simulation')
plt.legend(loc='upper right')
plt.xlabel("Time (s)")
plt.ylabel("Altitude (ft)")
```

```python
plt.figure()
plt.plot(times, 3.28*states[1], label='subarktos.simulate()')
plt.plot(read_t, read_vel, label='OpenRocket simulation')
plt.legend(loc='upper right')
plt.xlabel("Time (s)")
plt.ylabel("Velocity (ft/s)")
```

```python
plt.figure()
plt.plot(times, 3.28*states[2], label='subarktos.simulate()')
plt.plot(read_t, read_acc, label='OpenRocket simulation')
plt.legend(loc='upper right')
plt.xlabel("Time (s)")
plt.ylabel("Acceleration (ft/s^2)")
```

```python
'''Now let's filter! We can do that by telling the simulation to update using noisy sensor data instead of no data.
Later this'll pull from subarktos.sensors, but for now they're passed in as a parameter to 'simulate'.'''

sensor_data = np.vstack([(read_alt + alt_noise, read_acc + acc_noise)]).T
filtered_times, filtered_states, _ = subarktos.simulate(dt=0.001, timeout=120, kalman=(read_t, sensor_data))
```

```python
# To see what that did, let's plot the altimeter readings (without noise) over the filtered results
plt.plot(read_t, read_alt, label="OpenRocket without noise")
plt.plot(filtered_times, 3.28*filtered_states[0], label="subarktos.simulate() filtered from OpenRocket + noise")
plt.legend(loc='upper right')
plt.title("Kalman filtered rocket altitude against true values.")
plt.xlabel("Time (s)")
plt.ylabel("Altitude (ft)")
```

```python
# and the accelerometer readings
plt.plot(read_t, read_acc, label="OpenRocket without noise")
plt.plot(filtered_times, 3.28*filtered_states[2], label="subarktos.simulate() filtered from OpenRocket + noise")
plt.legend(loc='upper right')
plt.title("Kalman filtered rocket acceleration against true values.")
plt.xlabel("Time (s)")
plt.ylabel(r"Acceleration $(ft/s^2)$")
```

```python
# To really see the effect of filtering, let's plot the errors in both before and after filtering.

def interpolate_state_data(data):
        def toreturn(t):
            if t < np.min(read_t) or t > np.max(read_t):
                return 0
            return interp.interp1d(read_t, data)(t)
        return toreturn

alt_error_after = 3.28*filtered_states[0] - np.vectorize(interpolate_state_data(read_alt))(filtered_times)
acc_error_after = 3.28*filtered_states[2] - np.vectorize(interpolate_state_data(read_acc))(filtered_times)
```

```python
# altitude error
plt.plot(read_t, alt_noise, label="Error before filtering")
plt.plot(filtered_times, alt_error_after, label="Error after filtering")
plt.title("Altitude error reduction due to Kalman filtering.")
plt.legend(loc='upper right')
plt.xlabel("Time (s)")
plt.ylabel("Error in altitude (ft)")

print(np.std(alt_noise))
print(np.std(alt_error_after))
```

```python
# acceleration error
'''n = np.where(np.isclose(read_t, t_apogee, atol=1e-2))[0][0]
m = np.where(np.isclose(filtered_times, t_apogee, atol=1e-2))[0][0]
ns = np.where(np.isclose(read_t, 0.2,atol=1e-2))[0][0]
ms = np.where(np.isclose(filtered_times,0.2,atol=1e-2))[0][0]'''
plt.plot(read_t, acc_noise, label="Error before filtering")
plt.plot(filtered_times, acc_error_after, label="Error after filtering")
plt.title("Acceleration error reduction due to Kalman filtering.")
plt.legend(loc='lower right')
plt.xlabel("Time (s)")
plt.ylabel(r"Error in acceleration $(ft/s^2)$")

print(np.std(acc_noise))
print(np.std(acc_error_after))
```


# Optimization

Marcos Duarte

> “If there occur some changes in nature, the amount of action necessary for this change must be as small as possible.” Maupertuis (sec XVIII).

## Definition

Optimization is the process of finding the best value from possible alternatives with regards to a certain criteria [Wikipedia](http://en.wikipedia.org/wiki/Mathematical_optimization).
Usually, such best value is the value that maximizes or minimizes the criteria. In this context, to solve an optimization problem is to find the maximum or minimum of a function. To solve an optimization problem, we first have to model the problem and define the objective, the variables, and the constraints of the problem. In optimization, these terms are usually defined as:

1. Cost or objective function: a function describing what we want to optimize.
2. Design variable(s): variables that will be manipulated to optimize the cost function.
3. Constraint functions: constrain the solution to a set of possible values.

Read more about that in [Introduction to Optimization](http://neos-guide.org/content/optimization-introduction) from the  [NEOS Guide](http://neos-guide.org/).

## Maxima and minima

In mathematics, the maximum and minimum of a function are the largest and smallest values that the function takes at a point either within a neighborhood (local) or on the function entire domain  (global) ([Wikipedia](http://en.wikipedia.org/wiki/Maxima_and_minima)). For a function of one variable, if the maximum or minimum of a function is not at the limits of the domain and if at least the first and second derivatives of the function exist, a maximum and minimum can be found as the point where the first derivative of the function is zero. If the second derivative on that point is positive, then it's a minimum, if it is negative, it's a maximum.

Figure. Maxima and minima of a function of one variable.

### Maximum range of projectile motion

What is the optimal angle to throw a projectile to achieve the greatest distance?

The equations of motion for a projectile launched with an angle $\theta$ with the horizontal (consider constant gravitational acceleration, $g$) and neglecting the air resistance are:

$$ \begin{array}{l l}
x(t) = x_0 + v_0\cos(\theta)\:t \\
y(t) = y_0 + v_0\sin(\theta)\:t - \frac{g\:t^2}{2} \\
\\
v_x(t) = v_0\cos(\theta) \\
v_y(t) = v_0\sin(\theta) - g\:t \\
\\
a_x(t) = 0 \\
a_y(t) = -g
\end{array} $$

**Time of flight**
The time of flight can be calculated from the equation for the vertical velocity and using the properties that at the maximum height, the vertical velocity is zero and the time of rising is equal to the time of falling:

$$ t_{flight} = \frac{2v_0\sin(\theta)}{g} $$

**Range**
The range, $R$, is the horizontal distance reached by the projectile with respect to the point of release:

$$ \begin{array}{l l}
R = v_0\cos(\theta)\:t_{flight} \\
\\
R = \frac{v_0^2\sin(2\theta)}{g}
\end{array} $$

The maximum range is obtained by finding the angle that maximizes the equation above. In the context of optimization, the equation for $R$ is the cost function, $\theta$ is the design variable, and the  constraint is that feasible angles are in the interval $[0,\: 90^o]$.
The solution of this problem is trivial, we know by inspection that the maximum range is given when $\sin(2\theta)$ is maximum, and for that, $\theta=45^o$. But let's show the solution with the  approach using derivatives:

$$ \frac{\mathrm{d} R}{\mathrm{d} \theta} = \frac{2v_0^2\cos(2\theta)}{g} $$

Find the value of $\theta$ where this derivative is zero:

$$ \frac{2v_0^2\cos(2\theta)}{g} = 0 \quad \Rightarrow \quad \theta=45^o $$

We can check this value corresponds to a maximum if the second derivative of $R$ at this value is negative:

$$ \frac{\mathrm{d}^2 R}{\mathrm{d} \theta^2} = -\frac{4v_0^2\sin(2\theta)}{g} $$

Which indeed is negative at $\theta=45^o$.

We can use Sympy to solve the steps of this problem. Let's import the necessary Python libraries and customize the environment for that:

```python
import numpy as np
%matplotlib inline
import matplotlib
import matplotlib.pyplot as plt
matplotlib.rcParams['lines.linewidth'] = 3
matplotlib.rcParams['font.size'] = 13
matplotlib.rcParams['lines.markersize'] = 5
matplotlib.rc('axes', grid=False, labelsize=14, titlesize=16, ymargin=0.05)
matplotlib.rc('legend', numpoints=1, fontsize=11)

import sympy as sym
from sympy.plotting import plot
#from sympy.interactive import printing
#printing.init_printing()
from IPython.display import display
from IPython.core.display import Math
```

Let's create symbolic variables, define the equation, differentiate it, and find the maximum value:

```python
v0, g, theta = sym.symbols('v0 g theta')
R = v0**2*sym.sin(2*theta)/g
Rdiff = sym.diff(R, theta)
display(Math(sym.latex('R =') + sym.latex(R)))
display(Math(sym.latex(r'\frac{\mathrm{d}R}{\mathrm{d}\theta} =') + sym.latex(Rdiff)))
ang = sym.solve(Rdiff, theta)
display(Math(sym.latex('Sols:') + sym.latex(ang)))
display(Math(sym.latex(r'R(\theta=\pi/4)=') +
             sym.latex(sym.N(R.subs({v0: 10, g: 9.8, theta: sym.pi/4}), 4))))
plot(R.subs({v0: 10, g: 9.8}), (theta, 0, sym.pi/2),
     xlabel=r'$\theta(rad)$', ylabel=r'$R$');
```

### Maximum volume of a box

We want to make a box from a square cardboard with side $a$ such that its volume should be maximum. What is the optimal distance where the square cardbox should be cut and folded to make a box with maximum volume?

Figure. A box to be made from a cardboard such that its volume should be maximum. Where we should cut?

If the distance where to cut and fold the cardbox is $b$, see figure above, the volume of the box will be:

$$ \begin{array}{l l}
V(b) = b(a-2b)(a-2b) \\
\\
V(b) = a^2b - 4ab^2 + 4b^3
\end{array} $$

The expression for $V$ is the cost function, $b$ is the design variable, and the constraint is that feasible values of $b$ are in the interval $[0, a/2]$.
The first derivative of $V$ is:

$$ \frac{\mathrm{d}V}{\mathrm{d}b} = a^2 - 8ab + 12b^2  $$

We have to find the values for $b$ where this derivative is zero. Let's use Sympy for that:

```python
a, b = sym.symbols('a b')
V = b*(a - 2*b)*(a - 2*b)
Vdiff = sym.expand(sym.diff(V, b))
roots = sym.solve(Vdiff, b)
display(Math(sym.latex('V =') + sym.latex(V)))
display(Math(sym.latex('\\frac{\mathrm{d}V}{\mathrm{d}b} =') + sym.latex(Vdiff)))
display(Math(sym.latex('Roots:') + sym.latex(roots)))
```

$$V =b \left(a - 2 b\right)^{2}$$

$$\frac{\mathrm{d}V}{\mathrm{d}b} =a^{2} - 8 a b + 12 b^{2}$$

$$Roots:\left [ \frac{a}{6}, \quad \frac{a}{2}\right ]$$

Discarding the solution $b=a/2$ (where $V=0$), $b=a/6$ results in the maximum volume. We can check that by ploting the volume of the cardbox for $a=1$ and $b: [0,\:0.5]$:

```python
plot(V.subs({a: 1}), (b, 0, .5), xlabel='b', ylabel='V')
display(Math(sym.latex('V_{a=1}^{max} =') +
             sym.latex(sym.N(V.subs({b: a/6}).subs({a: 1}), 2))))
```

The examples above are trivial problems of optimization; there was only one design variable in each case and the cost functions, although nonlinear, were simple. When there is more than one design variable (the cost function depends on more than one variable), the general idea of finding minimum and maximum values where the derivatives are zero still holds, but the problem gets more complicated. For an introduction on that, particularly applied to biomechanics, see chapter 4 (start on page 609) of Nigg and Herzog (2006).

Let's see now a classical problem in biomechanics where optimization is useful and there is more than one design variable.

## The distribution problem in biomechanics

Using the inverse dynamics approach in biomechanics, we can determine the net force and torque acting on a joint if we know the external forces on the segments and the kinematics and inertial properties of the segments. But with this approach we are unable to determine the individual muscles forces that  created such torque, as expressed in the following equation:

$$ M_{total} = M_1 + M_2 + \dots + M_n = r_1F_1 + r_2F_2 + \dots + r_nF_n $$

where $r_i$ is the moment arm of the force $F_i$ that generates a torque $M_i$, a parcel of the (known) total torque $M_{total}$.

Even if we know the moment arm of each muscle (e.g., from cadaveric data or from image analysis), the equation above has $n$ unknowns. Because there is more than one muscle that potentially created such torque, there are more unknowns than equations, and the problem is undetermined. So, the problem is how to find how the torque is distributed among the muscles of that joint.

One solution is to consider that we (biological systems) optimize our effort in order to minimize energy expenditure, stresses on our tissues, fatigue, etc. The principle of least action, stated in the opening of this text, is an allusion that optimization might be ubiquitous in nature. With this rationale, let's solve the distribution problem in biomechanics using optimization and find the minimum force of each muscle necessary to complete a given task.

The following cost functions have been proposed to solve the distribution problem in biomechanics:

$$ \begin{array}{l l}
\sum_{i=1}^N F_i \quad &\text{(e.g., Seireg and Arkivar, 1973)}
\\
\sum_{i=1}^N F_i^2 \quad &
\\
\sum_{i=1}^N \left(\frac{F_i}{pcsa_i}\right)^2 \quad &\text{(e.g., Crowninshield and Brand, 1981)}
\\
\sum_{i=1}^N \left(\frac{F_i}{M_{max,i}}\right)^3 \quad &\text{(e.g., Herzog, 1987)}
\end{array} $$

Where $pcsa_i$ is the physiological cross-sectional area of muscle $i$ and $M_{max,i}$ is the maximum torque muscle $i$ can produce.
Each muscle force $F_i$ is a design variable and the following constraints must be satisfied:

$$ \begin{array}{l l}
0 \leq F_i \leq F_{max}
\\
\sum_{i=1}^N r_i \times F_i = M
\end{array} $$

Let's apply this concept to solve a distribution problem in biomechanics.

### Muscle force estimation

Consider the following main flexors of the elbow joint (see figure below): biceps long head, biceps short head, and brachialis. Suppose that the elbow net joint torque determined using inverse dynamics is 20 Nm (flexor). How much each of these muscles contributed to the net torque?

Figure. A view in OpenSim of the arm26 model showing three elbow flexors (Biceps long and short heads and Brachialis).

For the optimization, we will need experimental data for the moment arm, maximum moment, and pcsa of each muscle. Let's import these data from the OpenSim arm26 model:

```python
# time elbow_flexion BIClong BICshort BRA
r_ef = np.loadtxt('./../data/r_elbowflexors.mot', skiprows=7)
f_ef = np.loadtxt('./../data/f_elbowflexors.mot', skiprows=7)
```

The maximum isometric force of these muscles are defined in the arm26 model as: Biceps long head: 624.3 N, Biceps short head: 435.56 N, and Brachialis: 987.26 N. Let's compute the mamimum torques that each muscle could produce considering a static situation at the different elbow flexion angles:

```python
m_ef = r_ef*1
m_ef[:, 2:] = r_ef[:, 2:]*f_ef[:, 2:]
```

And let's visualize these data:

```python
labels = ['Biceps long head', 'Biceps short head', 'Brachialis']
fig, ax = plt.subplots(nrows=1, ncols=3, sharex=True, figsize=(10, 4))
ax[0].plot(r_ef[:, 1], r_ef[:, 2:])
#ax[0].set_xlabel('Elbow angle $(\,^o)$')
ax[0].set_title('Moment arm (m)')
ax[1].plot(f_ef[:, 1], f_ef[:, 2:])
ax[1].set_xlabel('Elbow angle $(\,^o)$', fontsize=16)
ax[1].set_title('Maximum force (N)')
ax[2].plot(m_ef[:, 1], m_ef[:, 2:])
#ax[2].set_xlabel('Elbow angle $(\,^o)$')
ax[2].set_title('Maximum torque (Nm)')
ax[2].legend(labels, loc='best', framealpha=.5)
ax[2].set_xlim(np.min(r_ef[:, 1]), np.max(r_ef[:, 1]))
plt.tight_layout()
plt.show()
```

These data don't have the pcsa value of each muscle. We will estimate the pcsa considering that the amount of maximum muscle force generated per area is constant and equal to $50N/cm^2$. Consequently, the pcsa (in $cm^2$) for each muscle is:

```python
a_ef = np.array([624.3, 435.56, 987.26])/50  # 50 N/cm2
```

### Static versus dynamic optimization

We can solve the distribution problem separately for each angle (instant) of the elbow; we will refer to that as static optimization. However, there is no guarantee that when we analyze all these solutions across the range of angles, they will be the best solution overall. One reason is that static optimization ignores the time history of the muscle force. Dynamic optimization refers to the optimization over a period of time. For such, we will need to input a cost function spanning the entire period of time at once. Dynamic optimization usually has a higher computational cost than static optimization.

For now, we will solve the present problem using static optimization.

### Solution of the optimization problem

For the present optimization, we are dealing with a problem of minimization, multidimensional (function of several variables), nonlinear, constrained, and we can't assume that the cost function is convex. Numerical optimization is hardly a simple task. There are many different algorithms and public and commercial software for performing optimization. For instance, look at [NEOS Server](http://www.neos-server.org/neos/), a free internet-based service for solving numerical optimization problems.
We will solve the present problem using the [scipy.optimize](http://docs.scipy.org/doc/scipy/reference/optimize.html#module-scipy.optimize) package which provides several optimization algorithms. We will use the function `minimize`:

```python
scipy.optimize.minimize(fun, x0, args=(), method=None, jac=None, hess=None, hessp=None, bounds=None, constraints=(), tol=None, callback=None, options=None)
"""Minimization of scalar function of one or more variables."""
```

Now, let's write Python functions for each cost function:

```python
from scipy.optimize import minimize
```

```python
def cf_f1(x):
    """Cost function: sum of forces."""
    return x[0] + x[1] + x[2]

def cf_f2(x):
    """Cost function: sum of forces squared."""
    return x[0]**2 + x[1]**2 + x[2]**2

def cf_fpcsa2(x, a):
    """Cost function: sum of squared muscle stresses."""
    return (x[0]/a[0])**2 + (x[1]/a[1])**2 + (x[2]/a[2])**2

def cf_fmmax3(x, m):
    """Cost function: sum of cubic forces normalized by moments."""
    return (x[0]/m[0])**3 + (x[1]/m[1])**3 + (x[2]/m[2])**3
```

Let's also define the Jacobian for each cost function (which is an optional parameter for the optimization):

```python
def cf_f1d(x):
    """Derivative of cost function: sum of forces."""
    dfdx0 = 1
    dfdx1 = 1
    dfdx2 = 1
    return np.array([dfdx0, dfdx1, dfdx2])

def cf_f2d(x):
    """Derivative of cost function: sum of forces squared."""
    dfdx0 = 2*x[0]
    dfdx1 = 2*x[1]
    dfdx2 = 2*x[2]
    return np.array([dfdx0, dfdx1, dfdx2])

def cf_fpcsa2d(x, a):
    """Derivative of cost function: sum of squared muscle stresses."""
    dfdx0 = 2*x[0]/a[0]**2
    dfdx1 = 2*x[1]/a[1]**2
    dfdx2 = 2*x[2]/a[2]**2
    return np.array([dfdx0, dfdx1, dfdx2])

def cf_fmmax3d(x, m):
    """Derivative of cost function: sum of cubic forces normalized by moments."""
    dfdx0 = 3*x[0]**2/m[0]**3
    dfdx1 = 3*x[1]**2/m[1]**3
    dfdx2 = 3*x[2]**2/m[2]**3
    return np.array([dfdx0, dfdx1, dfdx2])
```

Let's define initial values:

```python
M = 20  # desired torque at the elbow
iang = 69  # which will give the closest value to 90 degrees
r  = r_ef[iang, 2:]
f0 = f_ef[iang, 2:]
a  = a_ef
m  = m_ef[iang, 2:]
x0 = f_ef[iang, 2:]/10  # far from the correct answer for the sum of torques
print('M =', M)
print('x0 =', x0)
print('r * x0 =', np.sum(r*x0))
```

    M = 20
    x0 = [ 57.51311369  36.29974032  89.6470056 ]
    r * x0 = 6.62200444607

Inequality constraints (such as boundaries in our problem) can be entered with the parameter `bounds` to the `minimize` function:

```python
bnds = ((0, f0[0]), (0, f0[1]), (0, f0[2]))
```

Equality constraints (such as the sum of torques should equals the desired torque in our problem), as well as inequality constraints, can be entered with the parameter `constraints` to the `minimize` function (and we can also opt to enter the Jacobian of these constraints):

```python
# use this in combination with the parameter bounds:
cons = ({'type': 'eq',
         'fun' : lambda x, r, f0, M: np.array([r[0]*x[0] + r[1]*x[1] + r[2]*x[2] - M]),
         'jac' : lambda x, r, f0, M: np.array([r[0], r[1], r[2]]), 'args': (r, f0, M)})
```

```python
# to enter everything as constraints:
cons = ({'type': 'eq',
         'fun' : lambda x, r, f0, M: np.array([r[0]*x[0] + r[1]*x[1] + r[2]*x[2] - M]),
         'jac' : lambda x, r, f0, M: np.array([r[0], r[1], r[2]]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: f0[0]-x[0],
         'jac' : lambda x, r, f0, M: np.array([-1, 0, 0]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: f0[1]-x[1],
         'jac' : lambda x, r, f0, M: np.array([0, -1, 0]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: f0[2]-x[2],
         'jac' : lambda x, r, f0, M: np.array([0, 0, -1]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: x[0],
         'jac' : lambda x, r, f0, M: np.array([1, 0, 0]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: x[1],
         'jac' : lambda x, r, f0, M: np.array([0, 1, 0]), 'args': (r, f0, M)},
        {'type': 'ineq', 'fun' : lambda x, r, f0, M: x[2],
         'jac' : lambda x, r, f0, M: np.array([0, 0, 1]), 'args': (r, f0, M)})
```

Although more verbose, if all the Jacobians of the constraints are also informed, this alternative seems better than informing bounds for the optimization process (less error in the final result and less iterations).

Given the characteristics of the problem, if we use the function `minimize` we are limited to the SLSQP (Sequential Least SQuares Programming) solver.

Finally, let's run the optimization for the four different cost functions and find the optimal muscle forces:

```python
f1r = minimize(fun=cf_f1, x0=x0, args=(), jac=cf_f1d,
               constraints=cons, method='SLSQP',
               options={'disp': True})
```

    Optimization terminated successfully.    (Exit mode 0)
                Current function value: 409.5926601
                Iterations: 7
                Function evaluations: 7
                Gradient evaluations: 7

```python
f2r = minimize(fun=cf_f2, x0=x0, args=(), jac=cf_f2d,
               constraints=cons, method='SLSQP',
               options={'disp': True})
```

    Optimization terminated successfully.    (Exit mode 0)
                Current function value: 75657.3847913
                Iterations: 5
                Function evaluations: 6
                Gradient evaluations: 5

```python
fpcsa2r = minimize(fun=cf_fpcsa2, x0=x0, args=(a,), jac=cf_fpcsa2d,
                   constraints=cons, method='SLSQP',
                   options={'disp': True})
```

    Optimization terminated successfully.    (Exit mode 0)
                Current function value: 529.96397777
                Iterations: 11
                Function evaluations: 11
                Gradient evaluations: 11

```python
fmmax3r = minimize(fun=cf_fmmax3, x0=x0, args=(m,), jac=cf_fmmax3d,
                   constraints=cons, method='SLSQP',
                   options={'disp': True})
```

    Optimization terminated successfully.    (Exit mode 0)
                Current function value: 1075.13889317
                Iterations: 12
                Function evaluations: 12
                Gradient evaluations: 12

Let's compare the results for the different cost functions:

```python
dat = np.vstack((np.around(r*100,1), np.around(a,1), np.around(f0,0), np.around(m,1)))
opt = np.around(np.vstack((f1r.x, f2r.x, fpcsa2r.x, fmmax3r.x)),1)
er = ['-', '-', '-', '-',
      np.sum(r*f1r.x)-M, np.sum(r*f2r.x)-M, np.sum(r*fpcsa2r.x)-M, np.sum(r*fmmax3r.x)-M]
data = np.vstack((np.vstack((dat, opt)).T, er)).T

from pandas import DataFrame
rows = ['$\text{Moment arm}\;[cm]$', '$pcsa\;[cm^2]$', '$F_{max}\;[N]$', '$M_{max}\;[Nm]$',
        '$\sum F_i$', '$\sum F_i^2$', '$\sum(F_i/pcsa_i)^2$', '$\sum(F_i/M_{max,i})^3$']
cols = ['Biceps long head', 'Biceps short head', 'Brachialis', 'Error in M']
df = DataFrame(data, index=rows, columns=cols)
print('\nComparison of different cost functions for solving the distribution problem')
df
```

    Comparison of different cost functions for solving the distribution problem

      Biceps long head
      Biceps short head
      Brachialis
      Error in M

      $\text{Moment arm}\;[cm]$
         4.9
         4.9
         2.3
                       -

      $pcsa\;[cm^2]$
        12.5
         8.7
        19.7
                       -

      $F_{max}\;[N]$
       575.0
       363.0
       896.0
                       -

      $M_{max}\;[Nm]$
        28.1
        17.7
        20.4
                       -

      $\sum F_i$
       215.4
       194.2
         0.0
                     0.0

      $\sum F_i^2$
       184.7
       184.7
        86.1
                     0.0

      $\sum(F_i/pcsa_i)^2$
       201.7
        98.2
       235.2
                     0.0

      $\sum(F_i/M_{max,i})^3$
       241.1
       120.9
       102.0
       -3.5527136788e-15

## Comments

The results show that the estimations for the muscle forces depend on the cost function used in the optimization. Which one is correct? This is a difficult question and it's dependent on the goal of the actual task being modeled. Glitsch and Baumann (1997) investigated the effect of different cost functions on the optimization of walking and running and the predicted muscles forces were compared with the electromyographic activity of the corresponding muscles of the lower limb. They found that, among the analyzed cost functions, the minimization of the sum of squared muscle stresses resulted in the best similarity with the actual electromyographic activity.

In general, one should always test different algorithms and different initial values before settling for the solution found. Downey (2011), Kitchin (2013), and Kiusalaas (2013) present more examples on numerical optimization. The [NEOS Guide](http://neos-guide.org/) is a valuable source of information on this topic and [OpenOpt](http://openopt.org/) is a good alternative software for numerical optimization in Python.

## Exercises

1. Regarding the distribution problem for the elbow muscles presented in this text:
    a. Test different initial values for the optimization.
    b. Test other values for the elbow angle where the results are likely to change.

2. In an experiment to estimate forces of the elbow flexors, through inverse dynamics it was found an elbow flexor moment of 10 Nm. Consider the following data for maximum force (F0), moment arm (r), and pcsa (A) of the brachialis, brachioradialis, and biceps brachii muscles: F0 (N): 1000, 250, 700; r (cm): 2, 5, 4; A (cm$^2$): 33, 8, 23, respectively (data from Robertson et al. (2013)).
    a. Use static optimization to estimate the muscle forces.
    b. Test the robustness of the results using different initial values for the muscle forces.
    c. Compare the results for different cost functions.

## References

- Crowninshield RD, Brand RA (1981) [A physiologically based criterion of muscle force prediction in locomotion](http://www.ncbi.nlm.nih.gov/pubmed/7334039). Journal of Biomechanics, 14, 793–801.
- Downey AB (2011) [Physical Modeling in MATLAB](http://greenteapress.com/matlab/). Green Tea Press.
- Herzog W (1987) [Individual muscle force estimations using a non-linear optimal design](http://www.ncbi.nlm.nih.gov/pubmed/3682873). J Neurosci Methods, 21, 167-179.
- Glitsch U, Baumann W (1997) [The three-dimensional determination of internal loads in the lower extremity](http://www.ncbi.nlm.nih.gov/pubmed/9456380). Journal of Biomechanics, 30, 1123–1131.
- Kitchin J (2013) [pycse - Python Computations in Science and Engineering](http://kitchingroup.cheme.cmu.edu/pycse/).
- Kiusalaas (2013) [Numerical methods in engineering with Python 3](http://books.google.com.br/books?id=aJkXoxxoCoUC). 3rd edition. Cambridge University Press.
- Nigg BM and Herzog W (2006) [Biomechanics of the Musculo-skeletal System](https://books.google.com.br/books?id=hOIeAQAAIAAJ&dq=editions:ISBN0470017678). 3rd Edition. Wiley.
- Robertson G, Caldwell G, Hamill J, Kamen G (2013) [Research Methods in Biomechanics](http://books.google.com.br/books?id=gRn8AAAAQBAJ). 2nd Edition. Human Kinetics.
- Seireg A, Arvikar RJ (1973) [A mathematical model for evaluation of forces in lower extremeties of the musculo-skeletal system](http://www.ncbi.nlm.nih.gov/pubmed/4706941). Journal of Biomechanics, 6,  313–322, IN19–IN20, 323–326.


# Linear Regression

## Model

given a dataset $\left \{ (x^{(1)},y^{(1)}),...,(x^{(n)},y^{(n)}) \right \} $, where $x^{(i)} \in \mathbb{R}^{d},\ y^{(i)} \in \mathbb{R}$.

linear regression is trying to model $y^{(i)}$ from $x^{(i)}$ by a linear model:

$$h_{\theta}(x^{(i)}) = \theta_{0} + \theta_{1}x_{1}^{(i)} + \theta_{2}x_{2}^{(i)} + ... + \theta_{d}x_{d}^{(i)}\approx y^{(i)}$$

for the sake of simplicity, we set $x_{0}^{(i)}=1,\ x^{(i)} = (x_{0}^{(i)},...,x_{d}^{(i)}),\ \theta=(\theta_{0},...,\theta_{d})$, then:

$$h_{\theta}(x^{(i)}) = \sum_{j=0}^{d}\theta_{j}x_{j}^{(i)} = \theta^{T}x^{(i)}$$

## Loss Function

given $\hat{y}=(h_{\theta}^{(1)},...,h_{\theta}^{(n)}),\ y=(y^{(1)},...,y^{(n)})$.

we want to approximate $y$ by $\hat{y}$, or equivalent to say, we want to minimize the distance between $\hat{y}$ and $y$.

using euclidean distance, we derive the loss function for linear regression:

$$J(\theta) = \frac{1}{2}\sum_{i=1}^{n}(h_{\theta}x^{(i)} - y^{(i)})^2$$

## Gradient Descent

after defining model and loss function, our goal now is to find the model that minimize the loss function:

$$\hat{\theta} = \underset{\theta}{argmin}\ J(\theta)$$

to minimize the loss function, we can set

$$\nabla J(\theta)=0$$

but to find the analytic solution of this equation is usually impossible.

alternatively, we can init $\theta$ randomly, then iteratively move $\theta$ towards the direction that makes $J(\theta)$ smaller.

remmenber the oposite direction of gradient is the fastest direction that makes functions smaller, we derive gradient descent:

$$\theta := \theta - \alpha\nabla{J(\theta )}$$

$\alpha > 0$ is called the learning rate

appendix: proof of opposite gradient as fastest descent

for all $l\in \mathbb{R}^{d}, \left \| l \right \| =1$

$$\lim_{x \to 0} \frac{J(\theta + x l) - J(\theta ) }{x}=l \cdot \nabla{J(\theta )} >= -\left \| l \right \|\left \|J(\theta)  \right \| = -\left \|  J(\theta )\right \|   $$

equality obtained only if $l$ is in the opposite direction of $\nabla{J(\theta )}$

```python
import numpy as np

X = 2 * np.random.rand(100, 1)
y = 4 + 3 * X + np.random.randn(100, 1)
X_new = np.array([[0], [2]])
```

```python
"""LinearRegression actually uses SVD to get pseudoinverse, based on scipy.linalg.lstsq()"""
from sklearn.linear_model import LinearRegression

lin_reg = LinearRegression()
lin_reg.fit(X, y)
lin_reg.intercept_, lin_reg.coef_
```

    (array([3.8403334]), array([[3.07251955]]))

```python
y_predict = lin_reg.predict(X_new)
y_predict
```

    array([[3.8403334 ],
           [9.98537251]])

```python
import matplotlib.pyplot as plt

plt.plot(X_new, y_predict, "r-", linewidth=2, label="Predictions")
plt.plot(X, y, "b.")
plt.xlabel("$x_1$", fontsize=18)
plt.ylabel("$y$", rotation=0, fontsize=18)
plt.legend(loc="upper left", fontsize=14)
plt.axis([0, 2, 0, 15])
plt.show()
```

### stochastic gradient descent

in practice, to update $\theta$ by gradient descent(concretely batch gradient descent), at each step, we need to calculate all sample's gradient, too slow.

to fix this problem, we can only use one sample's gradient at a time, choose that sample randomly:

$$\theta := \theta - \alpha{l(h_{\theta}(x^{(i)}), y^{(i)})}$$

```python
from sklearn.linear_model import SGDRegressor

sgd_reg = SGDRegressor(max_iter=1000, tol=1e-3, penalty=None, eta0=0.1)
sgd_reg.fit(X, y.ravel())

sgd_reg.intercept_, sgd_reg.coef_
```

    (array([3.89786836]), array([3.17725484]))

### mini-batch gradient descent

sgd too random, batch gradient descent too slow, we can use some(not one, not all) samples at a time:

$$\theta := \theta - \alpha\sum_{i \in batch}{l(h_{\theta}(x^{(i)}), y^{(i)})}$$

## update rule of linear regression

for linear regression, we have:

$$
\begin{equation}
\begin{split}
\frac{\partial }{\partial \theta_{j}}J(\theta ) &=  \frac{\partial }{\partial \theta_{j}}\frac{1}{2}\sum_{i=1}^{n}(h_{\theta }(x^{(i)}) - y^{(i)})^2  \\
&=\sum_{i=1}^{n}(h_{\theta }(x^{(i)}) - y^{(i)})\cdot{}\frac{\partial }{\partial \theta_{j}}(h_{\theta }(x^{(i)}) - y^{(i)})\\
& =\sum_{i=1}^{n}(h_{\theta }(x^{(i)}) - y^{(i)})x_{j}^{(i)}
\end{split}
\end{equation}
$$

so we have the update rule for linear regression:

$$\theta_{j}: =\theta_{j} - \alpha\sum_{i=1}^{n} (h_{\theta }(x^{(i)}) - y^{(i)})x_{j}^{(i)} $$

combine all dimensions, we have:

$$\theta: =\theta - \alpha\sum_{i=1}^{n} (h_{\theta }(x^{(i)}) - y^{(i)})\cdot x^{(i)} $$

### matrix form

define $X = [(x^{(1)})^{T},...,(x^{(n)})^{T}]$, then we can write $J(\theta)$ in matrix form:

$$
\begin{equation}
\begin{split}
J(\theta) &= \frac{1}{2}\sum_{i=1}^{n}(h_{\theta}x^{(i)} - y^{(i)})^2 \\
&= \frac{1}{2}\sum_{i=1}^{n}(\theta^{T}x^{(i)} - y^{(i)})^2 \\
&= \frac{1}{2}(X\theta - y)^{T}(X\theta - y)
\end{split}
\end{equation}
$$

we then have:

$$
\begin{equation}
\begin{split}
\nabla{J(\theta )} &= \nabla\frac{1}{2}(X\theta - y)^{T}(X\theta - y) \\
&= \frac{1}{2}\nabla(\theta^{T}X^{T}X\theta - y^{T}(X\theta) - (X\theta)^{T}y) \\
&= \frac{1}{2}\nabla(\theta^{T}X^{T}X\theta - 2(X^{T}y)^{T}\theta) \\
&= \frac{1}{2}(2X^{T}X\theta - 2(X^{T}y)) \\
&= X^{T}X\theta - X^{T}y
\end{split}
\end{equation}
$$

here we use

1.$a^{T}b=b^{T}a$, obvious.

2.$\nabla{a^{T}x}=a$, obvious.

3.$\nabla{x^{T}Ax} = (A + A^{T})x$, proof:

$$
\begin{equation}
\begin{split}
\frac{\partial}{\partial x_{i}}x^{T}Ax &=
\frac{\partial}{\partial x_{i}}{\sum_{j=1}^{n}}{\sum_{k=1}^{n}}a_{jk}x_{j}x_{k} \\
&= \frac{\partial}{\partial x_{i}}(\sum_{j\ne{i}}a_{ji}x_{j}x_{i} + \sum_{k\ne{i}}a_{ik}x_{i}x_{k} + a_{ii}x_{i}^{2}) \\
&= \sum_{j\ne{i}}a_{ji}x_{j} + \sum_{k\ne{i}}a_{ik}x_{k} + 2a_{ii}x_{ii} \\
&= \sum_{j=1}^{n}a_{ji}x_{j} + \sum_{k=1}^{n}a_{ik}x_{k} \\
&= (A^{T}x)_{i} + (Ax)_{i}
\end{split}
\end{equation}
$$

finally, we get the matrix form of the update rule:

$$\theta: =\theta - \alpha X^{T}(X\theta-\mathbf{y} ) $$

## analytic solution

from above, we have:

$$\nabla{J(\theta )} = X^{T}X\theta - X^{T}y$$

so the equation of zero gradient change to:

$$X^{T}X\theta - X^{T}y = 0$$

if $X^{T}X$ is invertible:

$$\theta = (X^{T}X)^{-1}X^{T}y$$

if $X^{T}X$ is not invertible, the equation also have solution. proof:

on one hand $X\theta = 0 \Rightarrow X^{T}X\theta=0$

on the other hand $X^{T}X\theta=0 \Rightarrow \theta^{T}X^{T}X\theta=0 \Rightarrow (X\theta)^{T}X\theta=0 \Rightarrow X\theta=0$

so $X\theta=0 \Leftrightarrow X^{T}X\theta=0$, that is to say $null(X^{T}X)=null(X)$

we easily derive from the above that $rank(X^{T}X) = rank(X) = rank(X^{T})$

but $range(X^{T}X) \subseteq  range(X^{T})$, so we must have $range(X^{T}X)=range(X^{T})$

```python
X_b = np.c_[np.ones((100, 1)), X]
theta_best = np.linalg.inv(X_b.T.dot(X_b)).dot(X_b.T).dot(y)
theta_best
```

    array([[3.8403334 ],
           [3.07251955]])

## geometric interpretation of the linear regression

consider the linear space $S = span\left \{  columns\ of\ X \right \}$, linear combination of $S$ could be write as $X\theta$, then:

$X\theta$ is the projection of $y$ on $S \Leftrightarrow$ $X\theta - y$ orthogonal with $S \Leftrightarrow$ orthogonal with $columns\ of\ X \Leftrightarrow X^{T}(X\theta - y)=0$

so linear regression could be interpret as finding the projection of $y$ on $S$.

## probabilistic interpretation

assume targets and inputs are related via:

$$y^{(i)} = \theta^{T}x^{(i)} + \epsilon^{(i)}$$

where $\epsilon^{(i)}$ is the error term, assume that $\epsilon^{(i)}$ are distributed IID according to Gaussian with mean 0 and variance $\sigma^{2}$, i.e the density of $\epsilon^{(i)}$ is given by:

$$p(\epsilon^{(i)}) = \frac{1}{\sqrt{2\pi}\sigma}exp\left (-\frac{(\epsilon^{(i)})^{2}}{2\sigma^{2}}\right )$$

this implies:

$$p(y^{(i)}|x^{(i)}; \theta) = \frac{1}{\sqrt{2\pi}\sigma}exp\left ( -\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right)$$

we should denote that $\theta$ is not a random variable.

given $X$ and $\theta$, what is the probability of $y$? we call it the likelihood function:

$$L(\theta) = L(\theta; X,y) = p(y|X; \theta)$$

for the above assumptions:

$$
\begin{equation}
\begin{split}
L(\theta) &= \prod_{i=1}^{n}p(y^{(i)}|x^{(i)}; \theta) \\
&= \prod_{i=1}^{n}\frac{1}{\sqrt{2\pi}\sigma}exp\left ( -\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right)
\end{split}
\end{equation}
$$

we should choose $\theta$ so as to make the data as high probability as possible, i.e we should choose $\theta$ to maxmize $L(\theta)$, this is called maximum likelihood. one step further:

$$maximize\ L(\theta) \Leftrightarrow maxmize\ log(L(\theta))$$

so we maximize the log likelihood, this is simpler.

$$
\begin{equation}
\begin{split}
l(\theta) &= log(L(\theta)) \\
&= log\prod_{i=1}^{n}\frac{1}{\sqrt{2\pi}\sigma}exp\left ( -\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right) \\
&= \sum_{i=1}^{n}log\frac{1}{\sqrt{2\pi}\sigma}exp\left ( -\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right) \\
&= n\ log\frac{1}{\sqrt{2\pi}\sigma} - \frac{1}{2\sigma^{2}}\sum_{i=1}^{n}(y^{(i)} - \theta^{T}x^{(i)})^{2}
\end{split}
\end{equation}
$$

hence, maximizing $l(\theta)$ gives the same answer as minimizing

$$\frac{1}{2}\sum_{i=1}^{n}(y^{(i)} - \theta^{T}x^{(i)})^{2} = J(\theta)$$

so linear regression $\Leftrightarrow $ maximum likelihood given Gaussian error

## regularization

to lower variance $\Rightarrow $ to limit model's complexity $\Rightarrow $ to prevent the absolute value of parameters to be too large $\Rightarrow $ we add punishment term concerning the absolute value of parameters on $J(\theta)$

choose $\lambda{\left \| \theta   \right \|}_{2}^{2}$ as the punishment term:

$$J(\theta) := J(\theta) + \lambda{\left \| \theta   \right \|}_{2}^{2}$$

$\lambda$ is the regularization hyperparameter, linear regression with $l_{2}$ loss is ridge regression.

otherwise, if we choose $\lambda{\left \| \theta   \right \|}_{1}$ as the punishment term:

$$J(\theta) := J(\theta) + \lambda{\left \| \theta   \right \|}_{1}$$

that is called lasso regression.

## probabilistic interpretation of regularization

as before, we assume:

$$p(y^{(i)}|x^{(i)}; \theta) = \frac{1}{\sqrt{2\pi}\sigma}exp\left ( -\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right)$$

in bayes perspective, we can also assume $\theta \sim N(0, \sigma_{0})$, that is:

$$p(\theta) = \frac{1}{\sqrt{2\pi}\sigma_{0}}exp\left ( -\frac{\left \| \theta   \right \|_{2}^{2}}{2\sigma_{0}^{2}}\right)$$

by bayes rule:

$$P(\theta|y)=\frac{P(y|\theta)P(\theta)}{P(y)}$$

thus we have maximum a posteriori estimation(MAP):

$$
\begin{equation}
\begin{split}
\hat{\theta} =&\underset{\theta}{argmax}P(\theta|y)\\
=&\underset{\theta}{argmax}P(y|\theta)P(\theta) \\
=&\underset{\theta}{argmax}\ logP(y|\theta)P(\theta) \\
=&\underset{\theta}{argmax}\ log\ exp\left ( \sum_{i=1}^{n}-\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}}\right)exp\left ( -\frac{\left \| \theta   \right \|_{2}^{2}}{2\sigma_{0}^{2}}\right)\\
=&\underset{\theta}{argmin}\left(\sum_{i=1}^{n}\frac{(y^{(i)} - \theta^{T}x^{(i)})^{2}}{2\sigma^{2}} + \frac{\left \| \theta   \right \|_{2}^{2}}{2\sigma_{0}^{2}}\right)\\
=&\underset{\theta}{argmin}\left(\sum_{i=1}^{n}(y^{(i)} - \theta^{T}x^{(i)})^{2} + \frac{\sigma^{2}}{\sigma_{0}^{2}}\left \| \theta   \right \|_{2}^{2}\right)
\end{split}
\end{equation}
$$

that is exactly minimize:

$$J(\theta) = \frac{1}{2}\sum_{i=1}^{n}(y^{(i)} - \theta^{T}x^{(i)})^2 + \lambda{\left \| \theta   \right \|}^{2}$$

with $\lambda=\frac{\sigma^{2}}{2\sigma_{0}^{2}}$

in one word:

1. linear regression $\Leftrightarrow $ maximum likelihood estimation(MLE) where noise is guassian.
2. ridge regression $\Leftrightarrow $ maximum a posteriori estimation(MAP) where noise and prior are guassian.

```python
from sklearn.linear_model import Ridge

ridge_reg = Ridge(alpha=1, solver="cholesky", random_state=42)
ridge_reg.fit(X, y)
ridge_reg.predict([[1.5]])
```

    array([[8.41224604]])

```python
"""set SGDRegressor penalty=l2"""
sgd_reg = SGDRegressor(penalty="l2", max_iter=1000, tol=1e-3, random_state=42)
sgd_reg.fit(X, y.ravel())
sgd_reg.predict([[1.5]])
```

    array([8.52281944])

## Lasso

```python
"""use Lasso or set SGDRegressor penalty=l1"""
from sklearn.linear_model import Lasso

lasso_reg = Lasso(alpha=0.1)
lasso_reg.fit(X, y)
lasso_reg.predict([[1.5]])
```

    array([8.32572925])

```python
"""combination of ridge and lasso"""
from sklearn.linear_model import ElasticNet

elastic_net = ElasticNet(alpha=0.1, l1_ratio=0.5)
elastic_net.fit(X, y)
elastic_net.predict([[1.5]])
```

    array([8.22901029])

## Polynomial Regression

```python
m = 100
X = 6 * np.random.rand(m, 1) - 3
y = 0.5 * X**2 + X + 2 + np.random.randn(m, 1)
```

```python
plt.plot(X, y, "b.")
plt.xlabel("$x_1$", fontsize=18)
plt.ylabel("$y$", rotation=0, fontsize=18)
plt.axis([-3, 3, 0, 10])
plt.show()
```

```python
from sklearn.preprocessing import PolynomialFeatures

poly_features = PolynomialFeatures(degree=2, include_bias=False)
X_poly = poly_features.fit_transform(X)

lin_reg = LinearRegression()
lin_reg.fit(X_poly, y)
lin_reg.intercept_, lin_reg.coef_
```

    (array([1.84814767]), array([[0.92726154, 0.52786625]]))

```python
X_new=np.linspace(-3, 3, 100).reshape(100, 1)
X_new_poly = poly_features.transform(X_new)
y_new = lin_reg.predict(X_new_poly)
```

```python
plt.plot(X, y, "b.")
plt.plot(X_new, y_new, "r-", linewidth=2, label="Predictions")
plt.xlabel("$x_1$", fontsize=18)
plt.ylabel("$y$", rotation=0, fontsize=18)
plt.legend(loc="upper left", fontsize=14)
plt.axis([-3, 3, 0, 10])
plt.show()
```

```python
"""using pipeline"""
from sklearn.preprocessing import StandardScaler
from sklearn.pipeline import Pipeline

def polynomial_regression(degree, include_bias=True):
    return Pipeline([
        ("poly_features", PolynomialFeatures(degree=degree, include_bias=False)),
        ("std_scaler", StandardScaler()),
        ("lin_reg", LinearRegression()),
    ])
```

```python
poly_reg = polynomial_regression(degree=2)
poly_reg.fit(X, y)

X_new = np.linspace(-3, 3, 100).reshape(100, 1)
y_new = poly_reg.predict(X_new)

plt.plot(X, y, "b.")
plt.plot(X_new, y_new, "r-", linewidth=2, label="Predictions")
plt.xlabel("$x_1$", fontsize=18)
plt.ylabel("$y$", rotation=0, fontsize=18)
plt.legend(loc="upper left", fontsize=14)
plt.axis([-3, 3, 0, 10])
plt.show()
```

```python

```


# Bayesian example with continuous distributions
[](https://github.com/eabarnes1010/course_objective_analysis/tree/main/code)
[](https://colab.research.google.com/github/eabarnes1010/course_objective_analysis/blob/main/code/bayesian_sst_example.ipynb)

You are interested in measuring the sea-surface temperature (SST) off of the Washington coast to within 0.1 units. You know from previous years that the distribution of anomalous sea-surface temperature (denoted as random variable $X$) follows a uniform distribution with bounds $(-2,2)$. The problem is that the instrument you use to measure the temperature is known to introduce an additive error. While the exact error during each measurement is unknown, you know from calibration that the error follows a normal distribution with $\mu = 0$ and $\sigma = 0.75$, denoted at $W$. That is, you measure a random variable $Y$ that is the sum of the two random variables $X$ and $W$: $Y = X + W$.

You go out and take a measurement and the instrument says that the anomalous sea-surface temperature is $y = 2.2$. (a) What is the probability that $X = x \pm \epsilon$ given that you measured $y$, or in math, $\textbf{Pr}({X = x \pm \epsilon | Y = 2.2 \pm \epsilon)}$ for some range determined by $\epsilon$? (b) For each possible value of $y$, what is your best estimate of $x$?

### Part (a): What is the probability that $X = x \pm \epsilon$ given that you measured $y$, or in math, $\textbf{Pr}({X = x \pm \epsilon | Y = 2.2 \pm \epsilon)}$ for some range determined by $\epsilon$?

```python
#.............................................
# IMPORT STATEMENTS
#.............................................
import numpy as np
import matplotlib.pyplot as plt
import importlib
import scipy.stats as stats

#conda install -c conda-forge pymc3
import pymc3 as pm

#.............................................
# PLOTTING COMMANDS
#.............................................
import matplotlib as mpl
import seaborn as sns

# set figure defaults
linewidth_default = 2.0
fig_text_default = 18.
fig_title_default = fig_text_default*1.5

plt.rc('lines', linewidth=linewidth_default)
plt.rc('text', usetex=False)
plt.rc('font', size=fig_text_default, weight='normal',family='sans-serif')
plt.rc('axes',titlesize=fig_title_default,titleweight='bold')
mpl.rcParams['figure.dpi'] = 100
#mpl.rcParams['xtick.labelsize'] = fig_text_default
```

```python
def computeCDF(x,val):
    p = len(x[x<=val])/len(x)
    return p
```

Before we get too far into anything, let's start by plotting the distributions that we know. Namely, we know that SSTs, $X$, follows a uniform distribution, and that the error follows a non-standard normal distribution. Let's plot these and take a look at them.

```python
mu_w = 0.
var_w = 0.75

with pm.Model() as model:
    W = pm.Normal('W', mu=mu_w, sigma=var_w)
    X = pm.Uniform('X', lower=-2, upper=2)
    Y = pm.Deterministic('Y', W+X)

x = np.linspace(-5., 5, 1000)
plt.plot(x,np.exp(W.distribution.logp(x).eval()))
plt.plot(x,np.exp(X.distribution.logp(x).eval()))
plt.show()
```

```python
with model:
    trace = pm.sample(1000000, chains=1)
```

    /Users/eabarnes/opt/anaconda3/envs/env-655-sp22/lib/python3.7/site-packages/ipykernel_launcher.py:2: FutureWarning: In v4.0, pm.sample will return an `arviz.InferenceData` object instead of a `MultiTrace` by default. You can pass return_inferencedata=True or return_inferencedata=False to be safe and silence this warning.

    Auto-assigning NUTS sampler...
    Initializing NUTS using jitter+adapt_diag...
    Sequential sampling (1 chains in 1 job)
    NUTS: [X, W]

<div>
    <style>
        /* Turns off some styling */
        progress {
            /* gets rid of default border in Firefox and Opera. */
            border: none;
            /* Needs to be in here for Safari polyfill so background images work as expected. */
            background-size: auto;
        }
        .progress-bar-interrupted, .progress-bar-interrupted::-webkit-progress-bar {
            background: #F44336;
        }
    </style>
  <progress value='1001000' class='' max='1001000' style='width:300px; height:20px; vertical-align: middle;'></progress>
  100.00% [1001000/1001000 08:12<00:00 Sampling chain 0, 0 divergences]
</div>

    Sampling 1 chain for 1_000 tune and 1_000_000 draw iterations (1_000 + 1_000_000 draws total) took 492 seconds.
    Only one chain was sampled, this makes it impossible to run some convergence checks

These look like we expect. The question is, what is the distribution of $Y = X + W$? This is actually not so obvious. One way we could figure this out is by actually creating two random variables $X$ and $W$ and adding individual samples together for millions and millions of pairs and then plotting *that* distribution. We could do this, or we could use the nifty pacal package - which is a single line.

Now, let's plot the distribution of $X, W$ and their sum $Y$.

```python
pm.plot_posterior(trace)
```

```python
pm.traceplot(trace)
```

```python
plt.figure()
x = np.linspace(-5., 5, 100)
plt.plot(x,np.exp(W.distribution.logp(x).eval()),label='W')
plt.plot(x,np.exp(X.distribution.logp(x).eval()), label='X')
sns.distplot(trace['Y'], label='Y=X+W')
plt.legend(fontsize=10)
plt.show()
```

```python
yvals = trace['Y']
xvals = trace['X']
wvals = trace['W']
print(np.shape(yvals))
print(np.shape(xvals))
print(np.shape(wvals))
```

    (1000000,)
    (1000000,)
    (1000000,)

Looking at the green curve $Y$, perhaps you are not surprised that it lies somewhere "in the middle" of the two other distributions. Either way - there it is! Now we can move forward with the problem at hand, now knowing the distrubtion of the sum of the SST measurements ($X$) with the random measurement error ($W$).

We can now use Bayes' Theorem to compute what we want to know, namely
\begin{equation}
\textbf{Pr}(X = x \pm \epsilon |Y = y = 2.2 \pm \epsilon) = \frac{\textbf{Pr}(Y = y \pm \epsilon | X = x \pm \epsilon) \textbf{Pr}(X = x \pm \epsilon)} {\textbf{Pr}(Y = y \pm \epsilon)}
\end{equation}
where $\epsilon$ is determined by you. Here, we will let $\epsilon = 0.05$.

You know that $X$ follows a uniform distribution, and the distribution of $Y$ given $X = x$ follows the normal distribution of $W$ but centered on $\mu = x$ instead of zero. Determining the distribution of $Y$ is a bit trickier, but we used the pacal module to figure it out.

Plugging these distributions into Bayes' Theorem leads to...

```python
inc = .05
yout = 2.2

xvec = np.arange(-6.+inc/2.,6.+inc/2.,inc)

fx_y_expectation = []
fx_y = []

# probability of Y = yout
fy = computeCDF(yvals,yout+inc/2.) - computeCDF(yvals,yout-inc/2.)

for xout in xvec:

    # probability of Y=2.2 given X=x is just the error distribution about x
    fy_x = stats.norm.cdf(yout+inc/2.,loc = xout,scale = var_w) - stats.norm.cdf(yout-inc/2.,loc = xout,scale = var_w)

    # probability that x = xout
    fx = computeCDF(xvals,xout+inc/2.) - computeCDF(xvals,xout-inc/2.)

    a = (fy_x * fx/fy)

    fx_y.append(a)

fx_y = np.array(fx_y)
fx_y_expectation = np.sum(xvec * fx_y)

plt.figure(figsize=(6,5))
plt.plot(xvec,fx_y, 'o-', markersize = 4, linewidth = 1)

#print np.sum(fx_y)
#print np.trapz(fx_y/inc, dx=inc)
plt.plot([fx_y_expectation,fx_y_expectation],[0,.14],'--',color = 'gray')

plt.xlim(-4.5,4.5)
plt.ylim(0,0.075)
plt.xlabel('x')
plt.title('$\mathit{f}(X = x \pm \epsilon | y = ' + str(yout) + ' \pm \epsilon)$')
plt.ylabel('probability')

plt.show()
```

We see that the probability that $x$ falls within a given interval increases for larger $x$, since we know that $y$ itself is 2.2, and so, to have, say, a negative $x$, we would need to have had a very large error when measuring that sample. The probability goes to zero above 2.0 because we know a priori that $x$ cannot be larger than this.

The dashed gray line denotes the conditional expectation of $X$ based on $Y$, that is - the expectation of $X$ given that $y = 2.2 \pm \epsilon$.

### Part (b): For each possible value of $y$, what is your best estimate of $x$?

To answer this, you must decide what you mean by "best estimate". Here, we will define the best estimate based on the posteriori mean. That is, the best estimate of $X$ given $Y = y$ is defined by the *minimum mean squared error estimate*, which is just the expected value of $X$ given $Y = y$. In the case of $Y = y = 2.2$, the best estimate is $x = 1.46$ as shown by the gray dashed line above.

For other y-values, we need to iterate through them, perform the same calculation as above, and plot the result as a function of $y_{out}$, as shown below.

```python
inc = .05

xvec = np.arange(-6.+inc/2.,6.+inc/2.,inc)
yout_vec = xvec
yout = []

fx_y_expectation = []

for yout in yout_vec: #loop through many possibilites of y, not just y = 2.2

    fx_y = []

    # probability of Y = yout
    fy = computeCDF(yvals,yout+inc/2.) - computeCDF(yvals,yout-inc/2.)

    for xout in xvec:

        # probability of Y=2.2 given X=x is just the error distribution about x
        fy_x = stats.norm.cdf(yout+inc/2.,loc = xout,scale = var_w) - stats.norm.cdf(yout-inc/2.,loc = xout,scale = var_w)

        # probability that x = xout
        fx = computeCDF(xvals,xout+inc/2.) - computeCDF(xvals,xout-inc/2.)
        a = (fy_x * fx/fy)

        fx_y.append(a)

    fx_y = np.array(fx_y)
    fx_y_expectation.append(np.sum(xvec.astype(float) * fx_y))
```

    /Users/eabarnes/opt/anaconda3/envs/env-655-sp22/lib/python3.7/site-packages/ipykernel_launcher.py:23: RuntimeWarning: invalid value encountered in double_scalars
    /Users/eabarnes/opt/anaconda3/envs/env-655-sp22/lib/python3.7/site-packages/ipykernel_launcher.py:23: RuntimeWarning: divide by zero encountered in double_scalars

Now we plot the results.

```python
plt.figure(figsize=(6,5))
plt.plot(yout_vec,fx_y_expectation)
plt.xlabel('y')
plt.title('E(X|Y=y)')
plt.ylabel('x')

plt.xlim(-4.5,4.5)
plt.ylim(-2.5,2.5)
plt.axhline(color='lightgray')

plt.plot([-60,60],[-2,-2],'--',linewidth = 1.5, color = 'black')
plt.plot([-60,60],[2,2],'--',linewidth = 1.5, color = 'black')

plt.show()
```

Performing these calculations for all possible values of $y$ leads to the figure above, which shows the most likely sea-surface temperature given that you measure a particular value of $y$. Note how the curve slowly approaches -2 and 2 since $X$ is bounded by these two values.

```python

```


```python
import sys, os
sys.path.insert(0, os.path.join(os.pardir, 'src'))
from approx1D import least_squares, comparison_plot
import matplotlib.pyplot as plt
import sympy as sym
from math import factorial
import numpy as np

x = sym.Symbol('x')
f = sym.exp(-x)

Omega = [0, 8]

for N in 2,4,6:
    psi = [x**i for i in range(N+1)]
    u, c = least_squares(f,psi,Omega)
    print((N, u))
    comparison_plot(f, u, Omega, filename='tmp_exp_%d' % N,
                    plot_title='N=%d' % N)

```

```python

```

Data Science and Machine Learning/Machine-Learning-In-Python-THOROUGH/EXAMPLES/FINITE_ELEMENTS/INTRO/EXERCICES/07_EXP_POWERS.ipynb


```python
'''import numpy as np
import math
import sympy
import sympy as sp
from sympy import Eq, IndexedBase, symbols, Idx, Indexed, Sum, S, N
from sympy.functions.special.tensor_functions import KroneckerDelta
from sympy.vector import Vector, CoordSys3D, AxisOrienter, BodyOrienter, Del, curl, divergence, gradient, is_conservative, is_solenoidal, scalar_potential, Point, scalar_potential_difference, Del, express, matrix_to_vector
import matplotlib.pyplot as plt
from sympy.physics.vector import ReferenceFrame
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
from IPython.display import display
from IPython.display import display_latex
from sympy import latex
from sympy import Array, Matrix, transpose, zeros, diff, Function, Derivative, cos, sin, sqrt, solve, linsolve, acos, atan, asin
from sympy import symbols
from sympy.plotting import plot'''
from numpy import log, log2
from scipy.linalg import logm, expm

from spatialmath import SO2, SE2
from spatialmath.base import rotx, rot2, det, simplify, skew, vex, trot2, transl2, trplot2
from spatialmath.base import transl, trplot
import spatialmath.base.symbolic as sym
```

```python
np.set_printoptions(formatter={'float': lambda x: "{0: 0.4f}".format(x)})
```

Given
$^A\xi_B$,
A is the reference frame
B is the target frame

$^AP = ^A\xi_B \cdot ^BP$

We use the operator ⊕ to indicate
composition of relative poses

$^AP = (^A\xi_B \oplus ^B\xi_C) \bullet ^CP$

```python
R = rot2(0.2)
d = det(R)
d
```

$\displaystyle 1.0$

```python
theta = sym.symbol('theta')
R = rot2(theta)
R
```

    array([[cos(theta), -sin(theta)],
           [sin(theta), cos(theta)]], dtype=object)

```python
simplify(R * R)
```

$\displaystyle \left[\begin{matrix}\cos^{2}{\left(\theta \right)} & \sin^{2}{\left(\theta \right)}\\\sin^{2}{\left(\theta \right)} & \cos^{2}{\left(\theta \right)}\end{matrix}\right]$

```python
simplify(det(R))
```

$\displaystyle 1$

Matrix Exponential

```python
R = rot2(0.3)
R
```

    array([[ 0.9553, -0.2955],
           [ 0.2955,  0.9553]])

```python
R = rot2(0.3)
S = logm(R)
```

```python
k = skew(2)
```

```python
vex(S)
```

    array([ 0.3000])

```python
expm(S)
```

    array([[ 0.9553, -0.2955],
           [ 0.2955,  0.9553]])

$R = e^{[\theta]_x} \exists SO(2)$

```python
R = rot2(0.3)
R = expm(skew(0.3))
R
```

    array([[ 0.9553, -0.2955],
           [ 0.2955,  0.9553]])

Pose in 2-Dimensions

Homogeneous Transformation Matrix

$^AP = (^AR_B)$

```python
T1 = transl2(1, 2) * trot2(30, 'deg')
T1
```

    array([[ 0.8660, -0.0000,  0.0000],
           [ 0.0000,  0.8660,  0.0000],
           [ 0.0000,  0.0000,  1.0000]])

```python
trplot( transl(1,2,3), frame='A', rviz=True, width=1, dims=[0, 10, 0, 10, 0, 10])
```

```python
trplot2( transl2(1,2) * trot2(30, 'deg'), frame='A', rviz=True, width=1, dims=[0, 5, 0, 5])
```

```python
transl2(1,2) * trot2(30, 'deg')
```

    array([[ 0.8660, -0.0000,  0.0000],
           [ 0.0000,  0.8660,  0.0000],
           [ 0.0000,  0.0000,  1.0000]])

```python

```