# Calculating Receiver Position from Android Raw GNSS Measurements

*This notebook was written by [Mitchell Johnson](https://www.johnsonmitchelld.com/). The source code, as well as the gnssutils Python package containing the EphemerisManager module, can be found on [Github](https://github.com/johnsonmitchelld/gnss-analysis).*

Satellite navigation is absolutely fascinating. The GPS receiver in your smartphone uses concepts as diverse as orbital mechanics, digital signal processing, convex optimization, and the theory of relativity to determine your location anywhere on Earth within seconds. From the development of GPS in the 80's until the early 2010s, satellite navigation was a specialized tool, used primarily by the military, airlines, surveyors, and private citizens fortunate enough to own an expensive personal receiver. Widespread GNSS receiver adoption in smartphones over the past decade, combined with the development of new GNSS constellations from China and the EU, have commoditized satellite navigation like never before.

Despite these changes, GNSS receivers were mostly black boxes until recently. Most standalone receivers output only a final position, velocity, and time solution, with dilution of precision and satellite signal-to-noise ratios included if you're lucky.

That changed in 2016 with Google's introduction of the [raw GNSS measurement API](https://developer.android.com/guide/topics/sensors/gnss) in Android 7. Several applications have since been developed to make use of this data, including Google's own [GnssLogger](https://play.google.com/store/apps/details?id=com.google.android.apps.location.gps.gnsslogger&hl=en_US&gl=US), which provides an interface for logging raw GNSS measurements to a text file. It is now possible for any of the billions of Android device owners worldwide to experiment with raw GNSS measurement data for themselves.

What's still missing is an approachable, easy-to-use platform for processing this data. Google's recent paper, Android Raw GNSS Measurement Datasets for Precise Positioning, contains the following table listing publicly available software packages for GNSS positioning:

Notice that all of the open source options are either written in C/C++ or MATLAB. C and C++ are highly performant languages, but neither is particularly approachable without a software engineering background. MATLAB is much more familiar to engineers and scientists, but I believe its widespread use is harmful for reasons I outline [here](https://www.johnsonmitchelld.com/opinion/2020/12/20/stop-teaching-matlab.html).

To address this void in approachable GNSS software packages built on open source platforms, I decided to develop my own. This Jupyter notebook, along with the accompanying EphemerisManager class, provide everything a user needs to calculate the position of their Android phone using recorded GNSS measurements. All that's necessary (in addition to the phone, of course) is an environment capable of running Jupyter notebooks.

I would like to thank the developers of the tools listed above. I used Google's [GPS Measurement Tools](https://github.com/google/gps-measurement-tools) and the European Space Agency's [GNSS resources](https://gage.upc.edu/tutorials/) as references while developing this project, in addition to many other sources mentioned below.

## Data Aquisition

The experiment below was conducted with a sample dataset I collected with Google's GnssLogger app on my Samsung Galaxy S9 Plus while driving through downtown Seattle. If you'd like to collect your own data and process it with this notebook, you can find the code and documentation [here](https://github.com/johnsonmitchelld/gnss-analysis).

The import process and data directory determination below assume that the Python interpreter's working directory is the same as this notebook, and that the gnssutils package and data repository are located in the parent directory. If that is not the case, you'll have to modify accordingly.

```python
import sys, os, csv
parent_directory = os.path.split(os.getcwd())[0]
ephemeris_data_directory = os.path.join(parent_directory, 'data')
sys.path.insert(0, parent_directory)
from datetime import datetime, timezone, timedelta
import pandas as pd
import numpy as np
import navpy
from gnssutils import EphemerisManager
```

Let's read in a GNSS log file, format the satellite IDs to match up with the RINEX 3.0 standard, and convert the columns we need to calculate receiver position to numeric values. We'll also filter the data so that we're only working with GPS measurements to simplify the rest of the analysis.

```python
# Get path to sample file in data directory, which is located in the parent directory of this notebook
input_filepath = os.path.join(parent_directory, 'data', 'sample', 'gnss_log_2020_12_02_17_19_39.txt')

with open(input_filepath) as csvfile:
    reader = csv.reader(csvfile)
    for row in reader:
        if row[0][0] == '#':
            if 'Fix' in row[0]:
                android_fixes = [row[1:]]
            elif 'Raw' in row[0]:
                measurements = [row[1:]]
        else:
            if row[0] == 'Fix':
                android_fixes.append(row[1:])
            elif row[0] == 'Raw':
                measurements.append(row[1:])

android_fixes = pd.DataFrame(android_fixes[1:], columns = android_fixes[0])
measurements = pd.DataFrame(measurements[1:], columns = measurements[0])

# Format satellite IDs
measurements.loc[measurements['Svid'].str.len() == 1, 'Svid'] = '0' + measurements['Svid']
measurements.loc[measurements['ConstellationType'] == '1', 'Constellation'] = 'G'
measurements.loc[measurements['ConstellationType'] == '3', 'Constellation'] = 'R'
measurements['SvName'] = measurements['Constellation'] + measurements['Svid']

# Remove all non-GPS measurements
measurements = measurements.loc[measurements['Constellation'] == 'G']

# Convert columns to numeric representation
measurements['Cn0DbHz'] = pd.to_numeric(measurements['Cn0DbHz'])
measurements['TimeNanos'] = pd.to_numeric(measurements['TimeNanos'])
measurements['FullBiasNanos'] = pd.to_numeric(measurements['FullBiasNanos'])
measurements['ReceivedSvTimeNanos']  = pd.to_numeric(measurements['ReceivedSvTimeNanos'])
measurements['PseudorangeRateMetersPerSecond'] = pd.to_numeric(measurements['PseudorangeRateMetersPerSecond'])
measurements['ReceivedSvTimeUncertaintyNanos'] = pd.to_numeric(measurements['ReceivedSvTimeUncertaintyNanos'])

# A few measurement values are not provided by all phones
# We'll check for them and initialize them with zeros if missing
if 'BiasNanos' in measurements.columns:
    measurements['BiasNanos'] = pd.to_numeric(measurements['BiasNanos'])
else:
    measurements['BiasNanos'] = 0
if 'TimeOffsetNanos' in measurements.columns:
    measurements['TimeOffsetNanos'] = pd.to_numeric(measurements['TimeOffsetNanos'])
else:
    measurements['TimeOffsetNanos'] = 0

print(measurements.columns)
```

    Index(['ElapsedRealtimeMillis', 'TimeNanos', 'LeapSecond',
           'TimeUncertaintyNanos', 'FullBiasNanos', 'BiasNanos',
           'BiasUncertaintyNanos', 'DriftNanosPerSecond',
           'DriftUncertaintyNanosPerSecond', 'HardwareClockDiscontinuityCount',
           'Svid', 'TimeOffsetNanos', 'State', 'ReceivedSvTimeNanos',
           'ReceivedSvTimeUncertaintyNanos', 'Cn0DbHz',
           'PseudorangeRateMetersPerSecond',
           'PseudorangeRateUncertaintyMetersPerSecond',
           'AccumulatedDeltaRangeState', 'AccumulatedDeltaRangeMeters',
           'AccumulatedDeltaRangeUncertaintyMeters', 'CarrierFrequencyHz',
           'CarrierCycles', 'CarrierPhase', 'CarrierPhaseUncertainty',
           'MultipathIndicator', 'SnrInDb', 'ConstellationType', 'AgcDb',
           'CarrierFrequencyHz', 'Constellation', 'SvName'],
          dtype='object')

## Pre-processing

Those with experience in GNSS data processing will probably notice that we're missing a few important fields for calculating receiver position. For each satellite the process requires:

* **Time of signal transmission** - in order to calculate the satellite's location when the signal was transmitted
* **Measured pseudorange** - a rough estimate of the distance between each satellite and the receiver before correcting for clock biases, ionospheric delay, and other phenomena which we'll explain later
* **Ephemeris parameters** - a set of parameters required to calculate the satellite's position in space

Of these, the only one provided directly by Android's raw GNSS measurement API is the time of signal transmission, labeled `ReceivedSvTimeNanos`. Fortunately, the European Global Navigation Satellite Systems Agency (GSA) published a very helpful [white paper](https://www.gsa.europa.eu/system/files/reports/gnss_raw_measurement_web_0.pdf) explaining the parameters reported by Android and how they can be used to generate the values we need.

Ephemeris parameters are an exception. The Android API technically has commands for obtaining these, but they don't seem to work on my Galaxy S9 - at least not in the GnssLogger app. Instead we'll get them from the [International GNSS Service](https://igs.org/mgex/data-products/#data) (IGS), which maintains an array of GNSS data products available to the public through NASA's Crustal Dynamics Data Information System, the German Bundesamt für Kartographie und Geodäsie (BKG), and the French Institut Géographique National (IGN). More on this later.

### Timestamp Generation

First things first: what time were these measurements taken?

It turns out this question isn't as simple as it might sound. The GPS system has its own time scale, which differs from UTC by some number of leap seconds. Leap seconds are used to adjust UTC for changes in the rotation of the Earth, and are decided by the International Earth Rotation and Reference Systems Service. If you're interested in reading more about leap seconds and international time standards, you can check out this [FAQ](https://www.nist.gov/pml/time-and-frequency-division/nist-time-frequently-asked-questions-faq) from the US National Institute of Standards and Technology.

We're going to need a Python `datetime` timestamp for each measurement in order to determine which ephemeris files to retrieve from IGS. To get one, let's calculate the GPS time in nanoseconds using the equations from Section 2.4 of the GSA's white paper. Then we'll convert it to a normal Unix timestamp using the `Pandas.to_datetime()` function with the GPS reference epoch.

Note that the UTC flag is set to True in the `Pandas.to_datetime()` method. The timestamp column needs to be in UTC because the ephemeris files provided by IGS are labeled by UTC date and time. Since Python's datetime library defaults to local time on some systems, it was necessary to keep everything timezone-aware to avoid mix-ups.

This nomenclature is a bit confusing since we're using the `to_datetime()` method to create a column called `UnixTime`, not `Utc`. Unix time and UTC are technically in the same time zone, but UTC includes leap seconds, while Unix time does not. Since we're creating a timestamp from the number of seconds elapsed since the GPS epoch, the resulting timestamp could differ from UTC by some number of leap seconds. That's close enough to ensure we get the correct ephemeris file, and it won't affect the actual position calculations because we don't use the `UnixTime` column for those.

Finally, let's split the data into measurement epochs. We do this by creating a new column and setting it to 1 whenever the difference between a timestamp and the previous timestamp is greater than 200 milliseconds using the `DataFrame.shift()` command. Then we use the `Series.cumsum()` method to generate unique numbers for the individual epochs.

```python

measurements['GpsTimeNanos'] = measurements['TimeNanos'] - (measurements['FullBiasNanos'] - measurements['BiasNanos'])
gpsepoch = datetime(1980, 1, 6, 0, 0, 0)
measurements['UnixTime'] = pd.to_datetime(measurements['GpsTimeNanos'], utc = True, origin=gpsepoch)
measurements['UnixTime'] = measurements['UnixTime']

# Split data into measurement epochs
measurements['Epoch'] = 0
measurements.loc[measurements['UnixTime'] - measurements['UnixTime'].shift() > timedelta(milliseconds=200), 'Epoch'] = 1
measurements['Epoch'] = measurements['Epoch'].cumsum()
```

Next let's calculate the estimated signal transmission and reception times, once again using the equations found in Section 2.4 of the GSA white paper mentioned above. Once we've determined these, we can calculate a measured pseudorange value for each satellite by taking the difference between the two and multiplying by the speed of light:

$$
\rho_{measured} = \frac{(t_{Rx} - t_{Tx})}{1E9} \cdot c
$$

```python
WEEKSEC = 604800
LIGHTSPEED = 2.99792458e8

# This should account for rollovers since it uses a week number specific to each measurement

measurements['tRxGnssNanos'] = measurements['TimeNanos'] + measurements['TimeOffsetNanos'] - (measurements['FullBiasNanos'].iloc[0] + measurements['BiasNanos'].iloc[0])
measurements['GpsWeekNumber'] = np.floor(1e-9 * measurements['tRxGnssNanos'] / WEEKSEC)
measurements['tRxSeconds'] = 1e-9*measurements['tRxGnssNanos'] - WEEKSEC * measurements['GpsWeekNumber']
measurements['tTxSeconds'] = 1e-9*(measurements['ReceivedSvTimeNanos'] + measurements['TimeOffsetNanos'])
# Calculate pseudorange in seconds
measurements['prSeconds'] = measurements['tRxSeconds'] - measurements['tTxSeconds']

# Conver to meters
measurements['PrM'] = LIGHTSPEED * measurements['prSeconds']
measurements['PrSigmaM'] = LIGHTSPEED * 1e-9 * measurements['ReceivedSvTimeUncertaintyNanos']
```

Now that we have pseudorange values, we can begin the standard process for calculating receiver position. First we need to retrieve the ephemeris data for each satellite from one of the International GNSS Service (IGS) analysis centers. These include NASA's [CDDIS](https://cddis.nasa.gov/Data_and_Derived_Products/GNSS/orbit_products.html) and the German [BKG](https://igs.bkg.bund.de/dataandproducts/index).

This process is, frankly, a pain, so I wrote a Python module to handle the details. You can view the code and documentation [here](https://github.com/johnsonmitchelld/gnss-analysis/tree/main/gnssutils), but all you have to do is initialize an instance of the `EphemerisManager` class with a path to the directory where you want it to cache downloaded files. Then, provide the `get_ephemeris()` method with a Python datetime object and and list of satellites, and it will return a Pandas dataframe of valid ephemeris parameters for the requested satellites.

Broadcast ephemerides are good for four hours from the time they are issued, but the GPS control segment generally uploads new values every two hours. The `EphemerisManager` class downloads a whole day's worth of data at a time and then returns the most recent parameters at the requested time for each satellite. Because of this we're going to run the `get_ephemeris()` method for every epoch in case new parameters were issued since the last measurement was taken.

For the sake of demonstration, let's start by walking through the position solution for just one measurement epoch. We'll use the first epoch in the dataset with measurements from at least five satellites.

```python
manager = EphemerisManager(ephemeris_data_directory)

epoch = 0
num_sats = 0
while num_sats < 5 :
    one_epoch = measurements.loc[(measurements['Epoch'] == epoch) & (measurements['prSeconds'] < 0.1)].drop_duplicates(subset='SvName')
    timestamp = one_epoch.iloc[0]['UnixTime'].to_pydatetime(warn=False)
    one_epoch.set_index('SvName', inplace=True)
    num_sats = len(one_epoch.index)
    epoch += 1

sats = one_epoch.index.unique().tolist()
ephemeris = manager.get_ephemeris(timestamp, sats)
print(timestamp)
print(one_epoch[['UnixTime', 'tTxSeconds', 'GpsWeekNumber']])
```

    Retrieving gnss/data/daily/2020/brdc/brdc3380.20n.gz from gdc.cddis.eosdis.nasa.gov
    2020-12-03 01:19:57.432062+00:00
                                      UnixTime     tTxSeconds  GpsWeekNumber
    SvName
    G05    2020-12-03 01:19:57.432062976+00:00  350397.354393         2134.0
    G07    2020-12-03 01:19:57.432062976+00:00  350397.362980         2134.0
    G08    2020-12-03 01:19:57.432062976+00:00  350397.359065         2134.0
    G09    2020-12-03 01:19:57.432062976+00:00  350397.356830         2134.0
    G13    2020-12-03 01:19:57.432062976+00:00  350397.353015         2134.0
    G14    2020-12-03 01:19:57.432062976+00:00  350397.359692         2134.0
    G27    2020-12-03 01:19:57.432062976+00:00  350397.354134         2134.0
    G28    2020-12-03 01:19:57.432062976+00:00  350397.358284         2134.0
    G30    2020-12-03 01:19:57.432062976+00:00  350397.363215         2134.0

Okay, we've got valid ephemeris parameters and signal transmission time in seconds of the GPS week for one measurement epoch's worth of data. Time to figure out where these satellites are (or where they were at approximately 01:19:57.432 UTC on December 3rd, 2020).

## Coordinate Systems Primer

A brief geometry lesson is in order before I continue. If you ask someone on the street what their location is, they will most likely provide you with some sort of named landmark for reference, e.g. a street name, intersection, or address. Not every location on Earth has a named landmark nearby for reference, so most children learn about the concept of latitude and longitude in school.

### LLA

Latitude, longitude, and altitude (LLA from here on) together make up a [spherical coordinate system](https://en.wikipedia.org/wiki/Spherical_coordinate_system), which was developed over millennia by geographers for use in celestial navigation. Latitude, longitude and altitude are handy because they quickly give you an idea of where you are generally on Earth - positive latitudes are in the northern hemisphere, positive longitudes west of the Prime Meridian, etc. However, spherical coordinates tend to complicate the math involved with satellite navigation because they are not linear. One degree of latitude equals roughly 69 miles (111 km) at the equator but only 49 miles (79 km) at 45 degrees north or south.

### ECEF

The Earth-centered, Earth-fixed ([ECEF](https://en.wikipedia.org/wiki/ECEF)) coordinate system solves this issue. It is a Cartesian coordinate system with coordinates on X, Y, and Z axes which rotate with the Earth. Satellite navigation systems perform most of their calculations in ECEF coordinates before converting to LLA for user output; hence our function for calculating satellite position will output ECEF coordinates.

### ENU/NED

Another challenge arises when calculating the vector **between** two points on Earth. A linear coordinate system is also desired in this case. However, ECEF doesn't quite work for relative positioning on Earth's surface, because, for example, the X and Y axes can point east, west, up, down, or somewhere in between depending on your location. Hence a third system called [local tangent plane coordinates](https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates) is used to define vectors between coordinates. The most common of these are east, north, up (ENU) and north, east, down (NED), which we will use later.

Calculating satellite position from ephemeris data is a bit involved, but the details are conveniently laid out in Tables 20-III and 20-IV of the GPS [Interface Specification Document](https://www.gps.gov/technical/icwg/IS-GPS-200L.pdf). The function below performs this calculation, taking a Pandas dataframe with the ephemeris parameters for every satellite and a dataseries containing the transmit time for all received signals in seconds since the start of the current GPS week. It also calculates the satellite clock offsets for the given transmission time.

```python
def calculate_satellite_position(ephemeris, transmit_time):
    mu = 3.986005e14
    OmegaDot_e = 7.2921151467e-5
    F = -4.442807633e-10
    sv_position = pd.DataFrame()
    sv_position['sv']= ephemeris.index
    sv_position.set_index('sv', inplace=True)
    sv_position['t_k'] = transmit_time - ephemeris['t_oe']
    A = ephemeris['sqrtA'].pow(2)
    n_0 = np.sqrt(mu / A.pow(3))
    n = n_0 + ephemeris['deltaN']
    M_k = ephemeris['M_0'] + n * sv_position['t_k']
    E_k = M_k
    err = pd.Series(data=[1]*len(sv_position.index))
    i = 0
    while err.abs().min() > 1e-8 and i < 10:
        new_vals = M_k + ephemeris['e']*np.sin(E_k)
        err = new_vals - E_k
        E_k = new_vals
        i += 1

    sinE_k = np.sin(E_k)
    cosE_k = np.cos(E_k)
    delT_r = F * ephemeris['e'].pow(ephemeris['sqrtA']) * sinE_k
    delT_oc = transmit_time - ephemeris['t_oc']
    sv_position['delT_sv'] = ephemeris['SVclockBias'] + ephemeris['SVclockDrift'] * delT_oc + ephemeris['SVclockDriftRate'] * delT_oc.pow(2)

    v_k = np.arctan2(np.sqrt(1-ephemeris['e'].pow(2))*sinE_k,(cosE_k - ephemeris['e']))

    Phi_k = v_k + ephemeris['omega']

    sin2Phi_k = np.sin(2*Phi_k)
    cos2Phi_k = np.cos(2*Phi_k)

    du_k = ephemeris['C_us']*sin2Phi_k + ephemeris['C_uc']*cos2Phi_k
    dr_k = ephemeris['C_rs']*sin2Phi_k + ephemeris['C_rc']*cos2Phi_k
    di_k = ephemeris['C_is']*sin2Phi_k + ephemeris['C_ic']*cos2Phi_k

    u_k = Phi_k + du_k

    r_k = A*(1 - ephemeris['e']*np.cos(E_k)) + dr_k

    i_k = ephemeris['i_0'] + di_k + ephemeris['IDOT']*sv_position['t_k']

    x_k_prime = r_k*np.cos(u_k)
    y_k_prime = r_k*np.sin(u_k)

    Omega_k = ephemeris['Omega_0'] + (ephemeris['OmegaDot'] - OmegaDot_e)*sv_position['t_k'] - OmegaDot_e*ephemeris['t_oe']

    sv_position['x_k'] = x_k_prime*np.cos(Omega_k) - y_k_prime*np.cos(i_k)*np.sin(Omega_k)
    sv_position['y_k'] = x_k_prime*np.sin(Omega_k) + y_k_prime*np.cos(i_k)*np.cos(Omega_k)
    sv_position['z_k'] = y_k_prime*np.sin(i_k)
    return sv_position

# Run the function and check out the results:
sv_position = calculate_satellite_position(ephemeris, one_epoch['tTxSeconds'])
print(sv_position)
```

                 t_k   delT_sv           x_k           y_k           z_k
    sv
    G05  4797.354393 -0.000027 -2.169285e+07  4.568031e+06  1.461087e+07
    G07  4797.362980 -0.000426 -3.714100e+06 -1.642940e+07  2.075276e+07
    G08  4797.359065 -0.000002  7.360204e+06 -1.916176e+07  1.671180e+07
    G09  4797.356830 -0.000297 -7.513898e+06 -2.535861e+07  2.249150e+06
    G13  4797.353015  0.000071 -1.324738e+07  1.036799e+07  2.038926e+07
    G14  4797.359692  0.000052 -2.110816e+07 -1.395532e+07  8.071176e+06
    G27  4797.354134 -0.000027  1.340518e+07 -7.095042e+06  2.161816e+07
    G28  4797.358284  0.000648 -2.237537e+07 -1.338044e+07  5.115684e+06
    G30  4797.363215 -0.000346 -1.305779e+07 -9.338017e+06  2.122874e+07

We now have raw pseudorange values, satellite positions, and satellite clock offsets.

These pseudoranges can be repsresented by the equation ([Misra and Enge, 2006](https://www.gpstextbook.com/)):

$$
\begin{equation}
\rho^{(k)}_{measured} = \sqrt{(x^{(k)} - x_r)^2 + (y^{(k)} - y_r)^2 + (z^{(k)} - z_r)^2} + c \cdot \delta t_r  - c \cdot \delta t^{(k)} + I^{(k)}(t) + T^{(k)}(t) + \epsilon_\rho^{(k)}
\end{equation}
$$

where $x_r$, $y_r$, $z_r$, and $ \delta t_r $  are the receiver position (in ECEF coordinates) and clock bias (in seconds), $x^{(k)}$, $y^{(k)}$, $z^{(k)}$, and $ \delta t^{(k)} $ are the position and clock bias of the $k$th satellite, $c$ is the speed of light, and $I^{(k)}(t)$ and $T^{(k)}(t)$ are the ionospheric and trophospheric delay terms for the $k$th satellite. $\epsilon_\rho^{(k)}$ accounts for all remaining measurement and modeling errors. Following convention in the GNSS community, a superscript is used to identify measurements from a specific satellite, with parenthesis to distinguish between superscripts and exponents.

Since we've calculated the satellite clock offsets, let's go ahead and apply them to generate a corrected pseudorange. We're also going to ignore ionospheric and trophospheric delays, thus lumping them into the error term $\epsilon_\rho^{(k)}$. The expression for the corrected pseudorange then becomes:

$$
\rho^{(k)}_{corrected} = \rho^{(k)} + c \cdot \delta t^{(k)} = \sqrt{(x^{(k)} - x_r)^2 + (y^{(k)} - y_r)^2 + (z^{(k)} - z_r)^2} + c \cdot \delta t_r + \epsilon_\rho^{(k)}
$$

```python
#initial guesses of receiver clock bias and position
b0 = 0
x0 = np.array([0, 0, 0])
xs = sv_position[['x_k', 'y_k', 'z_k']].to_numpy()

# Apply satellite clock bias to correct the measured pseudorange values
pr = one_epoch['PrM'] + LIGHTSPEED * sv_position['delT_sv']
pr = pr.to_numpy()
```

The following is adapted from ([Blewitt, 1997](http://www.nbmg.unr.edu/staff/pdfs/blewitt%20basics%20of%20gps.pdf)).

Let's drop the subscript for the corrected pseudorange measurements and refer to them simply as $p^{(k)}$. With four or more linearly independent pseudorange measurements, we end up with the system of equations:

$$
\begin{gather*}
\rho^{(1)} = \sqrt{(x^{(1)} - x_r)^2 + (y^{(1)} - y_r)^2 + (z^{(1)} - z_r)^2} + c \cdot \delta t_r + \epsilon_\rho^{(1)} \\
\rho^{(2)} = \sqrt{(x^{(2)} - x_r)^2 + (y^{(2)} - y_r)^2 + (z^{(2)} - z_r)^2} + c \cdot \delta t_r + \epsilon_\rho^{(2)} \\
\rho^{(3)} = \sqrt{(x^{(3)} - x_r)^2 + (y^{(3)} - y_r)^2 + (z^{(3)} - z_r)^2} + c \cdot \delta t_r + \epsilon_\rho^{(3)} \\
\cdots \\
\rho^{(k)} = \sqrt{(x^{(k)} - x_r)^2 + (y^{(k)} - y_r)^2 + (z^{(k)} - z_r)^2} + c \cdot \delta t_r + \epsilon_\rho^{(k)}
\end{gather*}
$$

where $x_r, y_r, z_r$, and $\delta t_r$ are the unkowns. We will substitute the receiver clock bias term with an equivalent value in meters:

$$
b = c \cdot \delta t_r
$$

Since the system is nonlinear, we must approximate the solution using the [Gauss-Newton algorithm](https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm) for solving [non-linear least squares](https://en.wikipedia.org/wiki/Non-linear_least_squares) problems. Let us define a function, $P$, for calculating an estimated or modeled pseudorange $\hat{\rho}^{(k)}$ based on estimates $\hat{x}, \hat{y}, \hat{z}, \hat{b}$ of the real position and clock offset values $x, y, z, b$:

$$
\hat{\rho}^{(k)} = P(\hat{x}, \hat{y}, \hat{z}, \hat{b}, k) = \sqrt{(x^{(k)}\ - \hat{x})^2 + (y^{(k)} - \hat{y})^2 + (z^{(k)} - \hat{z})} + \hat{b}
$$

We perform a first-order Taylor series expansion about our estimated values:

$$
P(x, y, z, b, k) = P(\hat{x}, \hat{y}, \hat{z}, \hat{b}, k) + P'(\hat{x}, \hat{y}, \hat{z}, \hat{b}, k)(x - \hat{x}, y - \hat{y}, z - \hat{z}, b - \hat{b})
$$

where $P(x, y, z, b, k)$ is equal to our measured or observed psedurange $\rho^{(k)}$. The difference between the observed and computed pseudorange is therefore:

$$
\Delta \rho = P(x, y, z, b, k) - P(\hat{x}, \hat{y}, \hat{z}, \hat{b}, k) = P'(\hat{x}, \hat{y}, \hat{z}, \hat{b}, k)(x - \hat{x}, y - \hat{y}, z - \hat{z}, b - \hat{b})
$$

Converting to matrix form and adding the error term $\epsilon$, we have:

$$
\Delta \rho= \begin{pmatrix} \frac{\delta \rho}{\delta x} & \frac{\delta \rho}{\delta y} &
\frac{\delta \rho}{\delta z} & \frac{\delta \rho}{\delta b} \end{pmatrix}
\begin{pmatrix} \Delta x \\ \Delta y \\ \Delta z \\ \Delta b \end{pmatrix} + \epsilon
$$

Thus for $k$ satellites, we now have the system of $k$ linear equations:

$$
\begin{pmatrix} \Delta \rho^{(1)} \\
\Delta \rho^{(2)} \\
\Delta \rho^{(3)} \\
\vdots \\
\Delta \rho^{(k)}
\end{pmatrix}
=
\begin{pmatrix} \frac{\delta P^{(1)}}{\delta x} & \frac{\delta P^{(1)}}{\delta y} &
\frac{\delta \rho^{(1)}}{\delta z} & \frac{\delta \rho^{(1)}}{\delta b} \\
\frac{\delta \rho^{(2)}}{\delta x} & \frac{\delta \rho^{(2)}}{\delta y} &
\frac{\delta \rho^{(2)}}{\delta z} & \frac{\delta \rho^{(2)}}{\delta b} \\
\frac{\delta \rho^{(3)}}{\delta x} & \frac{\delta \rho^{(3)}}{\delta y} &
\frac{\delta \rho^{(3)}}{\delta z} & \frac{\delta \rho^{(3)}}{\delta b} \\
\vdots & \vdots & \vdots & \vdots \\
\frac{\delta \rho^{(k)}}{\delta x} & \frac{\delta \rho^{(k)}}{\delta y} &
\frac{\delta \rho^{(k)}}{\delta z} & \frac{\delta \rho^{(k)}}{\delta b}
\end{pmatrix}
\begin{pmatrix} \Delta x \\ \Delta y \\ \Delta z \\ \Delta b \end{pmatrix} +
\begin{pmatrix}
\epsilon^{(1)} \\
\epsilon^{(2)} \\
\epsilon^{(3)} \\
\vdots \\
\epsilon^{(k)} \\
\end{pmatrix}
$$

In matrix notation, this equation is commonly written

$$
\mathbf{b} = \mathbf{Ax} + \mathbf{\epsilon}
$$

where vectors are denoted with boldface lower case characters and matrices with boldface upper case. GNSS resources typically refer to the matrix $\mathbf{A}$ as the geometry matrix $\mathbf{G}$. We will follow this convention here. We will call the pseudorange residual matrix $\Delta \mathbf{\rho}$. It also improves clarity to separate the receiver position from the clock offset, so we will continue to denote the clock offset $b$ while writing the position $\Delta x, \Delta y, \Delta z$ in vector form $\Delta \mathbf{x}$. Thus our system of linear equations becomes,

$$
\Delta \mathbf{\rho} = \mathbf{G} \begin{pmatrix} \Delta \mathbf{x} \\ \Delta b \end{pmatrix} + \mathbf{\epsilon}
$$

It turns out the first three columns of $\mathbf{G}$ are just the unit vectors between the $k$th satellite and the estimated receiver position. This is because the unit vector effectively represents the proportion of the pseudorange which is tangent to each dimension. These unit vectors are found by normalizing the vectors $\mathbf{x}^{(k)} - \mathbf{\hat{x}}$ by their magnitudes $r^{(k)}$, where

$$
r^{(k)} = \lVert \mathbf{x}^{(k)} - \mathbf{\hat{x}} \rVert = \sqrt{(x^{(k)} - \hat{x})^2 + (y^{(k)} - \hat{y})^2 + (z^{(k)} - \hat{z})^2}
$$

All values in the fourth column equal 1, because the clock offset adds directly to the pseudorange with no geometric scaling factor required. Thus the geometry matrix G becomes:

$$
\mathbf{G} =
\begin{pmatrix}
\frac{x^{(1)} - \hat{x}}{r^{(1)}} & \frac{y^{(1)} - \hat{y}}{r^{(1)}} &
\frac{z^{(1)} - \hat{z}}{r^{(1)}} & 1 \\
\frac{x^{(2)} - \hat{x}}{r^{(2)}} & \frac{y^{(2)} - \hat{y}}{r^{(2)}} &
\frac{z^{(2)} - \hat{z}}{r^{(2)}} & 1 \\
\frac{x^{(3)} - \hat{x}}{r^{(3)}} & \frac{y^{(3)} - \hat{y}}{r^{(3)}} &
\frac{z^{(3)} - \hat{z}}{r^{(3)}} & 1 \\
\vdots & \vdots & \vdots & \vdots \\
\frac{x^{(k)} - \hat{x}}{r^{(k)}} & \frac{y^{(k)} - \hat{y}}{r^{(k)}} &
\frac{z^{(k)} - \hat{z}}{r^{(k)}} & 1 \\
\end{pmatrix}
$$

For a full derivation of these partial derivatives, refer to Misra and Enge, [Raquet (2013)](http://indico.ictp.it/event/a12180/session/21/contribution/12/material/0/0.pdf), or Ankur Mohan's excellent [blog post](https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/).

The matrix $\Delta \mathbf{\rho}$ is the difference between the measured and calculated pseudorange with the estimated parameters, calculated as follows:

$$
\Delta \mathbf{\rho} =
\begin{pmatrix} \Delta P^{(1)} \\
\Delta P^{(2)} \\
\Delta P^{(3)} \\
\vdots \\
\Delta P^{(k)}
\end{pmatrix}
=
\begin{pmatrix}
\rho^{(1)} - \hat{\rho}^{(1)} \\
\rho^{(2)} - \hat{\rho}^{(2)}  \\
\rho^{(3)} - \hat{\rho}^{(3)}  \\
\vdots \\
\rho^{(k)} -  \hat{\rho}^{(k)}  \\
\end{pmatrix}
=
\begin{pmatrix}
\rho^{(1)} - r^{(1)} - b \\
\rho^{(2)} - r^{(2)} - b \\
\rho^{(3)} - r^{(3)} - b \\
\vdots \\
\rho^{(k)} - r^{(k)} - b  \\
\end{pmatrix}
$$

Solving the linear system to minimise the sum of the squared residuals $\epsilon^{(k)}$ results in the solution to the normal equations,

$$
\begin{pmatrix} \Delta x \\ \Delta y \\ \Delta z \\ \Delta b \end{pmatrix}
=
\begin{pmatrix} \Delta \mathbf{x} \\ \Delta b \end{pmatrix}
=
\begin{pmatrix}
\mathbf{G}^T \mathbf{G}
\end{pmatrix}^{-1} \mathbf{G}^T \Delta \mathbf{\rho}
$$

See p. 17 of [Blewitt](http://www.nbmg.unr.edu/staff/pdfs/blewitt%20basics%20of%20gps.pdf) (1997) for a full derivation of this solution. After performing each linear least squares update, the estimated parameters $\mathbf{\hat{x}}, \mathbf{b}$ are updated:

$$
\begin{pmatrix}
\hat{x}_{i + 1} \\
\hat{y}_{i + 1} \\
\hat{z}_{i + 1} \\
\hat{b}_{i + 1} \\
\end{pmatrix}
=
\begin{pmatrix}
\hat{x}_{i} \\
\hat{y}_{i} \\
\hat{z}_{i} \\
\hat{b}_{i} \\
\end{pmatrix}
+
\begin{pmatrix} \Delta x \\ \Delta y \\ \Delta z \\ \Delta b \end{pmatrix}
$$

This process is performed iteratively until the magnitude of the change in position, $\lVert \Delta \mathbf{x} \rVert$, falls below a set threshold. In practice it usually only takes a few iterations for the solution to converge.

Below is the function to calculate rough receiver position. Its inputs are Numpy arrays containing satellite positions in the ECEF frame, pseudorange measurements in meters, and initial estimates of receiver position and clock bias. The unit for clock bias is meters, so you need to divide by the speed of light to convert to seconds.

This function is adapted and simplified from the MATLAB code in Ankur Mohan's [blog](https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/). It's a simple least squares iteration with no correction for ionospheric delay or satellite movement during signal time of flight.

```python
def least_squares(xs, measured_pseudorange, x0, b0):
    dx = 100*np.ones(3)
    b = b0
    # set up the G matrix with the right dimensions. We will later replace the first 3 columns
    # note that b here is the clock bias in meters equivalent, so the actual clock bias is b/LIGHTSPEED
    G = np.ones((measured_pseudorange.size, 4))
    iterations = 0
    while np.linalg.norm(dx) > 1e-3:
        r = np.linalg.norm(xs - x0, axis=1)
        phat = r + b0
        deltaP = measured_pseudorange - phat
        G[:, 0:3] = -(xs - x0) / r[:, None]
        sol = np.linalg.inv(np.transpose(G) @ G) @ np.transpose(G) @ deltaP
        dx = sol[0:3]
        db = sol[3]
        x0 = x0 + dx
        b0 = b0 + db
    norm_dp = np.linalg.norm(deltaP)
    return x0, b0, norm_dp

x, b, dp = least_squares(xs, pr, x0, b0)
print(navpy.ecef2lla(x))
print(b/LIGHTSPEED)
print(dp)
```

    (47.58626777321704, -122.32842161909167, -4.783203067258)
    2.214374492484914e-07
    29.948493972396502

Looks like it worked! The pseudorange residual is in the tens of meters, which is reasonable considering we didn't correct for ionospheric delay or the rotation of the Earth during signal transmission.

Now let's wrap this entire process into a loop to calculate receiver position for every measurement epoch in the dataset with more than 4 satellite measurements.

```python
ecef_list = []
for epoch in measurements['Epoch'].unique():
    one_epoch = measurements.loc[(measurements['Epoch'] == epoch) & (measurements['prSeconds'] < 0.1)]
    one_epoch = one_epoch.drop_duplicates(subset='SvName').set_index('SvName')
    if len(one_epoch.index) > 4:
        timestamp = one_epoch.iloc[0]['UnixTime'].to_pydatetime(warn=False)
        sats = one_epoch.index.unique().tolist()
        ephemeris = manager.get_ephemeris(timestamp, sats)
        sv_position = calculate_satellite_position(ephemeris, one_epoch['tTxSeconds'])

        xs = sv_position[['x_k', 'y_k', 'z_k']].to_numpy()
        pr = one_epoch['PrM'] + LIGHTSPEED * sv_position['delT_sv']
        pr = pr.to_numpy()

        x, b, dp = least_squares(xs, pr, x, b)
        ecef_list.append(x)

ecef_array = np.stack(ecef_list, axis=0)
lla_array = np.stack(navpy.ecef2lla(ecef_array), axis=1)
ref_lla = lla_array[0, :]
ned_array = navpy.ecef2ned(ecef_array, ref_lla[0], ref_lla[1], ref_lla[2])

```

Finally, we'll convert the list of NED translations into a Pandas dataframe and create a plot of change in position relative to the first measurement epoch.

```python
import matplotlib.pyplot as plt
ned_df = pd.DataFrame(ned_array, columns=['N', 'E', 'D'])
plt.style.use('dark_background')
plt.plot(ned_df['E'], ned_df['N'])

# Add titles
plt.title('Position Offset From First Epoch')
plt.xlabel("East (m)")
plt.ylabel("North (m)")
plt.gca().set_aspect('equal', adjustable='box')
```

```python

```


# Batch Normalization
One way to make deep networks easier to train is to use more sophisticated optimization procedures such as SGD+momentum, RMSProp, or Adam. Another strategy is to change the architecture of the network to make it easier to train.
One idea along these lines is batch normalization which was proposed by [1] in 2015.

The idea is relatively straightforward. Machine learning methods tend to work better when their input data consists of uncorrelated features with zero mean and unit variance. When training a neural network, we can preprocess the data before feeding it to the network to explicitly decorrelate its features; this will ensure that the first layer of the network sees data that follows a nice distribution. However, even if we preprocess the input data, the activations at deeper layers of the network will likely no longer be decorrelated and will no longer have zero mean or unit variance since they are output from earlier layers in the network. Even worse, during the training process the distribution of features at each layer of the network will shift as the weights of each layer are updated.

The authors of [1] hypothesize that the shifting distribution of features inside deep neural networks may make training deep networks more difficult. To overcome this problem, [1] proposes to insert batch normalization layers into the network. At training time, a batch normalization layer uses a minibatch of data to estimate the mean and standard deviation of each feature. These estimated means and standard deviations are then used to center and normalize the features of the minibatch. A running average of these means and standard deviations is kept during training, and at test time these running averages are used to center and normalize features.

It is possible that this normalization strategy could reduce the representational power of the network, since it may sometimes be optimal for certain layers to have features that are not zero-mean or unit variance. To this end, the batch normalization layer includes learnable shift and scale parameters for each feature dimension.

[1] [Sergey Ioffe and Christian Szegedy, "Batch Normalization: Accelerating Deep Network Training by Reducing
Internal Covariate Shift", ICML 2015.](https://arxiv.org/abs/1502.03167)

```python
# As usual, a bit of setup
import time
import numpy as np
import matplotlib.pyplot as plt
from cs231n.classifiers.fc_net import *
from cs231n.data_utils import get_CIFAR10_data
from cs231n.gradient_check import eval_numerical_gradient, eval_numerical_gradient_array
from cs231n.solver import Solver

%matplotlib inline
plt.rcParams['figure.figsize'] = (10.0, 8.0) # set default size of plots
plt.rcParams['image.interpolation'] = 'nearest'
plt.rcParams['image.cmap'] = 'gray'

# for auto-reloading external modules
# see http://stackoverflow.com/questions/1907993/autoreload-of-modules-in-ipython
%load_ext autoreload
%autoreload 2

def rel_error(x, y):
    """ returns relative error """
    return np.max(np.abs(x - y) / (np.maximum(1e-8, np.abs(x) + np.abs(y))))

def print_mean_std(x,axis=0):
    print('  means: ', x.mean(axis=axis))
    print('  stds:  ', x.std(axis=axis))
    print()
```

    run the following from the cs231n directory and try again:
    python setup.py build_ext --inplace
    You may also need to restart your iPython kernel

```python
# Load the (preprocessed) CIFAR10 data.
data = get_CIFAR10_data()
for k, v in data.items():
    print('%s: ' % k, v.shape)
```

    X_train:  (49000, 3, 32, 32)
    y_train:  (49000,)
    X_val:  (1000, 3, 32, 32)
    y_val:  (1000,)
    X_test:  (1000, 3, 32, 32)
    y_test:  (1000,)

## Batch normalization: forward
In the file `cs231n/layers.py`, implement the batch normalization forward pass in the function `batchnorm_forward`. Once you have done so, run the following to test your implementation.

Referencing the paper linked to above in [1] may be helpful!

```python
# Check the training-time forward pass by checking means and variances
# of features both before and after batch normalization

# Simulate the forward pass for a two-layer network
np.random.seed(231)
N, D1, D2, D3 = 200, 50, 60, 3
X = np.random.randn(N, D1)
W1 = np.random.randn(D1, D2)
W2 = np.random.randn(D2, D3)
a = np.maximum(0, X.dot(W1)).dot(W2)

print('Before batch normalization:')
print_mean_std(a,axis=0)

gamma = np.ones((D3,))
beta = np.zeros((D3,))
# Means should be close to zero and stds close to one
print('After batch normalization (gamma=1, beta=0)')
a_norm, _ = batchnorm_forward(a, gamma, beta, {'mode': 'train'})
print_mean_std(a_norm,axis=0)

gamma = np.asarray([1.0, 2.0, 3.0])
beta = np.asarray([11.0, 12.0, 13.0])
# Now means should be close to beta and stds close to gamma
print('After batch normalization (gamma=', gamma, ', beta=', beta, ')')
a_norm, _ = batchnorm_forward(a, gamma, beta, {'mode': 'train'})
print_mean_std(a_norm,axis=0)
```

    Before batch normalization:
      means:  [ -2.3814598  -13.18038246   1.91780462]
      stds:   [27.18502186 34.21455511 37.68611762]

    After batch normalization (gamma=1, beta=0)
      means:  [5.32907052e-17 7.04991621e-17 1.85962357e-17]
      stds:   [0.99999999 1.         1.        ]

    After batch normalization (gamma= [1. 2. 3.] , beta= [11. 12. 13.] )
      means:  [11. 12. 13.]
      stds:   [0.99999999 1.99999999 2.99999999]

```python
# Check the test-time forward pass by running the training-time
# forward pass many times to warm up the running averages, and then
# checking the means and variances of activations after a test-time
# forward pass.

np.random.seed(231)
N, D1, D2, D3 = 200, 50, 60, 3
W1 = np.random.randn(D1, D2)
W2 = np.random.randn(D2, D3)

bn_param = {'mode': 'train'}
gamma = np.ones(D3)
beta = np.zeros(D3)

for t in range(50):
    X = np.random.randn(N, D1)
    a = np.maximum(0, X.dot(W1)).dot(W2)
    batchnorm_forward(a, gamma, beta, bn_param)

bn_param['mode'] = 'test'
X = np.random.randn(N, D1)
a = np.maximum(0, X.dot(W1)).dot(W2)
a_norm, _ = batchnorm_forward(a, gamma, beta, bn_param)

# Means should be close to zero and stds close to one, but will be
# noisier than training-time forward passes.
print('After batch normalization (test-time):')
print_mean_std(a_norm,axis=0)
```

    After batch normalization (test-time):
      means:  [-0.00856644 -0.02735023 -0.05918961]
      stds:   [1.06152724 1.02815831 0.95174132]

## Batch normalization: backward
Now implement the backward pass for batch normalization in the function `batchnorm_backward`.

To derive the backward pass you should write out the computation graph for batch normalization and backprop through each of the intermediate nodes. Some intermediates may have multiple outgoing branches; make sure to sum gradients across these branches in the backward pass.

Once you have finished, run the following to numerically check your backward pass.

```python
# Gradient check batchnorm backward pass
np.random.seed(231)
N, D = 4, 5
x = 5 * np.random.randn(N, D) + 12
gamma = np.random.randn(D)
beta = np.random.randn(D)
dout = np.random.randn(N, D)

bn_param = {'mode': 'train'}
fx = lambda x: batchnorm_forward(x, gamma, beta, bn_param)[0]
fg = lambda a: batchnorm_forward(x, a, beta, bn_param)[0]
fb = lambda b: batchnorm_forward(x, gamma, b, bn_param)[0]

dx_num = eval_numerical_gradient_array(fx, x, dout)
da_num = eval_numerical_gradient_array(fg, gamma.copy(), dout)
db_num = eval_numerical_gradient_array(fb, beta.copy(), dout)

_, cache = batchnorm_forward(x, gamma, beta, bn_param)
dx, dgamma, dbeta = batchnorm_backward(dout, cache)
#You should expect to see relative errors between 1e-13 and 1e-8
print('dx error: ', rel_error(dx_num, dx))
print('dgamma error: ', rel_error(da_num, dgamma))
print('dbeta error: ', rel_error(db_num, dbeta))
```

    dx error:  1.7029235612572515e-09
    dgamma error:  7.420414216247087e-13
    dbeta error:  2.8795057655839487e-12

## Batch normalization: alternative backward
In class we talked about two different implementations for the sigmoid backward pass. One strategy is to write out a computation graph composed of simple operations and backprop through all intermediate values. Another strategy is to work out the derivatives on paper. For example, you can derive a very simple formula for the sigmoid function's backward pass by simplifying gradients on paper.

Surprisingly, it turns out that you can do a similar simplification for the batch normalization backward pass too!

In the forward pass, given a set of inputs $X=\begin{bmatrix}x_1\\x_2\\...\\x_N\end{bmatrix}$,

we first calculate the mean $\mu$ and variance $v$.
With $\mu$ and $v$ calculated, we can calculate the standard deviation $\sigma$  and normalized data $Y$.
The equations and graph illustration below describe the computation ($y_i$ is the i-th element of the vector $Y$).

\begin{align}
& \mu=\frac{1}{N}\sum_{k=1}^N x_k  &  v=\frac{1}{N}\sum_{k=1}^N (x_k-\mu)^2 \\
& \sigma=\sqrt{v+\epsilon}         &  y_i=\frac{x_i-\mu}{\sigma}
\end{align}

The meat of our problem during backpropagation is to compute $\frac{\partial L}{\partial X}$, given the upstream gradient we receive, $\frac{\partial L}{\partial Y}.$ To do this, recall the chain rule in calculus gives us $\frac{\partial L}{\partial X} = \frac{\partial L}{\partial Y} \cdot \frac{\partial Y}{\partial X}$.

The unknown/hart part is $\frac{\partial Y}{\partial X}$. We can find this by first deriving step-by-step our local gradients at
$\frac{\partial v}{\partial X}$, $\frac{\partial \mu}{\partial X}$,
$\frac{\partial \sigma}{\partial v}$,
$\frac{\partial Y}{\partial \sigma}$, and $\frac{\partial Y}{\partial \mu}$,
and then use the chain rule to compose these gradients (which appear in the form of vectors!) appropriately to compute $\frac{\partial Y}{\partial X}$.

If it's challenging to directly reason about the gradients over $X$ and $Y$ which require matrix multiplication, try reasoning about the gradients in terms of individual elements $x_i$ and $y_i$ first: in that case, you will need to come up with the derivations for $\frac{\partial L}{\partial x_i}$, by relying on the Chain Rule to first calculate the intermediate $\frac{\partial \mu}{\partial x_i}, \frac{\partial v}{\partial x_i}, \frac{\partial \sigma}{\partial x_i},$ then assemble these pieces to calculate $\frac{\partial y_i}{\partial x_i}$.

You should make sure each of the intermediary gradient derivations are all as simplified as possible, for ease of implementation.

After doing so, implement the simplified batch normalization backward pass in the function `batchnorm_backward_alt` and compare the two implementations by running the following. Your two implementations should compute nearly identical results, but the alternative implementation should be a bit faster.

```python
np.random.seed(231)
N, D = 100, 500
x = 5 * np.random.randn(N, D) + 12
gamma = np.random.randn(D)
beta = np.random.randn(D)
dout = np.random.randn(N, D)

bn_param = {'mode': 'train'}
out, cache = batchnorm_forward(x, gamma, beta, bn_param)

t1 = time.time()
dx1, dgamma1, dbeta1 = batchnorm_backward(dout, cache)
t2 = time.time()
dx2, dgamma2, dbeta2 = batchnorm_backward_alt(dout, cache)
t3 = time.time()

print('dx difference: ', rel_error(dx1, dx2))
print('dgamma difference: ', rel_error(dgamma1, dgamma2))
print('dbeta difference: ', rel_error(dbeta1, dbeta2))
print('speedup: %.2fx' % ((t2 - t1) / (t3 - t2)))
```

    dx difference:  1.0689044038190631e-12
    dgamma difference:  0.0
    dbeta difference:  0.0
    speedup: 1.14x

## Fully Connected Nets with Batch Normalization
Now that you have a working implementation for batch normalization, go back to your `FullyConnectedNet` in the file `cs231n/classifiers/fc_net.py`. Modify your implementation to add batch normalization.

Concretely, when the `normalization` flag is set to `"batchnorm"` in the constructor, you should insert a batch normalization layer before each ReLU nonlinearity. The outputs from the last layer of the network should not be normalized. Once you are done, run the following to gradient-check your implementation.

HINT: You might find it useful to define an additional helper layer similar to those in the file `cs231n/layer_utils.py`. If you decide to do so, do it in the file `cs231n/classifiers/fc_net.py`.

```python
np.random.seed(231)
N, D, H1, H2, C = 2, 15, 20, 30, 10
X = np.random.randn(N, D)
y = np.random.randint(C, size=(N,))

# You should expect losses between 1e-4~1e-10 for W,
# losses between 1e-08~1e-10 for b,
# and losses between 1e-08~1e-09 for beta and gammas.
for reg in [0, 3.14]:
    print('Running check with reg = ', reg)
    model = FullyConnectedNet([H1, H2], input_dim=D, num_classes=C,
                            reg=reg, weight_scale=5e-2, dtype=np.float64,
                            normalization='batchnorm')

    loss, grads = model.loss(X, y)
    print('Initial loss: ', loss)

    for name in sorted(grads):
        f = lambda _: model.loss(X, y)[0]
        grad_num = eval_numerical_gradient(f, model.params[name], verbose=False, h=1e-5)
        print('%s relative error: %.2e' % (name, rel_error(grad_num, grads[name])))
        if reg == 0: print()
```

    Running check with reg =  0
    Initial loss:  2.2611955101340957
    W1 relative error: 1.10e-04

    W2 relative error: 2.85e-06

    W3 relative error: 4.05e-10

    b1 relative error: 2.66e-07

    b2 relative error: 1.67e-08

    b3 relative error: 1.01e-10

    beta1 relative error: 7.33e-09

    beta2 relative error: 1.89e-09

    gamma1 relative error: 6.96e-09

    gamma2 relative error: 1.96e-09

    Running check with reg =  3.14
    Initial loss:  6.996533220108303
    W1 relative error: 1.98e-06
    W2 relative error: 2.29e-06
    W3 relative error: 2.79e-08
    b1 relative error: 1.94e-08
    b2 relative error: 8.22e-07
    b3 relative error: 2.10e-10
    beta1 relative error: 6.65e-09
    beta2 relative error: 4.23e-09
    gamma1 relative error: 6.27e-09
    gamma2 relative error: 5.28e-09

# Batchnorm for deep networks
Run the following to train a six-layer network on a subset of 1000 training examples both with and without batch normalization.

```python
np.random.seed(231)
# Try training a very deep net with batchnorm
hidden_dims = [100, 100, 100, 100, 100]

num_train = 1000
small_data = {
  'X_train': data['X_train'][:num_train],
  'y_train': data['y_train'][:num_train],
  'X_val': data['X_val'],
  'y_val': data['y_val'],
}

weight_scale = 2e-2
bn_model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization='batchnorm')
model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization=None)

print('Solver with batch norm:')
bn_solver = Solver(bn_model, small_data,
                num_epochs=10, batch_size=50,
                update_rule='adam',
                optim_config={
                  'learning_rate': 1e-3,
                },
                verbose=True,print_every=20)
bn_solver.train()

print('\nSolver without batch norm:')
solver = Solver(model, small_data,
                num_epochs=10, batch_size=50,
                update_rule='adam',
                optim_config={
                  'learning_rate': 1e-3,
                },
                verbose=True, print_every=20)
solver.train()
```

    Solver with batch norm:
    (Iteration 1 / 200) loss: 2.340975
    (Epoch 0 / 10) train acc: 0.111000; val_acc: 0.123000
    (Epoch 1 / 10) train acc: 0.335000; val_acc: 0.272000
    (Iteration 21 / 200) loss: 2.039365
    (Epoch 2 / 10) train acc: 0.426000; val_acc: 0.294000
    (Iteration 41 / 200) loss: 2.036710
    (Epoch 3 / 10) train acc: 0.493000; val_acc: 0.326000
    (Iteration 61 / 200) loss: 1.769536
    (Epoch 4 / 10) train acc: 0.544000; val_acc: 0.309000
    (Iteration 81 / 200) loss: 1.265761
    (Epoch 5 / 10) train acc: 0.576000; val_acc: 0.313000
    (Iteration 101 / 200) loss: 1.261353
    (Epoch 6 / 10) train acc: 0.633000; val_acc: 0.316000
    (Iteration 121 / 200) loss: 1.102834
    (Epoch 7 / 10) train acc: 0.692000; val_acc: 0.314000
    (Iteration 141 / 200) loss: 1.215466
    (Epoch 8 / 10) train acc: 0.697000; val_acc: 0.295000
    (Iteration 161 / 200) loss: 0.777501
    (Epoch 9 / 10) train acc: 0.756000; val_acc: 0.320000
    (Iteration 181 / 200) loss: 0.850938
    (Epoch 10 / 10) train acc: 0.776000; val_acc: 0.326000

    Solver without batch norm:
    (Iteration 1 / 200) loss: 2.302332
    (Epoch 0 / 10) train acc: 0.129000; val_acc: 0.131000
    (Epoch 1 / 10) train acc: 0.283000; val_acc: 0.250000
    (Iteration 21 / 200) loss: 2.041970
    (Epoch 2 / 10) train acc: 0.316000; val_acc: 0.277000
    (Iteration 41 / 200) loss: 1.900473
    (Epoch 3 / 10) train acc: 0.373000; val_acc: 0.282000
    (Iteration 61 / 200) loss: 1.713156
    (Epoch 4 / 10) train acc: 0.390000; val_acc: 0.310000
    (Iteration 81 / 200) loss: 1.662209
    (Epoch 5 / 10) train acc: 0.434000; val_acc: 0.300000
    (Iteration 101 / 200) loss: 1.696059
    (Epoch 6 / 10) train acc: 0.535000; val_acc: 0.345000
    (Iteration 121 / 200) loss: 1.557987
    (Epoch 7 / 10) train acc: 0.530000; val_acc: 0.304000
    (Iteration 141 / 200) loss: 1.432189
    (Epoch 8 / 10) train acc: 0.628000; val_acc: 0.339000
    (Iteration 161 / 200) loss: 1.033932
    (Epoch 9 / 10) train acc: 0.661000; val_acc: 0.340000
    (Iteration 181 / 200) loss: 0.901034
    (Epoch 10 / 10) train acc: 0.726000; val_acc: 0.318000

Run the following to visualize the results from two networks trained above. You should find that using batch normalization helps the network to converge much faster.

```python
def plot_training_history(title, label, baseline, bn_solvers, plot_fn, bl_marker='.', bn_marker='.', labels=None):
    """utility function for plotting training history"""
    plt.title(title)
    plt.xlabel(label)
    bn_plots = [plot_fn(bn_solver) for bn_solver in bn_solvers]
    bl_plot = plot_fn(baseline)
    num_bn = len(bn_plots)
    for i in range(num_bn):
        label='with_norm'
        if labels is not None:
            label += str(labels[i])
        plt.plot(bn_plots[i], bn_marker, label=label)
    label='baseline'
    if labels is not None:
        label += str(labels[0])
    plt.plot(bl_plot, bl_marker, label=label)
    plt.legend(loc='lower center', ncol=num_bn+1)

plt.subplot(3, 1, 1)
plot_training_history('Training loss','Iteration', solver, [bn_solver], \
                      lambda x: x.loss_history, bl_marker='o', bn_marker='o')
plt.subplot(3, 1, 2)
plot_training_history('Training accuracy','Epoch', solver, [bn_solver], \
                      lambda x: x.train_acc_history, bl_marker='-o', bn_marker='-o')
plt.subplot(3, 1, 3)
plot_training_history('Validation accuracy','Epoch', solver, [bn_solver], \
                      lambda x: x.val_acc_history, bl_marker='-o', bn_marker='-o')

plt.gcf().set_size_inches(15, 15)
plt.show()
```

# Batch normalization and initialization
We will now run a small experiment to study the interaction of batch normalization and weight initialization.

The first cell will train 8-layer networks both with and without batch normalization using different scales for weight initialization. The second layer will plot training accuracy, validation set accuracy, and training loss as a function of the weight initialization scale.

```python
np.random.seed(231)
# Try training a very deep net with batchnorm
hidden_dims = [50, 50, 50, 50, 50, 50, 50]
num_train = 1000
small_data = {
  'X_train': data['X_train'][:num_train],
  'y_train': data['y_train'][:num_train],
  'X_val': data['X_val'],
  'y_val': data['y_val'],
}

bn_solvers_ws = {}
solvers_ws = {}
weight_scales = np.logspace(-4, 0, num=20)
for i, weight_scale in enumerate(weight_scales):
    print('Running weight scale %d / %d' % (i + 1, len(weight_scales)))
    bn_model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization='batchnorm')
    model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization=None)

    bn_solver = Solver(bn_model, small_data,
                  num_epochs=10, batch_size=50,
                  update_rule='adam',
                  optim_config={
                    'learning_rate': 1e-3,
                  },
                  verbose=False, print_every=200)
    bn_solver.train()
    bn_solvers_ws[weight_scale] = bn_solver

    solver = Solver(model, small_data,
                  num_epochs=10, batch_size=50,
                  update_rule='adam',
                  optim_config={
                    'learning_rate': 1e-3,
                  },
                  verbose=False, print_every=200)
    solver.train()
    solvers_ws[weight_scale] = solver
```

    Running weight scale 1 / 20
    Running weight scale 2 / 20
    Running weight scale 3 / 20
    Running weight scale 4 / 20
    Running weight scale 5 / 20
    Running weight scale 6 / 20
    Running weight scale 7 / 20
    Running weight scale 8 / 20
    Running weight scale 9 / 20
    Running weight scale 10 / 20
    Running weight scale 11 / 20
    Running weight scale 12 / 20
    Running weight scale 13 / 20
    Running weight scale 14 / 20
    Running weight scale 15 / 20
    Running weight scale 16 / 20
    Running weight scale 17 / 20
    Running weight scale 18 / 20
    Running weight scale 19 / 20
    Running weight scale 20 / 20

```python
# Plot results of weight scale experiment
best_train_accs, bn_best_train_accs = [], []
best_val_accs, bn_best_val_accs = [], []
final_train_loss, bn_final_train_loss = [], []

for ws in weight_scales:
    best_train_accs.append(max(solvers_ws[ws].train_acc_history))
    bn_best_train_accs.append(max(bn_solvers_ws[ws].train_acc_history))

    best_val_accs.append(max(solvers_ws[ws].val_acc_history))
    bn_best_val_accs.append(max(bn_solvers_ws[ws].val_acc_history))

    final_train_loss.append(np.mean(solvers_ws[ws].loss_history[-100:]))
    bn_final_train_loss.append(np.mean(bn_solvers_ws[ws].loss_history[-100:]))

plt.subplot(3, 1, 1)
plt.title('Best val accuracy vs weight initialization scale')
plt.xlabel('Weight initialization scale')
plt.ylabel('Best val accuracy')
plt.semilogx(weight_scales, best_val_accs, '-o', label='baseline')
plt.semilogx(weight_scales, bn_best_val_accs, '-o', label='batchnorm')
plt.legend(ncol=2, loc='lower right')

plt.subplot(3, 1, 2)
plt.title('Best train accuracy vs weight initialization scale')
plt.xlabel('Weight initialization scale')
plt.ylabel('Best training accuracy')
plt.semilogx(weight_scales, best_train_accs, '-o', label='baseline')
plt.semilogx(weight_scales, bn_best_train_accs, '-o', label='batchnorm')
plt.legend()

plt.subplot(3, 1, 3)
plt.title('Final training loss vs weight initialization scale')
plt.xlabel('Weight initialization scale')
plt.ylabel('Final training loss')
plt.semilogx(weight_scales, final_train_loss, '-o', label='baseline')
plt.semilogx(weight_scales, bn_final_train_loss, '-o', label='batchnorm')
plt.legend()
plt.gca().set_ylim(1.0, 3.5)

plt.gcf().set_size_inches(15, 15)
plt.show()
```

## Inline Question 1:
Describe the results of this experiment. How does the scale of weight initialization affect models with/without batch normalization differently, and why?

## Answer:
With high weight scale loss is very high too without batchnorm. Batchnorm reduce this effect and also help with a low weight scale.

# Batch normalization and batch size
We will now run a small experiment to study the interaction of batch normalization and batch size.

The first cell will train 6-layer networks both with and without batch normalization using different batch sizes. The second layer will plot training accuracy and validation set accuracy over time.

```python
def run_batchsize_experiments(normalization_mode):
    np.random.seed(231)
    # Try training a very deep net with batchnorm
    hidden_dims = [100, 100, 100, 100, 100]
    num_train = 1000
    small_data = {
      'X_train': data['X_train'][:num_train],
      'y_train': data['y_train'][:num_train],
      'X_val': data['X_val'],
      'y_val': data['y_val'],
    }
    n_epochs=10
    weight_scale = 2e-2
    batch_sizes = [5,10,50]
    lr = 10**(-3.5)
    solver_bsize = batch_sizes[0]

    print('No normalization: batch size = ',solver_bsize)
    model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization=None)
    solver = Solver(model, small_data,
                    num_epochs=n_epochs, batch_size=solver_bsize,
                    update_rule='adam',
                    optim_config={
                      'learning_rate': lr,
                    },
                    verbose=False)
    solver.train()

    bn_solvers = []
    for i in range(len(batch_sizes)):
        b_size=batch_sizes[i]
        print('Normalization: batch size = ',b_size)
        bn_model = FullyConnectedNet(hidden_dims, weight_scale=weight_scale, normalization=normalization_mode)
        bn_solver = Solver(bn_model, small_data,
                        num_epochs=n_epochs, batch_size=b_size,
                        update_rule='adam',
                        optim_config={
                          'learning_rate': lr,
                        },
                        verbose=False)
        bn_solver.train()
        bn_solvers.append(bn_solver)

    return bn_solvers, solver, batch_sizes

batch_sizes = [5,10,50]
bn_solvers_bsize, solver_bsize, batch_sizes = run_batchsize_experiments('batchnorm')
```

    No normalization: batch size =  5
    Normalization: batch size =  5
    Normalization: batch size =  10
    Normalization: batch size =  50

```python
plt.subplot(2, 1, 1)
plot_training_history('Training accuracy (Batch Normalization)','Epoch', solver_bsize, bn_solvers_bsize, \
                      lambda x: x.train_acc_history, bl_marker='-^', bn_marker='-o', labels=batch_sizes)
plt.subplot(2, 1, 2)
plot_training_history('Validation accuracy (Batch Normalization)','Epoch', solver_bsize, bn_solvers_bsize, \
                      lambda x: x.val_acc_history, bl_marker='-^', bn_marker='-o', labels=batch_sizes)

plt.gcf().set_size_inches(15, 10)
plt.show()
```

## Inline Question 2:
Describe the results of this experiment. What does this imply about the relationship between batch normalization and batch size? Why is this relationship observed?

## Answer:
As we can read in the original paper, batchnorm works well with big batches. Maybe running mean and var calculated by the small batches are very noise and it's doesn't work well.

# Layer Normalization
Batch normalization has proved to be effective in making networks easier to train, but the dependency on batch size makes it less useful in complex networks which have a cap on the input batch size due to hardware limitations.

Several alternatives to batch normalization have been proposed to mitigate this problem; one such technique is Layer Normalization [2]. Instead of normalizing over the batch, we normalize over the features. In other words, when using Layer Normalization, each feature vector corresponding to a single datapoint is normalized based on the sum of all terms within that feature vector.

[2] [Ba, Jimmy Lei, Jamie Ryan Kiros, and Geoffrey E. Hinton. "Layer Normalization." stat 1050 (2016): 21.](https://arxiv.org/pdf/1607.06450.pdf)

## Inline Question 3:
Which of these data preprocessing steps is analogous to batch normalization, and which is analogous to layer normalization?

1. Scaling each image in the dataset, so that the RGB channels for each row of pixels within an image sums up to 1.
2. Scaling each image in the dataset, so that the RGB channels for all pixels within an image sums up to 1.
3. Subtracting the mean image of the dataset from each image in the dataset.
4. Setting all RGB values to either 0 or 1 depending on a given threshold.

## Answer:
[FILL THIS IN]

# Layer Normalization: Implementation

Now you'll implement layer normalization. This step should be relatively straightforward, as conceptually the implementation is almost identical to that of batch normalization. One significant difference though is that for layer normalization, we do not keep track of the moving moments, and the testing phase is identical to the training phase, where the mean and variance are directly calculated per datapoint.

Here's what you need to do:

* In `cs231n/layers.py`, implement the forward pass for layer normalization in the function `layernorm_backward`.

Run the cell below to check your results.
* In `cs231n/layers.py`, implement the backward pass for layer normalization in the function `layernorm_backward`.

Run the second cell below to check your results.
* Modify `cs231n/classifiers/fc_net.py` to add layer normalization to the `FullyConnectedNet`. When the `normalization` flag is set to `"layernorm"` in the constructor, you should insert a layer normalization layer before each ReLU nonlinearity.

Run the third cell below to run the batch size experiment on layer normalization.

```python
# Check the training-time forward pass by checking means and variances
# of features both before and after layer normalization

# Simulate the forward pass for a two-layer network
np.random.seed(231)
N, D1, D2, D3 =4, 50, 60, 3
X = np.random.randn(N, D1)
W1 = np.random.randn(D1, D2)
W2 = np.random.randn(D2, D3)
a = np.maximum(0, X.dot(W1)).dot(W2)

print('Before layer normalization:')
print_mean_std(a,axis=1)

gamma = np.ones(D3)
beta = np.zeros(D3)
# Means should be close to zero and stds close to one
print('After layer normalization (gamma=1, beta=0)')
a_norm, _ = layernorm_forward(a, gamma, beta, {'mode': 'train'})
print_mean_std(a_norm,axis=1)

gamma = np.asarray([3.0,3.0,3.0])
beta = np.asarray([5.0,5.0,5.0])
# Now means should be close to beta and stds close to gamma
print('After layer normalization (gamma=', gamma, ', beta=', beta, ')')
a_norm, _ = layernorm_forward(a, gamma, beta, {'mode': 'train'})
print_mean_std(a_norm,axis=1)
```

    Before layer normalization:
      means:  [-59.06673243 -47.60782686 -43.31137368 -26.40991744]
      stds:   [10.07429373 28.39478981 35.28360729  4.01831507]

    After layer normalization (gamma=1, beta=0)
      means:  [ 4.81096644e-16 -7.40148683e-17  2.22044605e-16 -5.92118946e-16]
      stds:   [0.99999995 0.99999999 1.         0.99999969]

    After layer normalization (gamma= [3. 3. 3.] , beta= [5. 5. 5.] )
      means:  [5. 5. 5. 5.]
      stds:   [2.99999985 2.99999998 2.99999999 2.99999907]

```python
# Gradient check batchnorm backward pass
np.random.seed(231)
N, D = 4, 5
x = 5 * np.random.randn(N, D) + 12
gamma = np.random.randn(D)
beta = np.random.randn(D)
dout = np.random.randn(N, D)

ln_param = {}
fx = lambda x: layernorm_forward(x, gamma, beta, ln_param)[0]
fg = lambda a: layernorm_forward(x, a, beta, ln_param)[0]
fb = lambda b: layernorm_forward(x, gamma, b, ln_param)[0]

dx_num = eval_numerical_gradient_array(fx, x, dout)
da_num = eval_numerical_gradient_array(fg, gamma.copy(), dout)
db_num = eval_numerical_gradient_array(fb, beta.copy(), dout)

_, cache = layernorm_forward(x, gamma, beta, ln_param)
dx, dgamma, dbeta = layernorm_backward(dout, cache)

#You should expect to see relative errors between 1e-12 and 1e-8
print('dx error: ', rel_error(dx_num, dx))
print('dgamma error: ', rel_error(da_num, dgamma))
print('dbeta error: ', rel_error(db_num, dbeta))
```

    dx error:  1.0
    dgamma error:  4.519489546032799e-12
    dbeta error:  2.276445013433725e-12

# Layer Normalization and batch size

We will now run the previous batch size experiment with layer normalization instead of batch normalization. Compared to the previous experiment, you should see a markedly smaller influence of batch size on the training history!

```python
ln_solvers_bsize, solver_bsize, batch_sizes = run_batchsize_experiments('layernorm')

plt.subplot(2, 1, 1)
plot_training_history('Training accuracy (Layer Normalization)','Epoch', solver_bsize, ln_solvers_bsize, \
                      lambda x: x.train_acc_history, bl_marker='-^', bn_marker='-o', labels=batch_sizes)
plt.subplot(2, 1, 2)
plot_training_history('Validation accuracy (Layer Normalization)','Epoch', solver_bsize, ln_solvers_bsize, \
                      lambda x: x.val_acc_history, bl_marker='-^', bn_marker='-o', labels=batch_sizes)

plt.gcf().set_size_inches(15, 10)
plt.show()
```

## Inline Question 4:
When is layer normalization likely to not work well, and why?

1. Using it in a very deep network
2. Having a very small dimension of features
3. Having a high regularization term

## Answer:
[FILL THIS IN]


# Práctica 2 - Cinemática directa y dinámica de manipuladores

En este documento se describe el proceso de obtención de la dinámica de un robot manipulador (pendulo doble), por medio de la ecuación de Euler-Lagrange, empecemos importando las librerias necesarias:

```python
from sympy import var, sin, cos, pi, Matrix, Function, Rational, simplify
from sympy.physics.mechanics import mechanics_printing
mechanics_printing()
```

Definimos de una vez todas las variables necesarias:

```python
var("l1:3")
```

```python
var("m1:3")
```

```python
var("J1:3")
```

```python
var("g t")
```

Y definimos las variables que dependen de otra variable, especificamente en este calculo, todo lo anterior es constante y solo $q_1$ es una variable dependiente del tiempo:

```python
q1 = Function("q1")(t)
q2 = Function("q2")(t)
```

Esta vez vamos a hacer algo ligeramente diferente, vamos a automatizar un poco el proceso de obtención de la posición generalizada del manipulador, para esto vamos a apoyarnos de nuestros conocimientos de cinemática directa, para lo cual, primero necesitamos definir una función ```DH``` la cual tome una lista de parametros, en un orden especifico, y nos devuelva la matriz de transformación homogénea asociada a este eslabón:

---
### Ejercicio

Define una función ```DH``` que tome los parametros Denavit-Hartenberg de un eslabón de un manipulador y devuelva la matriz de transformación homogénea asociada a este eslabón.

```python
def DH(params):
    from sympy import Matrix, sin, cos
    a, d, α, θ = params
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    return A
```

```python
from nose.tools import assert_equal
from sympy import var, Matrix, sin, cos, eye
var("l1")
q1 = Function("q1")(t)
A = Matrix([[cos(q1), -sin(q1), 0, l1*cos(q1)],
            [sin(q1), cos(q1), 0, l1*sin(q1)],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])
assert_equal(DH([l1, 0, 0, q1]), A)
assert_equal(DH([0, 0, 0, 0]), eye(4))
print("Sin errores")
```

Una vez que tenemos la función ```DH``` para calcular las matrices de transformación homogéneas, ahora procedemos a calcular la posición de cada articulación por medio de estas matrices:

```python
A1 = DH([l1, 0, 0, q1])
A2 = DH([l2, 0, 0, q2])
```

```python
A1
```

Recordemos que la posición de cada articulación se obtendrá por medio del ultimo vector de esta matriz, por lo que podemos:

```python
p1 = A1[0:3, 3:4]
p1
```

Para la posición de la segunda articulación necesitamos multiplicar las primeras dos matrices:

```python
p2 = (A1*A2)[0:3, 3:4]
p2
```

Aunque en este caso, podemos simplificar mas estas expresiones:

```python
p2 = simplify(p2)
p2
```

Teniendo estas posiciones, para obtener la velocidad, necesitamos obtener su derivada:

```python
v1 = p1.diff("t")
v1
```

```python
v2 = p2.diff("t")
v2
```

Una vez que tenemos la velocidad, obtener el cuadrado de esta velocidad es facil, para un vector podemos decir que:

$$
\left| v \right|^2 = v^T \cdot v
$$

```python
v1c = (v1.T*v1)[0]
v1c = v1c.simplify()
v1c
```

```python
v2c = (v2.T*v2)[0]
v2c = v2c.simplify()
v2c
```

Y calculando la altura y velocidad rotacional del eslabon:

```python
h1, h2 = p1[1], p2[1]
ω1, ω2 = q1.diff(t), q1.diff(t) + q2.diff(t)
```

---
### Ejercicio

Define una función ```ener_cin```, la cual tome los parametros ```m```, ```v```, ```J``` y ```ω``` y devuelva la energía cinética del eslabon.

```python
def ener_cin(params):
    from sympy import Rational
    m, v, J, ω = params
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    return K
```

```python
from nose.tools import assert_equal
from sympy import var, Matrix, sin, cos, Rational
var("m1 J1 l1 ω1")
q1 = Function("q1")(t)
v1 = Matrix([[l1*cos(q1)], [l1*sin(q1)], [0]])
assert_equal(ener_cin([m1, v1, J1, ω1]), Rational(1,2)*m1*l1**2 + Rational(1,2)*J1*ω1**2)
assert_equal(ener_cin([0, Matrix([[0],[0],[0]]), 0, 0]), 0)
print("Sin errores")
```

Si ahora calculamos las energías cinéticas, tenemos:

```python
h1, h2 = p1[1], p2[1]
ω1, ω2 = q1.diff(t), q1.diff(t) + q2.diff(t)
```

```python
K1 = ener_cin([m1, v1, J1, ω1])
K1
```

```python
K2 = ener_cin([m2, v2, J2, ω2])
K2
```

---
### Ejercicio

Defina una función ```ener_pot```, la cual tome los parametros ```m``` y ```h``` y devuelva la energía potencial del eslabon.

```python
def ener_pot(params):
    m, h = params
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    return U
```

```python
from nose.tools import assert_equal
from sympy import var
var("m1 m2 g h1 h2")
assert_equal(ener_pot([m1, h1]), m1*g*h1)
assert_equal(ener_pot([m2, h2]), m2*g*h2)
print("Sin errores")
```

Y calculando las energías potenciales:

```python
h1, h2 = p1[1], p2[1]
ω1, ω2 = q1.diff(t), q1.diff(t) + q2.diff(t)
```

```python
U1 = ener_pot([m1, h1])
U2 = ener_pot([m2, h2])
```

Una vez que tenemos las energías cinéticas y potenciales de cada eslabón, podemos calcular la energía cinética total y la energía potencial total del manipulador:

```python
K = K1 + K2
U = U1 + U2
```

Con estas energias se puede calcular el Lagrangiano:

```python
L = K - U
```

```python
L
```

Ya con el Lagrangiano, podemos calcular la ecuación de Euler-Lagrange para cada grado de libertad del manipulador:

```python
τ1 = (L.diff(q1.diff(t)).diff(t) - L.diff(q1)).expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
```

```python
τ1
```

```python
τ2 = (L.diff(q2.diff(t)).diff(t) - L.diff(q2)).expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
```

```python
τ2
```

Una vez que hemos concluido este proceso, podemos pasar al documento llamado ```numerico.ipynb```


# SMC samplers

This tutorial gives a basic introduction to SMC samplers, and explains how to run the SMC samplers already implemented in ``particles``. For a more advanced tutorial on how to design new SMC samplers, see the next tutorial. For more background on SMC samplers, check Chapter 17 of the book.

## SMC samplers: what for?

A SMC sampler is a SMC algorithm that samples from a sequence of probability distributions $\pi_t$, $t=0,\ldots,T$ (and compute their normalising constants). Sometimes one is genuinely interested in each $\pi_t$; more often one is interested only in the final distribution $\pi_T$. In the latter case, the sequence is purely instrumental.

Examples of SMC sequences are:

1. $\pi_t(\theta) = p(\theta|y_{0:t})$, the Bayesian posterior distribution of parameter $\theta$ given data $y_{0:t}$, for a certain model.

2. A tempering sequence, $\pi_t(\theta) \propto \nu(\theta) L(\theta)^{\gamma_t}$ ,where the $\gamma_t$'s form an increasing sequence of exponents: $0=\gamma_0 < \ldots < \gamma_T=1$. You can think of $\nu$ being the prior, $L$ the likelihood function, and $\pi_T$ the posterior. However, more generally, tempering is a way to interpolate between any two distributions, $\nu$ and $\pi$, with $\pi(\theta) \propto \nu(\theta) L(\theta)$.

We discuss first how to specify a sequence of the first type.

## Defining a Bayesian model

To define a particular Bayesian model, you must subclass `StaticModel`, and define method `logpyt`, which evaluates the log-likelihood of datapoint $Y_t$ given parameter $\theta$ and past datapoints $Y_{0:t-1}$. Here is a simple example:

```python
%matplotlib inline
from matplotlib import pyplot as plt
import seaborn as sb
import numpy as np
from scipy import stats

import particles
from particles import smc_samplers as ssp
from particles import distributions as dists

class ToyModel(ssp.StaticModel):
    def logpyt(self, theta, t):  # density of Y_t given theta and Y_{0:t-1}
        return stats.norm.logpdf(self.data[t], loc=theta['mu'],
                                 scale = theta['sigma'])
```

In words, we are considering a model where the observations are $Y_t\sim N(\mu, \sigma^2)$ (independently). The parameter is $\theta=(\mu, \sigma)$. Note the fields notation; more about this later.

Class `ToyModel` implicitely defines the  likelihood of the considered model for any sample size (since the likelihood at time $t$ is $p^\theta(y_{0:t})=\prod_{s=0}^t p^\theta(y_s|y_{0:s-1})$, and method `logpyt` defines each factor in this product; note that $y_s$ does not depend on the past values in our particular example). We now define the data and the prior:

```python
T = 30
my_data = stats.norm.rvs(loc=3.14, size=T)  # simulated data
my_prior = dists.StructDist({'mu': dists.Normal(scale=10.),
                             'sigma': dists.Gamma()})
```

For more details about to define prior distributions, see the documentation of module `distributions`, or the previous [tutorial on Bayesian estimation of state-space models](Bayes_estimation_ssm.ipynb). Now that we have everything, let's specify our static model:

```python
my_static_model = ToyModel(data=my_data, prior=my_prior)
```

This time, object `my_static_model` entirely defines the posterior.

```python
thetas = my_prior.rvs(size=5)
my_static_model.logpost(thetas, t=2)
# if t is omitted, gives the full posterior
```

    array([-8.38356144e+03, -1.70110905e+02, -1.84383513e+03, -1.34842726e+02,
           -2.00329818e+05])

The input of `logpost` and output of `myprior.rvs()` are [structured arrays](https://docs.scipy.org/doc/numpy/user/basics.rec.html), that is, arrays with fields:

```python
thetas['mu'][0]
```

    -10.584651715859902

Typically, you won't need to call `logpost` yourself, this will be done by the SMC sampler for you.

## IBIS

IBIS (iterated batch importance sampling) is the standard name for a SMC sampler that tracks a sequence of partial posterior distributions; i.e. $\pi_t$ is $p(\theta|y_{0:t})$, for $t=0,1,\ldots$.

Module `smc_samplers` defines `IBIS` as a subclass of `FeynmanKac`.

```python
my_ibis = ssp.IBIS(my_static_model, len_chain=50)
my_alg = particles.SMC(fk=my_ibis, N=20,
                       store_history=True, verbose=True)
my_alg.run()
```

    t=0, ESS=82.04
    t=1, Metropolis acc. rate (over 49 steps): 0.254, ESS=464.72
    t=2, Metropolis acc. rate (over 49 steps): 0.171, ESS=690.07
    t=3, ESS=438.62
    t=4, Metropolis acc. rate (over 49 steps): 0.211, ESS=849.38
    t=5, ESS=606.35
    t=6, ESS=412.14
    t=7, Metropolis acc. rate (over 49 steps): 0.289, ESS=732.81
    t=8, ESS=680.56
    t=9, ESS=411.45
    t=10, Metropolis acc. rate (over 49 steps): 0.288, ESS=282.34
    t=11, Metropolis acc. rate (over 49 steps): 0.317, ESS=850.79
    t=12, ESS=926.82
    t=13, ESS=936.24
    t=14, ESS=906.07
    t=15, ESS=650.82
    t=16, ESS=514.22
    t=17, ESS=426.00
    t=18, Metropolis acc. rate (over 49 steps): 0.325, ESS=878.73
    t=19, ESS=842.13
    t=20, ESS=826.67
    t=21, ESS=769.72
    t=22, ESS=865.55
    t=23, ESS=773.36
    t=24, ESS=690.59
    t=25, ESS=619.32
    t=26, ESS=645.08
    t=27, ESS=594.02
    t=28, ESS=510.84
    t=29, ESS=695.80

**Note**: we use option `verbose=True` in `SMC` in order to print some information on the intermediate distributions.

Since we set `store_history` to `True`, the particles and their weights have been saved at every time (in attribute `hist`, see previous tutorials on smoothing). Let's plot the posterior distributions of $\mu$ and $\sigma$ at various times.

```python
plt.style.use('ggplot')
for i, p in enumerate(['mu', 'sigma']):
    plt.subplot(1, 2, i + 1)
    for t in [1, 29]:
        plt.hist(my_alg.hist.X[t].theta[p], weights=my_alg.hist.wgts[t].W, label="t=%i" % t,
                 alpha=0.5, density=True)
    plt.xlabel(p)
plt.legend();
```

As expected, the posterior distribution concentrates progressively around the true values.

As always, once the algorithm is run, `my_smc.X` contains the final particles. However, object `my_smc.X` is no longer a simple numpy array. It is a `ThetaParticles` object, with attributes:

* `theta`: a structured array (an array with fields); i.e. `my_smc.X.theta['mu']` is a (N,) array that contains the the $\mu-$component of the $N$ particles;
* `lpost`: a 1D numpy array that contains the target (posterior) log-density of each of the particles;
* `shared`: a dictionary that contains "meta-data" on the particles; for instance `shared['acc_rates']` is a list of the acceptance rates of the successive Metropolis steps.

```python
print(["%2.f%%" % (100 * np.mean(r)) for r in my_alg.X.shared['acc_rates']])
plt.hist(my_alg.X.lpost, 30);
```

You do not need to know much more about class `ThetaParticles` in pratice (if you're curious, however, see the next tutorial on SMC samplers or the documention of module `smc_samplers`).

## Waste-free versus standard SMC samplers

The library now implements by default waste-free SMC ([Dau & Chopin, 2020](https://arxiv.org/abs/2011.02328)), a variant of SMC samplers that keeps all the intermediate Markov steps (rather than "wasting" them). In practice, this means that, in the piece of code above:

* at each time $t$, $N=20$ particles are resampled, and used as a starting points of the MCMC chains;

* the MCMC chains are run for 49 iterations, hence the chain length is 50 (parameter ``len_chain=50``)

* and since we keep all the intermediate steps, we get 50*20 = 1000 particles at each iteration. In particular, we do O(1000) operations at each step. (At time 0, we also generate 1000 particles.)

Thus, the number of particles is actually `N * len_chain`; given this number of particles, the performance typically does not depend too much on `N` and `len_chain`, provided the latter is "big enough" (relative to the mixing of the MCMC kernels).

See Dau & Chopin (2020) for more details on waste-free SMC. If you wish to run a standard SMC sampler instead, you may set `wastefree=False`, like this:

```python
my_ibis = ssp.IBIS(my_static_model, wastefree=False, len_chain=11)
my_alg = particles.SMC(fk=my_ibis, N=100, store_history=True)
my_alg.run()
```

This runs a standard SMC sampler which tracks $N=100$ particles; these particles are resampled from time to time, and then moved through 10 MCMC steps. (As explained in Dau & Chopin, 2020, you typically get a better performance vs CPU time trade-off with wastefree SMC.)

## Regarding the MCMC steps

The default MCMC kernel used to move the particles is a Gaussian random walk Metropolis kernel, whose covariance matrix is calibrated automatically to $\gamma$ times of the empirical covariance matrix of the particle sample, where $\gamma=2.38 / \sqrt{d}$ (standard choice in the literature).

It is possible to specify a different value for $\gamma$, or more generally other types of MCMC moves; for instance the following uses Metropolis kernels based on independent Gaussian proposals:

```python
mcmc = ssp.ArrayIndependentMetropolis(scale=1.1)
# Independent Gaussian proposal, with mean and variance determined by
# the particle sample (variance inflated by factor scale=1.1)
alt_move = ssp.MCMCSequenceWF(mcmc=mcmc)
# This object represents a particular way to apply several MCMC steps
# in a row. WF = WasteFree
alt_ibis = ssp.IBIS(my_static_model, move=alt_move)
alt_alg = particles.SMC(fk=alt_ibis, N=100,ESSrmin=0.2,
                        verbose=True)
alt_alg.run()
```

    t=0, ESS=56.14
    t=1, Metropolis acc. rate (over 9 steps): 0.377, ESS=447.47
    t=2, ESS=270.35
    t=3, ESS=117.34
    t=4, Metropolis acc. rate (over 9 steps): 0.491, ESS=848.91
    t=5, ESS=595.25
    t=6, ESS=391.41
    t=7, ESS=276.38
    t=8, ESS=199.79
    t=9, Metropolis acc. rate (over 9 steps): 0.660, ESS=765.88
    t=10, ESS=314.50
    t=11, ESS=278.97
    t=12, ESS=315.64
    t=13, ESS=270.67
    t=14, ESS=313.92
    t=15, ESS=179.27
    t=16, Metropolis acc. rate (over 9 steps): 0.759, ESS=937.78
    t=17, ESS=820.94
    t=18, ESS=951.60
    t=19, ESS=962.98
    t=20, ESS=938.37
    t=21, ESS=883.72
    t=22, ESS=843.66
    t=23, ESS=811.68
    t=24, ESS=736.35
    t=25, ESS=650.76
    t=26, ESS=597.71
    t=27, ESS=515.31
    t=28, ESS=456.33
    t=29, ESS=494.19

In the future, the package may also implement other type of MCMC kernels such as MALA. It is also possible to define your own MCMC kernels, as explained in the next tutorial.

For now, note the following practical detail: the algorithm resamples whenever the ESS gets below a certain threshold $\alpha * N$; the default value $\alpha=0.5$, but here we changed it (to $\alpha=0.2$) by setting `ESSrmin=0.2`.

```python
plt.plot(alt_alg.summaries.ESSs)
plt.xlabel('t')
plt.ylabel('ESS');
```

As expected, the algorithm waits until the ESS is below 200 to trigger a resample-move step.

## SMC tempering

SMC tempering is a SMC sampler that samples iteratively from the following sequence of distributions:

\begin{equation}
\pi_t(\theta) \propto \pi(\theta) L(\theta)^\gamma_t
\end{equation}

with $0=\gamma_0 < \ldots < \gamma_T = 1$. In words, this sequence is a **geometric bridge**, which interpolates between the prior and the posterior.

SMC tempering implemented in the same was as IBIS: as a sub-class of `FeynmanKac`, whose `__init__` function takes as argument a `StaticModel` object.

```python
fk_tempering = ssp.AdaptiveTempering(my_static_model)
my_temp_alg = particles.SMC(fk=fk_tempering, N=1000, ESSrmin=1.,
                            verbose=True)
my_temp_alg.run()
```

    t=0, ESS=5000.00, tempering exponent=0.000938
    t=1, Metropolis acc. rate (over 9 steps): 0.262, ESS=5000.00, tempering exponent=0.0121
    t=2, Metropolis acc. rate (over 9 steps): 0.241, ESS=5000.00, tempering exponent=0.0627
    t=3, Metropolis acc. rate (over 9 steps): 0.237, ESS=5000.00, tempering exponent=0.197
    t=4, Metropolis acc. rate (over 9 steps): 0.266, ESS=5000.00, tempering exponent=0.629
    t=5, Metropolis acc. rate (over 9 steps): 0.343, ESS=8583.28, tempering exponent=1

**Note**: Recall that `SMC` resamples every time the ESS drops below value N times option `ESSrmin`; here we set it to to 1, since we want to resample at every time. This makes sense: Adaptive SMC chooses adaptively the successive values of $\gamma_t$ so that the ESS equals a certain value ($N/2$ by default).

We have not saved the intermediate results this time (option `store_history` was not set) since they are not particularly interesting. Let's look at the final results:

```python
for i, p in enumerate(['mu', 'sigma']):
    plt.subplot(1, 2, i + 1)
    sb.histplot(my_temp_alg.X.theta[p], stat='density')
    plt.xlabel(p)
```

This looks reasonable!
You can see from the output that the algorithm automatically chooses the tempering exponents $\gamma_1, \gamma_2,\ldots$. In fact, at iteration $t$, the next value for $\gamma$ is set that the ESS drops at most to $N/2$. You can change this particular threshold by passing argument ESSrmin to TemperingSMC. (Warning: do not mistake this with the `ESSrmin` argument of class `SMC`):

```python
lazy_tempering = ssp.AdaptiveTempering(my_static_model, ESSrmin = 0.1)
lazy_alg = particles.SMC(fk=lazy_tempering, N=1000, verbose=True)
lazy_alg.run()
```

    t=0, ESS=1000.00, tempering exponent=0.0372
    t=1, Metropolis acc. rate (over 9 steps): 0.223, ESS=1000.00, tempering exponent=0.699
    t=2, Metropolis acc. rate (over 9 steps): 0.341, ESS=9125.17, tempering exponent=1

The algorithm progresses faster this time, but the ESS drops more between each step.
Another optional argument for Class `TemperingSMC` is `options_mh`, which works exactly as for `IBIS`, see above. That is, by default, the particles are moved according to a certain (adaptative) number of random walk steps, with a variance calibrated to the particle variance.

Again, all the indications above should be sufficient if you simply want to run a SMC sampler with random-walk Metropolis steps. If you are interested in more exotic SMC samplers (based on different MCMC kernels, or such that the state-space is not $\mathbb{R}^d$), see the next tutorial on SMC samplers.