# Draft of EKF implementation for temporal alignment

Using CTRA model.

Heavily based on [balzer82 implementation](https://github.com/balzer82/Kalman/blob/master/Extended-Kalman-Filter-CTRA.ipynb) and [giovannix adaptation](https://github.com/giovannix/kalman/blob/master/Extended-Kalman-Filter-CTRA.ipynb).

```python
from sympy import symbols, Matrix, init_printing
from IPython.display import display

init_printing()

vs, psis, dpsis, dts, xs, ys, lats, lons, axs = symbols('v \psi \dot\psi T x y lat lon a')

hs = Matrix([[xs],
             [ys],
             [psis],
             [vs],
             [dpsis],
             [axs]])

state = Matrix([xs,ys,psis,vs,dpsis,axs])

# Preallocation for Plotting
x0 = []
x1 = []
x2 = []
x3 = []
x4 = []
x5 = []
x6 = []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Pddx=[]
Pddy=[]
Pdv =[]
Kx = []
Ky = []
Kdx= []
Kdy= []
Kddx=[]
Kdv= []
dstate=[]
x_noisy_measurement = []
y_noisy_measurement = []
x_predicted = []
y_predicted = []
```

```python
def log_state_and_noises(iteration, state, predicted_state, noises, dt):
    updated_state_s, noise_s, sensors_std_s, = symbols('updated\_state noise sensors\_std')

    p_x, p_y, p_psi, p_v, p_dotpsi, p_a = symbols('\hat\ x \hat\ y \hat\ \psi \hat\ v \hat{\dot{\psi}} \hat\ a')
    predicted_state_matrix_s = Matrix([p_x, p_y, p_psi, p_v, p_dotpsi, p_a])
    predicted_state_s = symbols('predicted\_state')

    print(f'Iteration {iteration}, dt = {dt}:')
    print('='*100)

    print_precision = 5
    print("Predicted state", np.round(predicted_state.T, print_precision).tolist()[0])
    print("Updated state", np.round(x.reshape(-1), print_precision).tolist()[0])

    display(Eq(
        predicted_state_s,
        Eq(
            predicted_state_matrix_s,
            Matrix(predicted_state),
            evaluate=False
        )
    ))

    print('-'*100)
    display(Eq(
        updated_state_s,
        Eq(state, Matrix(x), evaluate=False)
    ))

    print('*Noise included')
    print('-'*100)

    display(Eq(
        Le(
            -sensors_std_s,
            Le(noise_s, sensors_std_s),
            evaluate=False
        ),
        Le(
            Matrix(-sensors_std),
            Le(Matrix(noises), Matrix(sensors_std), evaluate=False),
            evaluate=False
        )
    ))

    print('='*100)
```

## Implementing and Running the EKF

```python
import numpy as np
from numpy.random import default_rng
from sympy import Matrix, Eq, Le, Ge, symbols, init_printing
from copy import deepcopy
from IPython.display import display

init_printing()

# In this work HEADING is about where the vehicle is going and YAW is about the front of the vehicle
STATE_SIZE = 6 # States
STATE_X_IDX, STATE_Y_IDX, STATE_HEADING_IDX, STATE_VELOCITY_IDX, STATE_YAW_RATE_IDX, STATE_ACCELERATION_IDX = range(STATE_SIZE)

# Generated running the loop with correction commented, just with prediction
measurements_mock = np.array([
    [3.7538, 0.4616, 1.761, 6.6948, 0.4488, 4.0],
    [2.8449, 3.5203, 1.9512, 8.3896, 0.4488, 4.0],
    [1.0564, 6.6834, 2.2174, 8.8133, 0.6283, 1.0],
    [-1.627, 9.3919, 2.4836, 9.237, 0.6283, 1.0],
    [-5.0039, 11.3473, 2.7498, 9.237, 0.6283, 0.0],
    [-8.7762, 12.3455, 3.016, 9.237, 0.6283, 0.0],
    [-12.7749, 13.1978, 2.8496, 10.0844, -0.3927, 2.0],
    [-16.9109, 14.8321, 2.6832, 10.9318, -0.3927, 2.0],
    [-20.7895, 17.6414, 2.3504, 11.7792, -0.7854, 2.0],
    [-23.7432, 21.856, 2.0176, 12.6266, -0.7854, 2.0],
    [-26.3263, 27.2473, 2.0176, 15.5925, 0.0, 7.0],
    [-29.1809, 33.2053, 2.0176, 15.5925, 0.0, 0.0],
])

noises_mock = np.array([
    [0.7317, 0.8656, 0.0095, -0.5005, 0.0835, -0.4629],
    [-0.0571, -1.0837, -0.0015, 0.4288, -0.0634, -0.0808],
    [-0.5773, -1.1117, -0.0182, -0.778, 0.0696, -0.4022],
    [0.8567, 1.2855, 0.0112, -0.2399, 0.0904, -0.3941],
    [-0.8416, 0.1583, 0.0063, -0.6525, 0.084, -0.3697],
    [1.1896, -0.95, 0.0111, 0.4016, 0.0349, -0.1406],
    [0.5857, -0.9672, -0.0158, 0.3913, -0.0223, 0.2368],
    [-0.5729, -0.5062, 0.0078, -0.6339, -0.0688, 0.1991],
    [0.2298, 0.2873, -0.0159, 0.0898, -0.0375, -0.3652],
    [-0.2236, 0.5324, -0.0147, -0.1144, 0.0552, 0.3881],
    [0.6675, -0.5955, 0.018, 0.6145, -0.0493, -0.0704],
    [1.391, -1.0449, 0.0106, 0.9431, 0.0677, 0.3961],
])

def predict(x, P, dt):
    noise_pos     = 0.5*8.8*dt**2  # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
    noise_course  = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle
    noise_velocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
    noise_yaw_rate = 1.0*dt # assume 1.0rad/s2 as the maximum turn rate acceleration for the vehicle
    noise_accel   = 0.5 # assume 0.5m/s2

    # TODO: Can be improved and calibratred
    Q = np.diag([noise_pos**2, noise_pos**2, noise_course**2, noise_velocity**2, noise_yaw_rate**2, noise_accel**2])

    pos_x, pos_y, heading, velocity, yaw_rate, acceleration = x
    x = x.copy()
    P = P.copy()

    if np.abs(yaw_rate) < 0.00001: # Driving straight
        yaw_rate = 0.00001

    x[STATE_X_IDX] = pos_x + (1 / yaw_rate**2) * \
        ( \
            (velocity*yaw_rate + acceleration * yaw_rate * dt) * np.sin(heading + yaw_rate* dt) \
            + acceleration * np.cos(heading + yaw_rate * dt) \
            - velocity * yaw_rate * np.sin(heading) - acceleration * np.cos(heading) \
        )

    x[STATE_Y_IDX] = pos_y + (1 / yaw_rate**2) * \
        ( \
            (-velocity*yaw_rate - acceleration * yaw_rate * dt) * np.cos(heading + yaw_rate* dt) \
            + acceleration * np.sin(heading + yaw_rate * dt) \
            + velocity * yaw_rate * np.cos(heading) - acceleration * np.sin(heading) \
        )

    x[STATE_HEADING_IDX] = (heading + yaw_rate * dt + np.pi) % (2.0 * np.pi) - np.pi
    x[STATE_VELOCITY_IDX] = velocity + acceleration * dt
    x[STATE_YAW_RATE_IDX] = yaw_rate
    x[STATE_ACCELERATION_IDX] = acceleration

    pos_x, pos_y, heading, velocity, yaw_rate, acceleration = x

    # Calculate the Jacobian of the Dynamic Matrix A
    a13 = ( \
      (-yaw_rate*velocity*np.cos(heading) + acceleration*np.sin(heading) \
      - acceleration*np.sin(dt*yaw_rate + heading) + (dt*yaw_rate*acceleration + yaw_rate*velocity)*np.cos(dt*yaw_rate \
      + heading))/yaw_rate**2 \
    ).item(0)

    a14 = ( \
      (-yaw_rate*np.sin(heading) + yaw_rate*np.sin(dt*yaw_rate + heading))/yaw_rate**2 \
    ).item(0)

    a15 = ( \
      ( \
        -dt*acceleration*np.sin(dt*yaw_rate + heading) + dt*(dt*yaw_rate*acceleration + yaw_rate*velocity) \
        * np.cos(dt*yaw_rate + heading) - velocity*np.sin(heading) + (dt*acceleration + velocity) \
        * np.sin(dt*yaw_rate + heading) \
      ) / yaw_rate**2 \
      - 2*( \
        -yaw_rate*velocity*np.sin(heading) - acceleration \
        * np.cos(heading) + acceleration*np.cos(dt*yaw_rate + heading) + (dt*yaw_rate*acceleration + yaw_rate*velocity) \
        * np.sin(dt*yaw_rate + heading) \
      ) / yaw_rate**3 \
    ).item(0)

    a16 = ( \
      (dt*yaw_rate*np.sin(dt*yaw_rate + heading) - np.cos(heading) + np.cos(dt * yaw_rate + heading))/yaw_rate**2 \
    ).item(0)

    a23 = ( \
      ( \
        -yaw_rate * velocity * np.sin(heading) - acceleration * np.cos(heading) + acceleration * np.cos(dt * yaw_rate + heading) \
        - (-dt * yaw_rate*acceleration - yaw_rate * velocity) * np.sin(dt * yaw_rate + heading) \
      ) / yaw_rate**2 \
    ).item(0)

    a24 = ( \
      (yaw_rate * np.cos(heading) - yaw_rate*np.cos(dt*yaw_rate + heading))/yaw_rate**2 \
    ).item(0)

    a25 = ( \
      ( \
        dt * acceleration*np.cos(dt*yaw_rate + heading) - dt * (-dt*yaw_rate*acceleration - yaw_rate * velocity) \
        * np.sin(dt * yaw_rate + heading) + velocity*np.cos(heading) + (-dt*acceleration - velocity)*np.cos(dt*yaw_rate + heading) \
      ) / yaw_rate**2 \
      - 2*( \
        yaw_rate*velocity*np.cos(heading) - acceleration * np.sin(heading) + acceleration * np.sin(dt*yaw_rate + heading) \
        + (-dt * yaw_rate * acceleration - yaw_rate * velocity)*np.cos(dt*yaw_rate + heading) \
      ) / yaw_rate**3 \
    ).item(0)

    a26 = ( \
      (-dt*yaw_rate*np.cos(dt*yaw_rate + heading) - np.sin(heading) + np.sin(dt*yaw_rate + heading))/yaw_rate**2 \
    ).item(0)

    JA = np.matrix([[1.0, 0.0, a13, a14, a15, a16],
                    [0.0, 1.0, a23, a24, a25, a26],
                    [0.0, 0.0, 1.0, 0.0, dt, 0.0],
                    [0.0, 0.0, 0.0, 1.0, 0.0, dt],
                    [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
                    [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

    # Project the error covariance ahead
    P = JA*P*JA.T + Q

    return x, P

def update(measurement, x, P, R):
    hx = x.copy()  # Not making any transformation and considering all the atributes are being measured

    # Identity because is assumed that for each attribute in state there exists a respective measurement attribute
    JH = np.eye(hx.shape[0])

    S = JH*P*JH.T + R
    K = (P*JH.T) * np.linalg.inv(S)

    # Update the estimate via
    Z = measurement.reshape(JH.shape[0], 1)
    y = Z - hx  # Innovation or Residual
    x = x + K*y

    # Update the error covariance
    P = (np.eye(STATE_SIZE) - K*JH)*P

    # TODO: Remove in main C++ code
    x0.append(float(x[0]))
    x1.append(float(x[1]))
    x2.append(float(x[2]))
    x3.append(float(x[3]))
    x4.append(float(x[4]))
    x5.append(float(x[5]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Pddx.append(float(P[4,4]))
    Pdv.append(float(P[5,5]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))
    Kddx.append(float(K[4,0]))
    Kdv.append(float(K[5,0]))

    return x, P

if __name__ == '__main__':
    P = np.diag([10.0, 10.0, 10.0, 10.0, 10.0, 10.0])

    std_pos = 1.5 # standard deviation of positioning measurements
    std_speed = 1.0 # standard deviation of the speed measurements
    std_yaw_rate = 0.1 # standard deviation of the yaw_rate measurements
    std_heading = 0.02 # standard deviation of the heading measurements
    std_acc = 0.5 # standard deviation of the longitudinal acceleration

    R = np.diag([std_pos**2, std_pos**2, std_heading**2, std_speed**2, std_yaw_rate**2, std_acc**2])

    initial_state = np.array([[4, -2, np.pi/2, 5, np.pi/7, 4]], dtype='float64').T
    initial_noise = np.array([[-1.044, 1.0289, -0.0145, 0.9013, -0.0183, 0.0579]]).T

    x = initial_state + initial_noise

    random_generator = default_rng()

#     dts = [random_generator.random() for _ in measurements_mock]
#     dts = [np.round(dt, 4) for dt in dts]
#     print(dts)
    dts = (0.4571, 0.5364, 0.1067, 0.5539, 0.0843, 0.0452, 0.017, 0.1824, 0.5568, 0.9355, 0.2909, 0.1086, 0.4237)

    measurements = deepcopy(measurements_mock)

    all_noises, all_states, all_ps = [], [], []

    # for i, (measurement, dt) in enumerate(zip(measurements, dts), 1):
    for i, (measurement, noises, dt) in enumerate(zip(measurements, noises_mock, dts), 1):
        sensors_std = np.array([std_pos, std_pos, std_heading, std_speed, std_yaw_rate, std_acc])

        # Adding noise
#         noises = (random_generator.random() for _ in sensors_std)
#         noises = np.array([n * 2*std - std for std, n in zip(sensors_std, noises)])  # Putting noises in std range
        measurement += noises
        all_noises.append(noises)

        x_noisy_measurement.append(measurement[STATE_X_IDX])
        y_noisy_measurement.append(measurement[STATE_Y_IDX])

        x, P = predict(x, P, dt)  # <==== EKF
        predicted_state = deepcopy(x)

        x, P = update(measurement, x, P, R)  # <===== EKF

        log_state_and_noises(i, state, predicted_state, noises, dt)
        all_states.append(x)
        all_ps.append(P)

        x_predicted.append(predicted_state[STATE_X_IDX])
        y_predicted.append(predicted_state[STATE_Y_IDX])

```

    Iteration 1, dt = 0.4571:
    ====================================================================================================
    Predicted state [2.68103, 2.13316, 1.75308, 7.75617, 0.4305, 4.0579]
    Updated state [4.20009, 1.40653, 1.77032, 6.23164, 0.53316, 3.54363]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}2.68103049509595\\2.13315666391595\\1.75307739707431\\7.75616609\\0.430498950512828\\4.0579\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}4.20009035106605\\1.40652633329709\\1.77032421016975\\6.2316367584164\\0.533159342962015\\3.54363126874268\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}0.7317\\0.8656\\0.0095\\-0.5005\\0.0835\\-0.4629\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 2, dt = 0.5364:
    ====================================================================================================
    Predicted state [2.88776, 5.01469, 2.05631, 8.13244, 0.53316, 3.54363]
    Updated state [2.8853, 3.45836, 1.95632, 8.78508, 0.38746, 3.79022]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}2.88776078354664\\5.01468889656252\\2.05631088173458\\8.13244057096998\\0.533159342962015\\3.54363126874268\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}2.88529736741607\\3.45835868255254\\1.95631500627555\\8.78507759735717\\0.387455853851297\\3.79021783144163\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}-0.0571\\-1.0837\\-0.0015\\0.4288\\-0.0634\\-0.0808\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 3, dt = 0.1067:
    ====================================================================================================
    Predicted state [2.5063, 4.33915, 1.99766, 9.18949, 0.38746, 3.79022]
    Updated state [1.68037, 4.74538, 2.12017, 8.43064, 0.66433, 1.7948]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}2.50629803108141\\4.33915433346289\\1.99765654588149\\9.18949383997199\\0.387455853851297\\3.79021783144163\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}1.68037294828426\\4.74537925314337\\2.1201655632219\\8.4306370472555\\0.664325814240975\\1.79480462284443\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}-0.5773\\-1.1117\\-0.0182\\-0.778\\0.0696\\-0.4022\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 4, dt = 0.5539:
    ====================================================================================================
    Predicted state [-1.62354, 8.38727, 2.48814, 9.42478, 0.66433, 1.7948]
    Updated state [-1.14734, 9.62634, 2.49413, 9.00878, 0.71705, 1.05894]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-1.62353688087509\\8.38727129181504\\2.48813563172998\\9.42477932784903\\0.664325814240975\\1.79480462284443\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-1.14734348102342\\9.62633702114867\\2.49413218968645\\9.00877858758645\\0.717048044939883\\1.05894485813784\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}0.8567\\1.2855\\0.0112\\-0.2399\\0.0904\\-0.3941\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 5, dt = 0.0843:
    ====================================================================================================
    Predicted state [-1.76965, 10.06797, 2.55458, 9.09805, 0.71705, 1.05894]
    Updated state [-3.26234, 10.51576, 2.66535, 8.81877, 0.78394, 0.17681]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-1.76964784890326\\10.0679684728357\\2.55457933987488\\9.09804763912747\\0.717048044939883\\1.05894485813784\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-3.26233701513803\\10.5157554400528\\2.66535211032865\\8.81876986075193\\0.783935061276553\\0.17681219842872\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}-0.8416\\0.1583\\0.0063\\-0.6525\\0.084\\-0.3697\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 6, dt = 0.0452:
    ====================================================================================================
    Predicted state [-3.61992, 10.69226, 2.70079, 8.82676, 0.78394, 0.17681]
    Updated state [-4.72777, 10.8113, 2.82646, 9.20992, 0.84445, -0.01724]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-3.61991566546654\\10.6922591180914\\2.70078597509835\\8.8267617721209\\0.783935061276553\\0.17681219842872\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-4.72776731662939\\10.811298444987\\2.82646268257515\\9.20992374240458\\0.844453054208282\\-0.0172358380404339\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}1.1896\\-0.95\\0.0111\\0.4016\\0.0349\\-0.1406\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 7, dt = 0.017:
    ====================================================================================================
    Predicted state [-4.87697, 10.85875, 2.84082, 9.20963, 0.84445, -0.01724]
    Updated state [-6.42907, 11.16055, 2.82576, 9.6643, 0.44377, 1.3776]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-4.87696679210409\\10.8587544731098\\2.8408183844967\\9.20963073315789\\0.844453054208282\\-0.0172358380404339\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-6.42907113373913\\11.1605482814126\\2.82576224931975\\9.66430355481117\\0.443774473006801\\1.37759611869651\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}0.5857\\-0.9672\\-0.0158\\0.3913\\-0.0223\\0.2368\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 8, dt = 0.1824:
    ====================================================================================================
    Predicted state [-8.1471, 11.64563, 2.90671, 9.91558, 0.44377, 1.3776]
    Updated state [-9.85641, 12.22477, 2.7735, 10.27431, -0.29688, 1.88942]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-8.14710064986805\\11.6456311294589\\2.90670671319619\\9.91557708686142\\0.443774473006801\\1.37759611869651\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-9.85640559675293\\12.224769734493\\2.77349655601785\\10.274305957613\\-0.296875433104663\\1.88941567690844\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}-0.5729\\-0.5062\\0.0078\\-0.6339\\-0.0688\\0.1991\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 9, dt = 0.5568:
    ====================================================================================================
    Predicted state [-15.25972, 14.8488, 2.6082, 11.32633, -0.29688, 1.88942]
    Updated state [-17.91968, 16.66413, 2.35194, 11.86848, -0.81261, 1.74487]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-15.2597235262631\\14.8488027123312\\2.60819631486518\\11.3263326065156\\-0.296875433104663\\1.88941567690844\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-17.9196845918979\\16.664128563885\\2.3519388282027\\11.8684803056964\\-0.812613036542427\\1.74486910353753\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}0.2298\\0.2873\\-0.0159\\0.0898\\-0.0375\\-0.3652\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 10, dt = 0.9355:
    ====================================================================================================
    Predicted state [-22.35358, 27.36511, 1.59174, 13.50081, -0.81261, 1.74487]
    Updated state [-23.92608, 23.0596, 1.99358, 12.5246, -0.72884, 2.13437]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-22.3535808634516\\27.3651097018012\\1.59173933251726\\13.5008053520557\\-0.812613036542427\\1.74486910353753\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-23.9260763758206\\23.0595978853545\\1.99358247070683\\12.5246018001742\\-0.728843180283849\\2.13437418056173\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}-0.2236\\0.5324\\-0.0147\\-0.1144\\0.0552\\0.3881\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 11, dt = 0.2909:
    ====================================================================================================
    Predicted state [-25.08394, 26.60189, 1.78156, 13.14549, -0.72884, 2.13437]
    Updated state [-25.54408, 26.6981, 1.99634, 15.88754, -0.08726, 5.10322]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-25.0839391706721\\26.6018920100255\\1.78156198956225\\13.1454912492996\\-0.728843180283849\\2.13437418056173\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-25.5440777517895\\26.6980972138569\\1.99634087314007\\15.8875432169971\\-0.0872599147644831\\5.10322368687676\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}0.6675\\-0.5955\\0.018\\0.6145\\-0.0493\\-0.0704\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================
    Iteration 12, dt = 0.1086:
    ====================================================================================================
    Predicted state [-26.26114, 28.30044, 1.98686, 16.44175, -0.08726, 5.10322]
    Updated state [-26.79329, 29.57086, 2.01272, 16.50334, 0.03026, 2.19561]

$\displaystyle predicted\_{state} = \left[\begin{matrix}\hat x\\\hat y\\\hat \psi\\\hat v\\\hat{\dot{\psi}}\\\hat a\end{matrix}\right] = \left[\begin{matrix}-26.2611395785135\\28.3004430319744\\1.98686444639665\\16.4417533093919\\-0.0872599147644831\\5.10322368687676\end{matrix}\right]$

    ----------------------------------------------------------------------------------------------------

$\displaystyle updated\_{state} = \left[\begin{matrix}x\\y\\\psi\\v\\\dot\psi\\a\end{matrix}\right] = \left[\begin{matrix}-26.7932949002625\\29.5708638287191\\2.01272182980609\\16.5033434190006\\0.0302614610274864\\2.19560806171976\end{matrix}\right]$

    *Noise included
    ----------------------------------------------------------------------------------------------------

$\displaystyle - sensors\_{std} \leq noise \leq sensors\_{std} = \left[\begin{matrix}-1.5\\-1.5\\-0.02\\-1.0\\-0.1\\-0.5\end{matrix}\right] \leq \left[\begin{matrix}1.391\\-1.0449\\0.0106\\0.9431\\0.0677\\0.3961\end{matrix}\right] \leq \left[\begin{matrix}1.5\\1.5\\0.02\\1.0\\0.1\\0.5\end{matrix}\right]$

    ====================================================================================================

```python
# Saved info copy and paste friendly

print("Noises:")
for noises in all_noises:
    noises = np.round(noises, 4).tolist()
    print(str(noises) + ',')
print('-'*100)
print("Saved states:")
for state in all_states:
    state = np.round(state.T, 4).tolist()
    print(str(state[0]) + ',')
print('-'*100)
print("Saved Ps:")
for ps in all_ps:
    ps = np.round(ps.reshape(-1), 4).tolist()
    print(str(ps[0]) + ',')

print(np.round(all_states[0]).reshape(-1).tolist())
```

    Noises:
    [0.7317, 0.8656, 0.0095, -0.5005, 0.0835, -0.4629],
    [-0.0571, -1.0837, -0.0015, 0.4288, -0.0634, -0.0808],
    [-0.5773, -1.1117, -0.0182, -0.778, 0.0696, -0.4022],
    [0.8567, 1.2855, 0.0112, -0.2399, 0.0904, -0.3941],
    [-0.8416, 0.1583, 0.0063, -0.6525, 0.084, -0.3697],
    [1.1896, -0.95, 0.0111, 0.4016, 0.0349, -0.1406],
    [0.5857, -0.9672, -0.0158, 0.3913, -0.0223, 0.2368],
    [-0.5729, -0.5062, 0.0078, -0.6339, -0.0688, 0.1991],
    [0.2298, 0.2873, -0.0159, 0.0898, -0.0375, -0.3652],
    [-0.2236, 0.5324, -0.0147, -0.1144, 0.0552, 0.3881],
    [0.6675, -0.5955, 0.018, 0.6145, -0.0493, -0.0704],
    [1.391, -1.0449, 0.0106, 0.9431, 0.0677, 0.3961],
    ----------------------------------------------------------------------------------------------------
    Saved states:
    [4.2001, 1.4065, 1.7703, 6.2316, 0.5332, 3.5436],
    [2.8853, 3.4584, 1.9563, 8.7851, 0.3875, 3.7902],
    [1.6804, 4.7454, 2.1202, 8.4306, 0.6643, 1.7948],
    [-1.1473, 9.6263, 2.4941, 9.0088, 0.717, 1.0589],
    [-3.2623, 10.5158, 2.6654, 8.8188, 0.7839, 0.1768],
    [-4.7278, 10.8113, 2.8265, 9.2099, 0.8445, -0.0172],
    [-6.4291, 11.1605, 2.8258, 9.6643, 0.4438, 1.3776],
    [-9.8564, 12.2248, 2.7735, 10.2743, -0.2969, 1.8894],
    [-17.9197, 16.6641, 2.3519, 11.8685, -0.8126, 1.7449],
    [-23.9261, 23.0596, 1.9936, 12.5246, -0.7288, 2.1344],
    [-25.5441, 26.6981, 1.9963, 15.8875, -0.0873, 5.1032],
    [-26.7933, 29.5709, 2.0127, 16.5033, 0.0303, 2.1956],
    ----------------------------------------------------------------------------------------------------
    Saved Ps:
    [1.8716, -0.008, -0.0003, -0.0073, 0.0014, -0.0004, -0.008, 1.8964, -0.0001, 0.0252, 0.0004, 0.0009, -0.0003, -0.0001, 0.0004, 0.0, 0.0, -0.0, -0.0073, 0.0252, 0.0, 0.9614, 0.0, 0.0039, 0.0014, 0.0004, 0.0, 0.0, 0.01, 0.0, -0.0004, 0.0009, -0.0, 0.0039, 0.0, 0.2436],
    [1.3812, -0.0202, -0.0002, -0.0049, -0.0, -0.0027, -0.0202, 1.3987, -0.0001, 0.0069, -0.0, 0.0034, -0.0002, -0.0001, 0.0004, 0.0, 0.0, 0.0, -0.0049, 0.0069, 0.0, 0.9587, 0.0, 0.0018, -0.0, -0.0, 0.0, 0.0, 0.0097, 0.0, -0.0027, 0.0034, 0.0, 0.0018, 0.0, 0.1658],
    [0.8575, -0.0089, -0.0001, -0.0106, 0.0, -0.0007, -0.0089, 0.866, -0.0001, 0.0215, 0.0, 0.0009, -0.0001, -0.0001, 0.0002, 0.0, 0.0001, -0.0, -0.0106, 0.0215, 0.0, 0.6477, -0.0, 0.0026, 0.0, 0.0, 0.0001, -0.0, 0.0067, -0.0, -0.0007, 0.0009, -0.0, 0.0026, -0.0, 0.1561],
    [1.2566, -0.0186, -0.0001, -0.0058, -0.0, -0.0038, -0.0186, 1.2375, -0.0002, 0.0032, -0.0001, 0.0017, -0.0001, -0.0002, 0.0004, 0.0, 0.0, -0.0, -0.0058, 0.0032, 0.0, 0.9607, 0.0, 0.0013, -0.0, -0.0001, 0.0, 0.0, 0.0097, 0.0, -0.0038, 0.0017, -0.0, 0.0013, 0.0, 0.1547],
    [0.8081, -0.0085, -0.0001, -0.019, 0.0, -0.001, -0.0085, 0.7994, -0.0001, 0.0117, 0.0, 0.0004, -0.0001, -0.0001, 0.0002, 0.0, 0.0001, -0.0, -0.019, 0.0117, 0.0, 0.6016, -0.0, 0.0022, 0.0, 0.0, 0.0001, -0.0, 0.0062, 0.0, -0.001, 0.0004, -0.0, 0.0022, 0.0, 0.1545],
    [0.5954, -0.005, -0.0001, -0.0183, 0.0, -0.0003, -0.005, 0.5901, -0.0001, 0.0095, 0.0, 0.0001, -0.0001, -0.0001, 0.0002, 0.0, 0.0001, -0.0, -0.0183, 0.0095, 0.0, 0.4316, -0.0, 0.002, 0.0, 0.0, 0.0001, -0.0, 0.0044, 0.0, -0.0003, 0.0001, -0.0, 0.002, 0.0, 0.1545],
    [0.471, -0.0032, -0.0, -0.0138, 0.0, -0.0001, -0.0032, 0.4675, -0.0001, 0.0063, 0.0, 0.0, -0.0, -0.0001, 0.0001, 0.0, 0.0001, -0.0, -0.0138, 0.0063, 0.0, 0.3121, 0.0, 0.0012, 0.0, 0.0, 0.0001, 0.0, 0.0032, 0.0, -0.0001, 0.0, -0.0, 0.0012, 0.0, 0.1545],
    [0.4132, -0.0042, -0.0, -0.0147, -0.0, -0.0007, -0.0042, 0.4025, -0.0001, 0.0036, -0.0001, 0.0001, -0.0, -0.0001, 0.0002, 0.0, 0.0001, -0.0, -0.0147, 0.0036, 0.0, 0.743, 0.0, 0.0029, -0.0, -0.0001, 0.0001, 0.0, 0.0078, 0.0, -0.0007, 0.0001, -0.0, 0.0029, 0.0, 0.1545],
    [1.1713, -0.0248, -0.0002, -0.0066, -0.0001, -0.0036, -0.0248, 1.1506, -0.0003, 0.0046, -0.0001, 0.0026, -0.0002, -0.0003, 0.0004, -0.0, 0.0, 0.0, -0.0066, 0.0046, -0.0, 0.9612, -0.0, 0.0013, -0.0001, -0.0001, 0.0, -0.0, 0.0097, -0.0, -0.0036, 0.0026, 0.0, 0.0013, -0.0, 0.1544],
    [1.9769, 0.0021, -0.0001, 0.0005, -0.0, 0.0012, 0.0021, 1.9834, 0.0001, 0.0015, 0.0, 0.0027, -0.0001, 0.0001, 0.0004, -0.0, 0.0, -0.0, 0.0005, 0.0015, -0.0, 0.9857, -0.0, 0.0008, -0.0, 0.0, 0.0, -0.0, 0.0099, -0.0, 0.0012, 0.0027, -0.0, 0.0008, -0.0, 0.1544],
    [1.0921, -0.0013, -0.0003, -0.0017, -0.0001, 0.0002, -0.0013, 1.1113, -0.0, 0.0171, -0.0, 0.0015, -0.0003, -0.0, 0.0003, -0.0, 0.0, 0.0, -0.0017, 0.0171, -0.0, 0.8828, -0.0, 0.002, -0.0001, -0.0, 0.0, -0.0, 0.009, 0.0, 0.0002, 0.0015, 0.0, 0.002, 0.0, 0.1545],
    [0.7375, -0.0019, -0.0003, -0.0096, 0.0, 0.0, -0.0019, 0.7488, -0.0001, 0.0251, -0.0, 0.0005, -0.0003, -0.0001, 0.0002, -0.0, 0.0001, 0.0, -0.0096, 0.0251, -0.0, 0.6421, 0.0, 0.0026, 0.0, -0.0, 0.0001, 0.0, 0.0066, 0.0, 0.0, 0.0005, 0.0, 0.0026, 0.0, 0.1545],
    [[4.0, 1.0, 2.0, 6.0, 1.0, 4.0]]

## Generating values

```python
# Used to generate the measurements_mock array used previously
def generate_mock(initial_state, dts):
    x = list(np.round(initial_state.T[0], 4))
    P = np.diag([10.0, 10.0, 10.0, 10.0, 10.0, 10.0])

    print("# Copy and paste as content of 'mock' array")
    print(str(x) + ',')
    for i, _ in enumerate(dts, 1):
        if i == 3:
            x[STATE_ACCELERATION_IDX] -= 3
            x[STATE_YAW_RATE_IDX] = np.pi/5
        elif i == 5:
            x[STATE_ACCELERATION_IDX] -= 1
        elif i == 7:
            x[STATE_ACCELERATION_IDX] += 2
            x[STATE_YAW_RATE_IDX] = -np.pi/8
        elif i == 9:
            x[STATE_YAW_RATE_IDX] = -np.pi/4
        elif i == 11:
            x[STATE_ACCELERATION_IDX] += 5
            x[STATE_YAW_RATE_IDX] = 0
        elif i == 12:
            x[STATE_ACCELERATION_IDX] = 0
            x[STATE_YAW_RATE_IDX] = 0

        x, P = predict(x, P, dt)

        if i < len(dts):
            x = np.round(x, 4)
            print(str(list(x)) + ',')

generate_mock(initial_state, dts)
```

    # Copy and paste as content of 'mock' array
    [4.0, -2.0, 1.5708, 5.0, 0.4488, 4.0],
    [3.986, -1.4336, 1.6195, 5.4344, 0.4488, 4.0],
    [3.941, -0.8215, 1.6682, 5.8688, 0.4488, 4.0],
    [3.8566, -0.1839, 1.7364, 5.9774, 0.6283, 1.0],
    [3.7266, 0.458, 1.8046, 6.086, 0.6283, 1.0],
    [3.5517, 1.0952, 1.8728, 6.086, 0.6283, 0.0],
    [3.3337, 1.719, 1.941, 6.086, 0.6283, 0.0],
    [3.1038, 2.3512, 1.8984, 6.3032, -0.3927, 2.0],
    [2.8939, 3.0151, 1.8558, 6.5204, -0.3927, 2.0],
    [2.7213, 3.7138, 1.7705, 6.7376, -0.7854, 2.0],
    [2.6052, 4.4479, 1.6852, 6.9548, -0.7854, 2.0],
    [2.5143, 5.2393, 1.6852, 7.715, 0.0, 7.0],
    [2.4187, 6.0717, 1.6852, 7.715, 0.0, 0.0],

```python
import matplotlib.pyplot as plt

%matplotlib inline

fig = plt.figure(figsize=(10,10))
plt.gca().invert_xaxis()

# Copy and pasted from 'generate_mock' output
measurements = np.array([
    [4.0, -2.0, 1.5708, 5.0, 0.4488, 4.0],
    [3.986, -1.4336, 1.6195, 5.4344, 0.4488, 4.0],
    [3.941, -0.8215, 1.6682, 5.8688, 0.4488, 4.0],
    [3.8566, -0.1839, 1.7364, 5.9774, 0.6283, 1.0],
    [3.7266, 0.458, 1.8046, 6.086, 0.6283, 1.0],
    [3.5517, 1.0952, 1.8728, 6.086, 0.6283, 0.0],
    [3.3337, 1.719, 1.941, 6.086, 0.6283, 0.0],
    [3.1038, 2.3512, 1.8984, 6.3032, -0.3927, 2.0],
    [2.8939, 3.0151, 1.8558, 6.5204, -0.3927, 2.0],
    [2.7213, 3.7138, 1.7705, 6.7376, -0.7854, 2.0],
    [2.6052, 4.4479, 1.6852, 6.9548, -0.7854, 2.0],
    [2.5143, 5.2393, 1.6852, 7.715, 0.0, 7.0],
    [2.4187, 6.0717, 1.6852, 7.715, 0.0, 0.0],
])

x_gen = measurements[:, STATE_X_IDX]
y_gen = measurements[:, STATE_Y_IDX]

v_gen = measurements[:, STATE_VELOCITY_IDX]
a_gen = measurements[:, STATE_ACCELERATION_IDX]

heading_gen = measurements[:, STATE_HEADING_IDX]

plt.plot(y_gen[0], x_gen[0], 'o', label='START', c='w', ms=20, mew=2, mec='k')
plt.plot(y_gen, x_gen, 'o', ms=10)

vx_gen = np.cos(heading_gen) * v_gen
vy_gen = np.sin(heading_gen) * v_gen

ax_gen = np.cos(heading_gen) * a_gen
ay_gen = np.sin(heading_gen) * a_gen

plt.quiver(y_gen, x_gen, vy_gen, vx_gen, units='xy', angles='xy', color='b',
           width=0.03, headlength=2.5, headwidth=4, headaxislength=2, label='velocity_vector')

plt.quiver(y_gen, x_gen, ay_gen, ax_gen, units='xy', angles='xy', color='darkorange',
           width=0.03, headlength=2.5, headwidth=4, headaxislength=2, label='acceleration_vector')

plt.legend(labelspacing=1, borderpad=1)
plt.show()
```

```python
# Just some checkings about the prediction

P = np.diag([1, 1, 1, 1, 1, 1])  # Not relevant in this cell

# Maybe better a loop, but it's ok
print('Order: [x, y, yaw, v, yaw_rate, a]\n')

# [x, y, yaw, v, yaw_rate, a]
state, dt = np.array([0, 0, 0, 30, 0, 2]), 0
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, 0, 30, 0, 2]), 1
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, 0, 30, 0, 2]), 2
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, np.pi, 30, 0, 2]), 1
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, np.pi/2, 30, 0, 2]), 2
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, -np.pi/2, 30, 0, 2]), 2
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, 0, 0, np.pi/2, 0]), 1
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)

state, dt = np.array([0, 0, -np.pi/4, 20, 0, 0]), 1
predicted, _ = predict(state, P, dt)
print('Before: ', list(np.round(state, 4)), 'After: ', list(np.round(predicted, 4)), 'dt =', dt)
```

    Order: [x, y, yaw, v, yaw_rate, a]

    Before:  [0, 0, 0, 30, 0, 2] After:  [0, 0, 0, 30, 0, 2] dt = 0
    Before:  [0, 0, 0, 30, 0, 2] After:  [30, 0, 0, 32, 0, 2] dt = 1
    Before:  [0, 0, 0, 30, 0, 2] After:  [64, 0, 0, 34, 0, 2] dt = 2
    Before:  [0.0, 0.0, 3.1416, 30.0, 0.0, 2.0] After:  [-31.0, -0.0002, -3.1416, 32.0, 0.0, 2.0] dt = 1
    Before:  [0.0, 0.0, 1.5708, 30.0, 0.0, 2.0] After:  [-0.0007, 64.0, 1.5708, 34.0, 0.0, 2.0] dt = 2
    Before:  [0.0, 0.0, -1.5708, 30.0, 0.0, 2.0] After:  [0.0007, -64.0, -1.5708, 34.0, 0.0, 2.0] dt = 2
    Before:  [0.0, 0.0, 0.0, 0.0, 1.5708, 0.0] After:  [0.0, 0.0, 1.5708, 0.0, 1.5708, 0.0] dt = 1
    Before:  [0.0, 0.0, -0.7854, 20.0, 0.0, 0.0] After:  [14.1422, -14.1421, -0.7854, 20.0, 0.0, 0.0] dt = 1

    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:73: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:75: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:77: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:79: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:81: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:83: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:85: RuntimeWarning: invalid value encountered in double_scalars
    /home/icaroaragao/.local/lib/python3.6/site-packages/ipykernel_launcher.py:87: RuntimeWarning: invalid value encountered in double_scalars

## Demonstration on conversions to/from Object Model

```python
# Not using constants as indexes here is just about laziness

def get_headings(state):
    # [x, y, vx, vy, ax, ay, heading, yaw_rate]

    velocity_heading = np.arctan2(state[3], state[2])
    acceleration_heading = np.arctan2(state[5], state[4])

    return velocity_heading, acceleration_heading

def convert_to_object_model(state, yaw, velocity_heading, acceleration_heading):
    # Yaw is not important to the model. Only heading

    # From [x, y, yaw, v, yaw_rate, a] to [x, y, vx, vy, ax, ay, heading, yaw_rate]
    new_state = np.zeros(8)
    new_state[0], new_state[1], new_state[2], new_state[3] = state[0], state[1], np.cos(velocity_heading)*state[3], np.sin(velocity_heading)*state[3]
    new_state[4], new_state[5], new_state[6], new_state[7] = np.cos(acceleration_heading)*state[5], np.sin(acceleration_heading)*state[5], yaw, state[4]

    return np.round(new_state, 4)

def convert_from_object_model(state):
    # From [x, y, vx, vy, ax, ay, yaw, yaw_rate] to [x, y, heading, v, yaw_rate, a]
    new_state = np.zeros(6)
    heading = np.arctan2(state[2], state[3])

    new_state[0], new_state[1], new_state[2] = state[0], state[1], heading
    new_state[3], new_state[4], new_state[5] = np.hypot(state[2], state[3]), state[7], np.hypot(state[4], state[5])

    return np.round(new_state, 4)

init_state = np.array([4.0, -2.0, 0., 5., 0., 4., np.pi/2, np.pi/7], dtype='float64')
print("Initial state:", np.round(init_state, 4))

yaw = init_state[6]
velocity_heading, acceleration_heading = get_headings(init_state)
local_fmt = convert_from_object_model(init_state)
print("Local Format:", local_fmt)

object_model = convert_to_object_model(local_fmt, yaw, velocity_heading, acceleration_heading)
print("Object Model:", object_model)

assert len(object_model) == len(init_state)
assert np.allclose(object_model, init_state)
assert np.allclose(local_fmt, [4., -2., 0, 5., np.pi/7, 4.])  # Heading zero because is where velocity is pointing
```

    Initial state: [ 4.     -2.      0.      5.      0.      4.      1.5708  0.4488]
    Local Format: [ 4.     -2.      0.      5.      0.4488  4.    ]
    Object Model: [ 4.     -2.      0.      5.      0.      4.      1.5708  0.4488]

## Plots

```python
import matplotlib.pyplot as plt
%matplotlib inline

m = len(measurements_mock)
```

### Kalman Gain

```python
fig = plt.figure(figsize=(16,9))
plt.step(range(m),Kx, label='$x$')
plt.step(range(m),Ky, label='$y$')
plt.step(range(m),Kdx, label='$\psi$')
plt.step(range(m),Kdy, label='$v$')

plt.xlabel('Filter Step')
plt.ylabel('')
plt.title('Kalman Gain (the lower, the more the measurement fullfill the prediction)')
plt.legend(prop={'size':18})
plt.ylim([-0.2,0.2]);
```

### Uncertainties

```python

fig = plt.figure(figsize=(16,9))
plt.semilogy(range(m),Px, label='$x$')
plt.step(range(m),Py, label='$y$')
plt.step(range(m),Pdx, label='$\psi$')
plt.step(range(m),Pdy, label='$v$')
plt.step(range(m),Pddx, label='$\dot \psi$')

plt.xlabel('Filter Step')
plt.ylabel('')
plt.title('Uncertainty (Elements from Matrix $P$)')
plt.legend(loc='best',prop={'size':22})
```

### Position x/y

```python
fig = plt.figure(figsize=(20,20))
plt.gca().invert_xaxis()

x_measurement, y_measurement = measurements_mock[:, STATE_X_IDX], measurements_mock[:, STATE_Y_IDX]
x_ekf, y_ekf, x_noisy_measurement, y_noisy_measurement, x_predicted, y_predicted = np.array(
    [x0, x1, x_noisy_measurement, y_noisy_measurement, x_predicted, y_predicted],
    dtype='float64'
)

print('START =', f'({x_measurement[0]}, {y_measurement[0]})')
print('END =', f'({x_measurement[-1]}, {y_measurement[-1]})')

plt.plot(y_measurement[0], x_measurement[0], 'o', label='START', c='w', ms=20, mew=2, mec='k')
plt.plot(y_measurement[-1], x_measurement[-1], 'o', label='END', c='w', ms=20, mew=2, mec='g')

plt.plot(y_ekf, x_ekf, '-o', label='EKF', c='g', lw=2, ms=10)
plt.plot(y_predicted, x_predicted, 'o--', label='Predicted', c='r', lw=1, ms=10)

plt.plot(y_measurement, x_measurement, 'o:', label='Measurement - no noise', c='b', lw=1, ms=10)
plt.plot(y_noisy_measurement, x_noisy_measurement, 'o--', label='Measurement - with noise', c='y', lw=1, ms=10)

plt.xlabel("Y", fontsize=20)
plt.ylabel("X", fontsize=20)
plt.legend(loc="upper left", labelspacing=1.5, fontsize=10, borderpad=1)
plt.show()

print(y_measurement.shape[0], y_noisy_measurement.shape[0], y_ekf.shape[0], y_predicted.shape[0])
assert(y_measurement.shape[0] == y_noisy_measurement.shape[0] == y_ekf.shape[0] == y_predicted.shape[0])
```


# 第十讲：交叉验证、特征选择

回顾一下上一讲的内容，如果有一个有限的假设类$\lvert\mathcal{H}\rvert=k$，对于给定的$\gamma,\delta$，要保证$\varepsilon(\hat h)\leq\displaystyle\operatorname*{min}_{h\in\mathcal{H}}\varepsilon(h)+2\gamma$成立，并且在事件“训练误差与泛化误差之差小于$\gamma$”的概率至少为$1-\delta$时，需要满足：

$$
\begin{align}
m&\geq\frac{1}{2\gamma^2}\log\frac{2k}{\delta}\\
&=O\left(\frac{1}{\gamma^2}\log\frac{k}{\delta}\right)
\end{align}
$$

这是一个与样本复杂度相关的结论。现在我们想要将这一结论推广到无限假设类中。

## 4. 若$\mathcal{H}$是无限的

我们已经证明了一些在有限假设类下非常有用的理论，然而，包括“任意以实数为参数的假设函数”（如线性分类器）在内的很多的假设类实际上包含了无数个假设函数。我们能否将有限假设类下成立的结论推广到无限假设类中呢？

我们先抛出一个不严谨的论点，也许在某些方面不够“正确”，但它比较直观。之后，我们会给出一个更好且更普遍的结论。

假设我们要将上面关于样本复杂度的式子应用在逻辑回归中，此时我们的假设类是由线性判别边界构成的，所以$\mathcal{H}$是以$d$个实数作为参数（如果我用逻辑回归解决$n$个特征值的问题，则$d=n+1$，此时逻辑回归会找到一个以$n+1$个实数为参数的线性判别边界$g\left(\theta^Tx\right)$）。而我们都知道，计算机通常使用IEEE双精度浮点数（C语言中的`double`，$64$位）来近似实数，那么在这个逻辑回归的情况下，计算机需要$64d$个比特位（bits）来表示这些参数。那么对我们的假设类就有$k=\lvert\mathcal{H}\rvert=2^{64d}$个不同的假设函数。根据上一讲最后的结论，带入$O$表达式，为了保证样本复杂度中的条件$\varepsilon{\hat h}\leq\varepsilon{h^*}+2\gamma$的概率至少为$1-\delta$，就需要满足
$
m\geq O\left(\frac{1}{\gamma^2}\log\frac{2^{64d}}{\delta}\right)
=O\left(\frac{d}{\gamma^2}\log\frac{1}{\delta}\right)
=O_{\gamma,\delta}(d)
$
（最后一个$O$记号的下标$\gamma,\delta$表明复杂度可能依赖于这两个隐藏参数）。从这里可以大概了解，所需训练样本的数量与模型参数的个数大约最多呈线性关系。

虽然使用$64$位浮点数来推断这个结论并不能完全令人信服，但是这个结论大体上是正确的。如果要保证经验风险最小化生成的假设与最好假设间泛化误差的差距不超过$2\gamma$，则$m$必须大于大$O$记号中的量级，如果我们尝试最小化训练误差，为了保证学习效果我们使用了含有$d$个参数的假设类，则所需要的训练样本的数量大致与我们假设类的参数数目呈线性关系，即$m$与$d$大致呈线性关系。

（到目前为止，这些结论并不能说明一个使用经验风险最小化的算法的价值。因此，虽然多数尝试最小化或估计训练误差的判别学习算法对参数个数$d$为线性复杂度，但这个结论并不是对所有判别学习算法有效。对很多非经验风险最小化使用良好的假设保证仍是热门研究方向。）

上面推导出的结论还有一个不那么令人满意的地方——它依赖参数化$\mathcal{H}$。虽然直观上不容易看出问题，我们将具有$n+1$个参数$\theta_0,\cdots,\theta_n$线性分类器所在的类写作$1\left\{\theta_0^T+\theta_1^Tx_1+\cdots+\theta_n^Tx_n\geq0\right\}$，我们也可以将其写作$h_{u,v}(x)=1\left\{\left(u_0^2-v_0^2\right)+\left(u_1^2-v_1^2\right)x_1+\cdots+\left(u_n^2-v_n^2\right)x_n\geq0\right\}$，此时有$2n+2$个参数。不过，这两个式子表达的是同一个假设类$\mathcal{H}$，即$n$维线性分类器的集合。

为了给出一个更严谨的推导，我们先定义一些新概念。

对于某个由点$x^{(i)}\in\mathcal{X}$构成的集合$S=\left\{x^{(1)},\cdots,x^{(d)}\right\}$（并不要求是训练集），如果$\mathcal{H}$可以实现$S$任意一种标记方式，即对于任意集合$\left\{y^{(1)},\cdots,y^{(d)}\right\}$，存在$h\in\mathcal{H}$能够使$h\left(x^{(i)}\right)=y^{(i)},\ i=1,\cdots,d$成立，我们就称$\mathcal{H}$**分散了（shatters）**$S$。

对于给定假设类$\mathcal{H}$，我们定义**Vapnik-Chervonenkis维度（Vapnik-Chervonenkis dimension）**，写作$\mathrm{VC}(\mathcal{H})$，表示能够被$\mathcal{H}$分散的最大的集合。（如果$\mathcal{H}$可以分散任意大小的集合，则有$\mathrm{VC}(\mathcal{H})=\infty$）

举个例子，考虑下图中的点：

在两个维度上的（$h(x)=1\left\{\theta_0+\theta_1x_1+\theta_2x_2\geq0\right\}$）线性分类器的集合$\mathcal{H}$可以分散上面的点集吗？答案是肯定的。我们可以直接给出所有可能的八种分类标记的情况，而且对这个例子我们总是能够找到一个具有零训练误差的线性分类器（即完美分类）：

（对于上例的点集$S$，所有可能的分类情况都有对应的二维分类器将点分散）

此外，通过一系列计算能够证明，此假设类（即二维线性分类器）不可能分开任意含有$4$个点的集合（不论这四个点在$x_1,x_2$坐标系上如何排列，我们都无法找到一组能够将“四个点的所有可能的$x,o$的标记情况”分开的直线）。因此，$\mathcal{H}$可以分开的集合的元素总数为$3$，有$\mathrm{VC}(\mathcal{H})=3$。

值得注意的是，虽然$\mathcal{H}$的$\mathrm{VC}$维是$3$，但也存在分不开的布局。比如，如果三点共线（如下方左图所示），则存在二维线性分类器无法分开的标记情况（如下方右图所示）：

（因为已经存在某个$3$个点的集合可以被二维线性分类器分散，所以这并不影响二维线性分类器的$\mathrm{VC}$维为$3$。）

也就是说，从$\mathrm{VC}$维的定义有，为了证明$\mathrm{VC}(\mathcal{H})$至少是$d$，那么我们只需要找到一个大小为$d$的集合可以被$\mathcal{H}$散列即可。

**定理**，给定$\mathcal{H}$，令$d=\mathrm{VC}(\mathcal{H})$，在保证事件“训练误差与泛化误差在$\gamma$以内”的概率至少为$1-\delta$的条件下，对于$h\in\mathcal{H}$有：

$$
\left\lvert\varepsilon(h)-\hat\varepsilon(h)\right\rvert\leq O\left(\sqrt{\frac{d}{m}\log\frac{m}{d}+\frac{1}{m}\log\frac{1}{\delta}}\right)
$$

因此，在概率至少为$1-\delta$时可以得到：

$$
\varepsilon\left(\hat h\right)\leq\varepsilon\left(h^*\right)+O\left(\sqrt{\frac{d}{m}\log\frac{m}{d}+\frac{1}{m}\log\frac{1}{\delta}}\right)
$$

换句话说，如果一个假设类具有有限的$\mathrm{VC}$维度，则当$m$越来越大时会产生一致收敛。同上一讲一样，我们可以从中得到一个使用$\varepsilon\left(h^*\right)$表示的$\varepsilon(h)$的界限。我们还能够得到下面的推论：

**推论**，对于$h\in\mathcal{H}$（由于$\varepsilon\left(\hat h\right)\leq\varepsilon\left(h^*\right)+2\gamma$），要保证$\left\lvert\varepsilon(h)-\hat\varepsilon(h)\right\rvert\leq\gamma$的概率至少为$1-\delta$，则必须满足$m=O_{\gamma,\delta}(d)$。

也就是说，要保证从$\mathcal{H}$中选取的假设能够“学的好”，所需的训练样本的个数与$\mathcal{H}$的$\mathrm{VC}$维数呈线性关系。这个结论也可以解释为，对于“大多数”假设类（我们假设”合理的“参数化），其$\mathrm{VC}$维与其参数个数呈大致的线性关系（其实这两者的大小通常差不多，比如使用维度为$n$逻辑回归作为线性分类器时，一共需要$n+1$个参数，而$n$维线性分类器的假设类的$\mathrm{VC}$维是$n+1$）。综合上面的各结论有，（对于一个尝试最小化训练误差）算法所需的训练样本的数量通常与假设类$\mathcal{H}$参数的个数呈大致上的线性关系。这也表明了样本复杂度的上下界都是由$\mathrm{VC}$维给出的。

补充一点别的知识：回忆前几讲中的支持向量机算法，我们知道SVM会使用核函数将输入属性投影在无限维的特征空间中。那么，看起来SVM算法的$\mathrm{VC}$维数是无穷。不过，具有较大间隔的线性分类器通常$\mathrm{VC}$维都不会太大。我们可以做一个非正式的简要解释。给定了一个点集，如果我们只考虑使用具有较大间隔的线性分类器分隔这些点，那么我们的假设类将只包含这些间隔较大的判别边界。现在假设这个间隔就是$\gamma$，那么假设类中将不会有太靠近某个点的判别边界（假设类中的假设函数到各点的距离都不会小于$\gamma$）。如果点集中的点都分布在一个半径为$R$的圆内$\left\lVert x^{(i)}\right\rVert\leq R$，那么对于可以分隔这个点集的分类器$h$（间隔至少为$\gamma$）所在的假设类$\mathcal{H}$，有$\mathrm{VC}(\mathcal{H})\leq\left\lceil\frac{R^2}{4\gamma^2}\right\rceil+1$（$\lceil x\rceil$表示对$x$向上取整）。关于这个结论的证明我们就不展开介绍了。通过这个结论我们可以知道，$\mathrm{VC}$维数的取值范围与输入特征的维数是没有关系的。也就是说，即使输入$x$来自无限维空间，但只要我们令分类器必须满足一个较大的间隔时，$\mathrm{VC}$维实际上是被限制在一个比较小的数字上。所以，如果给定SVM一个需要满足的间隔，SVM将自动找到一个具有较小$\mathrm{VC}$维的假设类，算法并不会发生过拟合的现象。

关于经验风险最小化，如果我们只看其中的一个样本$x$，对于假设函数$h_\theta(x)=g\left(\theta^Tx\right)$，那么指示函数$1\left\{h_\theta(x)\neq y\right\}$将会型为一个阶梯函数，假设我们拿到的是一个负样本$y=0$，那么如果$\theta^Tx\gt0$，也就是误分类，此时阶梯函数值为$1$，如果$\theta^Tx\leq0$，那么就是分类正确，此时阶梯函数值为$0$。我们所要做的就是找到一组$\theta$，能够使这个阶梯函数最小化，也就是能够正确分类训练样本$x$（即令指示函数为$0$）。而这个指示函数是一个非凸函数，使用线性分类器最小化训练误差是一个NP困难问题（[中文](https://zh.wikipedia.org/wiki/NP%E5%9B%B0%E9%9A%BE)，[英文](https://en.wikipedia.org/wiki/NP-hardness)）。我们前面介绍的逻辑回归和支持向量机都是在使用凸近似逼近经验风险最小化这个NP困难问题。

# 第七部分：正则化及模型选择

有时，遇到一个问题，我们需要在几个不同的模型间取舍。比如，我们在使用多项回归模型$h_\theta(x)=g\left(\theta_0+\theta_1x+\theta_2x^2+\cdots+\theta_kx^k\right)$时，需要确定$k$到底取$1,2,\cdots10$中的那个值最为合适。那么，如何自动的让模型选择过程在偏差与方差间做出权衡？（原文使用”邪恶的双胞胎“来形容偏差与方差，可能使用”异卵双生“更合适）或者说，想要自动选择局部加权回归中的带宽参数$\tau$，或是希望自动确定$\mathscr{l}_1$正则化的SVM中的参数$C$（也就是SVM软边界中在间隔大小与惩罚力度之间做权衡的系数），我们应该如何去做呢？

为了更具体的描述问题，我们在本讲中均假设将要从有限的模型集合$\mathcal{M}=\left\{M_1,\cdots,M_d\right\}$中选取模型。比如说在第一个例子中，$M_i$就表示$i$阶多项回归模型。（推广到无限的$\mathcal{M}$也并不难。如果我们尝试从无限的模型集合中做出，使用带宽参数的例子$\tau\in\mathbb{R}^+$，我们就可以离散化$\tau$，然后令$M_i$为$\tau_i$的函数，再从中选择可能的$M_i$。一般的，本讲描述的算法可以看做是在模型空间中做优化选择，我们同样可以将这个选择过程应用于无限模型集合中。）再比如我们需要在SVM、神经网络和逻辑回归中做出选择，那么$\mathcal{M}$将包含这些模型。

## 1. 交叉验证（Cross validation）

像以前一样，假设有训练集$S$，从前面的经验风险最小化，我们可以得出一个关于模型选择的、乍看起来像个”算法“的步骤：

1. 用$S$训练每一个$M_i$，从而得到相应的假设函数$h_i$；
2. 选择训练误差最小的假设函数。

遗憾的是，这个”算法“并不能符合我们的预期。比如说用这个”算法“要从多项模型集合中选择阶数，我们知道对于多项回归模型，其阶数越高则对训练集的拟合越好，也就是训练误差越小。于是，该”算法“会始终选择阶数高、方差大的多项模型，而我们通常是不会使用这种模型的。

下面是一个比较好的算法，称为**保留交叉验证（hold-out cross validation）**（有时也称为**简单交叉验证（simple cross validation）**）：

1. 随机把训练集$S$分为两部分，$S_\mathrm{train}$（假设有训练集$70%$的样本）和$S_\mathrm{cv}$（剩下$30%$的样本），而这个$S_\mathrm{cv}$称为保留交叉验证集；
2. 仅使用$S_\mathrm{train}$训练每一个模型$M_i$，进而得到相应的假设函数$h_i$；
3. 使用保留交叉验证集测试$h_i$，选出测试结果$\hat\varepsilon_{S_\mathrm{cv}}(h_i)$最小的假设函数。（$\hat\varepsilon_{S_\mathrm{cv}}(h_i)$表示$h_i$在训练集$S_\mathrm{cv}$上的训练误差。）

通过使用模型没有训练过的样本集$S_\mathrm{cv}$的测试，我们对$h_i$的泛化误差就有了更好的估计，从而选择一个估计泛化误差最小的假设函数。我摸一般使用$\frac{1}{4}$到$\frac{1}{3}$的样本做保留交叉验证集合，而本例中的$30%$是常见的选择。

有时第三步也会选择按照$\mathrm{arg}\displaystyle\operatorname*{min}_i\hat\varepsilon_{S_\mathrm{cv}}(h_i)$直接输出相应的模型$M_i$，然后在使用整个训练集$M$重新训练$M_i$。（这通常是个好点子，不过一些对初始化条件或数据的扰动非常敏感的算法来说，并不能使用这种方法。对这些算法来说，模型在$S_\mathrm{train}$上表现的很好并不能得出它在$S_\mathrm{cv}$上也会表现的很好。所以，对这种算法最好不要使用这个重新训练的步骤。）

不过，保留交叉验证会“浪费”掉那$30%$的训练样本。即使我们使用改进的第三步，用整个训练集重新训练模型，但对于前面的模型选择过程，我们仍旧每次只使用了$0.7m$个训练样本，而不是包含$m$个样本的训练集。这种算法在训练样本有冗余或容易得到时，是没有什么问题的，但对于训练样本稀缺的学习问题（比如一共只有$m=20$个样本），我们就需要更好的算法了。

下面的算法称作**$k$重交叉验证（$k$-fold cross validation）**，该算法每次会保留更少的验证样本：

1. 随机把训练集$S$分为$k$个子集，每个子集有$\frac{m}{k}$个样本，记为$S_1,\cdots,S_k$；
2. 对每个模型$M_i$，我们执行下面的操作：

  `For` $j=1,\cdots,k$

    * 使用$S_1\cup\cdots\cup S_{j-1}\cup S_{j+1}\cup\cdots\cup S_k$（即使用除$S_j$以外的所以子集）训练$M_i$模型，从而得到假设$h_{ij}$；
    * 使用$S_j$测试$h_{ij}$，从而得到$\hat\varepsilon_{S_j}\left(h_{ij}\right)$。

  $M_i$的泛化误差估计为$\hat\varepsilon_{S_j}\left(h_{ij}\right)$（对于每个$j$）的平均值。

3. 选择具有最小泛化误差的$M_i$，然后使用整个训练集$S$重新训练模型，这次得到的假设函数就是最终的输出。

我们通常使用$k=10$重交叉验证。在这个算法中，虽然每次保留的数据仅为$\frac{1}{k}$（已经比原始的保留交叉验证小了很多），但整个$k$重计算过程的代价将比原始的保留交叉验证大很多，因为我们需要训练每个模型$k$次。

尽管$k=10$是我们的常用选择，但是在那些样本非常珍贵的学习问题中，我们可能会选择极端的算法——令$k=m$，从而使得每次保留的样本达到最小。在这个算法中，我们将使用$m-1$个样本训练模型，用剩下的$1$个保留样本作为测试，模型的泛化误差将通过求这$m=k$次训练的平均误差得到。由于该算法仅保留一个样本用做测试，它也被称作**留一交叉验证（leave-one-out cross validation）**。

上面我们介绍了模型选择时交叉验证算法的几个版本，这些算法还有更简单的用途——评估*单个*模型或算法的效果。举个例子，如果我们实现了一个学习算法，现在想要评估它在应用中的效果（或者我们发明了一个新的学习算法，并想要在论文中说明它在不同测试集上的表现时），那么交叉验证就给出了一个合理的方法。

## 2. 特征选择

特征选择是模型选择的一个非常重要的情形，我们把它拿出来单独介绍。假设有一个特征值$n$非常大（比如$n\gg m$）的监督学习问题，但是我们怀疑这些特征中仅有一小部分与学习问题有关。即使对$n$个特征使用非常简单的线性分类器（比如第三讲介绍的感知算法），假设类的$\mathrm{VC}$维数也会为$O(n)$的量级，除非有对于$n$来说大小相当的训练集，否则模型就会存在过拟合问题。

在这种情况下，我就可以使用特征选择算法减小特征值的数量。对于包含$n$个特征值的集合，有$2^n$种特征值子集（因为$n$个特征值的中的每一个都包含或不包含在子集中这两种状态）。于是，我们可以把特征选择看做一种在$2^n$个可能的模型中做出选择的模型选择问题。不过，对于过大的$n$，直接训练$2^n$个模型并进行比较的代价非常大，所以我们通常使用某种启发式搜索过程来寻找好的特征值子集。下面的搜索过程称为**向前搜索（forward search）**：

1. 初始化$\mathcal{F}=\emptyset$；
2. 重复`{`

  (a) 循环$i=1,\cdots,n$，如果$i\notin\mathcal{F}$，令$\mathcal{F}_i=\mathcal{F}\cup\{i\}$，然后使用某种交叉验证来估计特征$\mathcal{F}_i$。（即仅使用$\mathcal{F}_i$中的特征训练模型，然后估计模型的泛化误差。）

  (b) 令$\mathcal{F}$为步骤(a)中找到的最佳特征子集。

  `}`
3. 选择并输出上面的搜索过程得到的最佳的特征子集。

外层循环的终止条件可以设置为当$\mathcal{F}=\{1,2,\cdots,n\}$变为特征值全集时，也可以设置为当$\left\lvert\mathcal{F}\right\rvert$达到一个阈值时（也就是我们允许算法使用的特征值个数的上限）。

上面的算法称为**封装特征选择（wrapper model feature selection）**，因为算法中对模型有一个“封装”的过程，算法会重复调用模型检查评估不同特征子集的效果。除了向前搜索之外，我们还可以使用别的搜索过程，比如**向后搜索（backward search）**，该算法从全集$\mathcal{F}=\{1,\cdots,n\}$开始，然后每次删除一个特征值（同向前搜索评估每一个特征值的添加一样，向后搜索用相同的方法评估每一个特征值的删除），直到$\mathcal{F}=\emptyset$。

虽然封装特征选择算法通常都很奏效，但这个算法的计算量较大，因为它会不断的调用学习算法评估特征子集。完成整个向前搜索算法（算法在$\mathcal{F}=\{1,\cdots,n\}$时停止）需要调用学习算法大约$O\left(n^2\right)$次。（在计算资源能够满足要求的情况下可以使用这些算法，但是对于诸如文本分类的问题，特征值集合可以轻易达到几万个的数量级，如果再使用搜索算法就有些吃力了。）

（向前/向后搜索通常都不会找到最好的特征子集，实际上寻找最好的特征子集是一个NP困难问题。所以，相比较而言，这种搜索算法的效果通常都挺不错。）

下面介绍**过滤特征选择（Filter feature selection）**算法，该算法使用启发式且计算量更小的方法选择特征子集（不过，这种算法下相应的泛化误差可能会比搜索算法大）。算法的思路就是使用$S(i)$给特征$x_i$“打分”，看$x_i$能够为标记为$y$的类型提供多少信息，接下来就是简单的选择$k$个$S(i)$最高的特征值了。

“打分”函数$S(i)$的一种可能的定义：测量训练数据中$x_i$与$y$的相关度。该算法可能会使得我们选择的都是与标签$y$强相关的特征值。在实践中，我们通常选择能够表示$x_i$与$y$间的**互信息（mutual information）**$\mathrm{MI}\left(x_i,y\right)$的$S(i)$：

$$
\mathrm{MI}\left(x_i,y\right)=\sum_{x_i\in\{1,0\}}\sum_{y\in\{1,0\}}p\left(x_i,y\right)\log\frac{p\left(x_i,y\right)}{p\left(x_i\right)p(y)}
$$

（上面的式子假设$x_i$和$y$都是二元取值的变量，而更加一般化的式子应该是计算取值域内所有变量的情况之和。）关于$p\left(x_i,y\right)$、$p\left(x_i\right)$和$p(y)$的概率可以从训练集的经验分布中求得。

为了直观的解释这个“打分”函数的作用，应注意到互信息也可以表示成KL散度（Kullback-Leibler divergence，这是信息论中的概念，[中文](https://zh.wikipedia.org/wiki/%E7%9B%B8%E5%AF%B9%E7%86%B5)，[英文](https://en.wikipedia.org/wiki/Kullback%E2%80%93Leibler_divergence)）：

$$
\mathrm{MI}\left(x_i,y\right)=\mathrm{KL}\left(p\left(x_i,y\right) \parallel p\left(x_i\right)p(y)\right)
$$

在[问题集3](http://cs229.stanford.edu/materials/ps3.pdf)中有更多关于KL散度的信息，不过在这里，KL散度给出了衡量$p\left(x_i,y\right)$和$p\left(x_i\right)p(y)$间概率分布的差异程度的方法。如果$x_i$与$y$是独立随机变量，则有$p\left(x_i,y\right)=p\left(x_i\right)p(y)$，则这两个分布的KL散度为$0$。这与“$x_i$与$y$相互独立则$x_i$并不携带与$y$相关的信息”的观点一致。那么此时，$x_i$的得分$S(i)$就应该很小。相反，如果$x_i$携带的信息与$y$高度相关，那它们间的互信息$\mathrm{MI}\left(x_i,y\right)$将会非常大。

现在就有了一个关于特征值$S(i)$的排序，那么我们该选择前多少个特征值呢？其中一个“业界标准方法”就是使用交叉验证来判断究竟该选择多少个特征值。比如当我们使用朴素贝叶斯算法做文本分类时，会遇到词汇表条目数$n$过大的问题，而使用这个方法通常能够非常有效的提高分类器准确性。


\title{Combinational-Circuit Building Blocks aka medium scale integrated circuit (MSI) in myHDL}
\author{Steven K Armour}
\maketitle

# Table of Contents

1  Refs

2  Python Libraries Utilized

3  Multiplexers (mux)

3.1  Shannon’s Expansion Theorem

3.2  2:1 MultiPlexer

3.2.1  myHDL 2:1 MUX Gate Level and Testing

3.2.2  myHDL 2:1 MUX Gate Level HDL Synthesis

3.3  2:1 Multiplexer Behavioral

3.3.1  myHDL 2:1 MUX Behavioral Level and Testing

3.3.2  myHDL 2:1 MUX Behavioral Level HDL Synthesis

3.4  4:1 MUX

3.4.1  !? Insert Digram below

3.4.2  myHDL 4:1 MUX Gate Level and Testing

3.4.3  myHDL 4:1 MUX Gate Level HDL Synthesis

3.5  4:1 Multiplexer Behavioral

3.5.1  myHDL 4:1 MUX Behavioral Level and Testing

3.5.2  myHDL 4:1 MUX Behavioral Level HDL Synthesis

3.6  4:1 Multiplexer Behavioral with bitvectors

3.6.1  How bit vectors work in myHDL and in Verilog/VHDL

3.6.2  Understanding BitVector bit selection in myHDL

3.6.3  myHDL 4:1 MUX Behavioral with BitVecters and Testing

3.6.3.1  !? This needs to be checked

3.6.4  myHDL 4:1 MUX Behavioral with BitVecters HDL Synthesis

3.7  Generic Expressions via MUXs

3.7.1  myHDL Generic Expression via MUXs and Testing

3.7.2  myHDL Generic Expression via MUXs HDL Synthesis

4  Demultiplexers

5  Encoders

6  Decoders

# Refs
@book{brown_vranesic_2014,
place={New York, NY},
edition={3},
title={Fundamentals of digital logic with Verilog design},
publisher={McGraw-Hill},
author={Brown, Stephen and Vranesic, Zvonko G},
year={2014}
},

@book{lameres_2017,
title={Introduction to logic circuits & logic design with Verilog},
publisher={springer},
author={LaMeres, Brock J},
year={2017}
},

@misc{peeker_simple_mux,
url={http://www.xess.com/static/media/pages/peeker_simple_mux.html},
journal={Xess.com},
year={2017}
},

# Python Libraries Utilized

```python
import numpy as np
import pandas as pd
from sympy import *
init_printing()
from myhdl import *
from myhdlpeek import *
import random
from sympy_myhdl_tools import *
pass
```

# Multiplexers (mux)
a junction switch between one of n inputs to a single output; equivalent to a "if" or "case" statement

let $Z$ be its output $m_k$ the minterms of the controls to the mux and $I_k$ be the input feeds to the mux; then the expression for the mux in terms of boolean algebra becomes
$$Z=\sum^{2^k-1}_{k=0} m_k \cdot I_k= \text{OR}(m_k \& I_k) $$

## Shannon’s Expansion Theorem
The above is Shannon's theorem

it can be written more sincintly as:
$$f(x_1, x_2, ..., x_n)=\bar{x_1}f(0, x_2, ..., x_n)+x_1 f(x_1, x_2, ..., x_n)$$
and then each $f(0, x_2, ..., x_n)$ \& $f(x_1, x_2, ..., x_n)$ is broken down as the above till the maximum  number of control statement and minim inputs are needed

```python
def shannon_exspanson(f, term):
    """
    f is not a full equation
    """
    cof0=simplify(f.subs(term, 0)); cof1=simplify(f.subs(term, 1))
    return ((~term & cof0 | (term & cof1))), cof0, cof1
```

 ## 2:1 MultiPlexer

```python
sel, x_1in, x_2in=symbols('sel, x_1in, x_2in')
```

let $f(m_1, m_2, m_3)$ be the total set of minterms for a 3-bit then let $m_1$ be designated the select terms then by shannon's theorem states
$$f(m_1, m_2, m_3)=\bar{m_1} \cdot f_1'(0, m_2, m_3)+m_1 \cdot f_1(1, m_2, m_3)$$
in other words we want select the two subset of the f where $m_1$ is 1 or 0 and call thouse two subsets $f_1'$, $f_1$

```python
x_1in, x_2in, sel=symbols('x_1in, x_2in, sel')
```

$$f(m_1, m_2, m_3)$$

```python
ConversionTable=pd.DataFrame()
Terms=[bin(i, 3) for i in np.arange(0, 2**3)]
ConversionTable['sel']=[int(j[0]) for j in Terms]
ConversionTable['x_1in']=[int(j[1]) for j in Terms]
ConversionTable['x_2in']=[int(j[2]) for j in Terms]
#this is shannos theorm
ConversionTable['f']=list(ConversionTable.loc[ConversionTable['sel'] == 0]['x_1in'])+list(ConversionTable.loc[ConversionTable['sel'] == 1]['x_2in'])

ConversionTable.index.name='MinMaxTerm'
ConversionTable
```

      sel
      x_1in
      x_2in
      f

      MinMaxTerm

      0
      0
      0
      0
      0

      1
      0
      0
      1
      0

      2
      0
      1
      0
      1

      3
      0
      1
      1
      1

      4
      1
      0
      0
      0

      5
      1
      0
      1
      1

      6
      1
      1
      0
      0

      7
      1
      1
      1
      1

```python
POS=list(ConversionTable.loc[ConversionTable['f'] == 0].index)
SOP=list(ConversionTable.loc[ConversionTable['f'] == 1].index)
f"POS: {POS}, SOP:{SOP}"
```

    'POS: [0, 1, 4, 6], SOP:[2, 3, 5, 7]'

```python
f, _=POS_SOPformCalcater([sel, x_1in, x_2in], SOP, POS)
f
```

```python
a, b, c=shannon_exspanson(f, sel)
f,'= via shannaon', a
```

    ((sel & x_2in) | (x_1in & ~sel),
     '= via shannaon',
     (sel & x_2in) | (x_1in & ~sel))

$$\bar{m_1} \cdot f_1'(0, m_2, m_3)$$

```python
m1bar_f0=~sel&x_1in; m1bar_f0
```

```python
f0Table=ConversionTable.loc[ConversionTable['sel'] == 0].copy()
f0Table['f0']=[m1bar_f0.subs({sel:i, x_1in:j}) for i, j in zip(f0Table['sel'], f0Table['x_1in'])]
f0Table
```

      sel
      x_1in
      x_2in
      f
      f0

      MinMaxTerm

      0
      0
      0
      0
      0
      0

      1
      0
      0
      1
      0
      0

      2
      0
      1
      0
      1
      1

      3
      0
      1
      1
      1
      1

$$m_1 \cdot f_1(1, m_2, m_3)$$

```python
m1_f1=sel&x_2in; m1_f1
```

```python
f1Table=ConversionTable.loc[ConversionTable['sel'] == 1].copy()
f1Table['f1']=[m1_f1.subs({sel:i, x_2in:j}) for i, j in zip(f1Table['sel'], f1Table['x_2in'])]
f1Table
```

      sel
      x_1in
      x_2in
      f
      f1

      MinMaxTerm

      4
      1
      0
      0
      0
      0

      5
      1
      0
      1
      1
      1

      6
      1
      1
      0
      0
      0

      7
      1
      1
      1
      1
      1

and since this is the lowest order mux this case use of shannon's theorem is kind of trivial

### myHDL 2:1 MUX Gate Level and Testing

```python
def mux21_gates(sel, x_1in, x_2in,  f_out):
    @always_comb
    def logic():
        f_out.next=(sel and x_2in) or (x_1in and not sel)
    return logic
```

```python
Peeker.clear()

sel, x_1in, x_2in,  f_out=[Signal(bool(0)) for _ in range(4)]
Peeker(sel, 'sel'); Peeker(x_1in, 'x_1in'); Peeker(x_2in, 'x_2in')
Peeker(f_out, 'f_out')

DUT=mux21_gates(sel, x_1in, x_2in,  f_out)

inputs=[sel, x_1in, x_2in]
sim=Simulation(DUT, Combo_TB(inputs), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=0, stop_time=2*2**len(inputs), tock=True,
                  title='MUX 2:1 gate type simulation',
                  caption=f'after clock cycle {2**len(inputs)-1} ->random input')

```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson(start_time=0, stop_time=2**len(inputs) -1))
```

      f_out
      sel
      x_1in
      x_2in

      0
      0
      0
      0
      0

      1
      0
      0
      0
      1

      2
      1
      0
      1
      0

      3
      1
      0
      1
      1

      4
      0
      1
      0
      0

      5
      1
      1
      0
      1

      6
      0
      1
      1
      0

      7
      1
      1
      1
      1

### myHDL 2:1 MUX Gate Level HDL Synthesis

```python
sel, x_1in, x_2in,  f_out=[Signal(bool(0)) for _ in range(4)]

toVerilog(mux21_gates, sel, x_1in, x_2in,  f_out)
#toVHDL(mux21_gates sel, x_1in, x_2in,  f_out)
_=VerilogTextReader('mux21_gates')
```

    ***Verilog modual from mux21_gates.v***

     // File: mux21_gates.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:21:43 2017

    `timescale 1ns/10ps

    module mux21_gates (
        sel,
        x_1in,
        x_2in,
        f_out
    );

    input sel;
    input x_1in;
    input x_2in;
    output f_out;
    wire f_out;

    assign f_out = ((sel && x_2in) || (x_1in && (!sel)));

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL 2:1 MUX Gate level verilog code

however as will be shown doing gate implementation of MUXs is not sustainable in HDL code and this we will have to implement behavioral syntax as follows, thouse the caveat is that this only works for standard MUXs

## 2:1 Multiplexer Behavioral

### myHDL 2:1 MUX Behavioral Level and Testing

```python
def mux21_behavioral(sel, x_1in, x_2in,  f_out):
    @always_comb
    def logic():
            if sel:
                f_out.next=x_1in
            else:
                f_out.next=x_2in
    return logic
```

```python
Peeker.clear()

sel, x_1in, x_2in,  f_out=[Signal(bool(0)) for _ in range(4)]
Peeker(sel, 'sel'); Peeker(x_1in, 'x_1in'); Peeker(x_2in, 'x_2in')
Peeker(f_out, 'f_out')

DUT=mux21_behavioral(sel, x_1in, x_2in,  f_out)

inputs=[sel, x_1in, x_2in]
sim=Simulation(DUT, Combo_TB(inputs), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=0, stop_time=2*2**len(inputs), tock=True,
                  title='MUX 2:1 behaviroal type simulation',
                  caption=f'after clock cycle {2**len(inputs)-1} ->random input')

```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson(start_time=0, stop_time=2**len(inputs) -1))
```

      f_out
      sel
      x_1in
      x_2in

      0
      0
      0
      0
      0

      1
      1
      0
      0
      1

      2
      0
      0
      1
      0

      3
      1
      0
      1
      1

      4
      0
      1
      0
      0

      5
      0
      1
      0
      1

      6
      1
      1
      1
      0

      7
      1
      1
      1
      1

### myHDL 2:1 MUX Behavioral Level HDL Synthesis

```python
sel, x_1in, x_2in,  f_out=[Signal(bool(0)) for _ in range(4)]

toVerilog(mux21_behavioral, sel, x_1in, x_2in,  f_out)
#toVHDL(mux21_behavioral sel, x_1in, x_2in,  f_out)
_=VerilogTextReader('mux21_behavioral')
```

    ***Verilog modual from mux21_behavioral.v***

     // File: mux21_behavioral.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:21:46 2017

    `timescale 1ns/10ps

    module mux21_behavioral (
        sel,
        x_1in,
        x_2in,
        f_out
    );

    input sel;
    input x_1in;
    input x_2in;
    output f_out;
    reg f_out;

    always @(sel, x_1in, x_2in) begin: MUX21_BEHAVIORAL_LOGIC
        if (sel) begin
            f_out = x_1in;
        end
        else begin
            f_out = x_2in;
        end
    end

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL behavioral level 2:1 MUX's verilog code

## 4:1 MUX

If you try to repeat the above using a 4:1 which has four input lines and needs two select lines you can become overwhelmed quickly instead it is easier to use the following diagram to than synthesis the gate level architecture

### !? Insert Digram below

### myHDL 4:1 MUX Gate Level and Testing

```python
def MUX41_gates(sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out):
    @always_comb
    def logic():
        f_out.next=((not sel_1) and (not sel_2) and x_1in) or ((not sel_1) and ( sel_2) and x_2in) or (( sel_1) and (not sel_2) and x_3in) or (( sel_1) and ( sel_2) and x_4in)
    return logic
```

```python
Peeker.clear()

sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out=[Signal(bool(0)) for _ in range(7)]
Peeker(sel_1, 'sel_1'); Peeker(sel_2, 'sel_2');
Peeker(x_1in, 'x_1in'); Peeker(x_2in, 'x_2in'); Peeker(x_3in, 'x_3in'); Peeker(x_4in, 'x_4in')
Peeker(f_out, 'f_out')

DUT=MUX41_gates(sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)

inputs=[sel_1, sel_2, x_1in, x_2in, x_3in, x_4in]
sim=Simulation(DUT, Combo_TB(inputs), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=0, stop_time=2*2**len(inputs), tock=True,
                  title='MUX 4:1 gate type simulation',
                  caption=f'after clock cycle {2**len(inputs)-1} ->random input')

```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson(start_time=0, stop_time=2**len(inputs) -1))
```

      f_out
      sel_1
      sel_2
      x_1in
      x_2in
      x_3in
      x_4in

      0
      0
      0
      0
      0
      0
      0
      0

      1
      0
      0
      0
      0
      0
      0
      1

      2
      0
      0
      0
      0
      0
      1
      0

      3
      0
      0
      0
      0
      0
      1
      1

      4
      0
      0
      0
      0
      1
      0
      0

      5
      0
      0
      0
      0
      1
      0
      1

      6
      0
      0
      0
      0
      1
      1
      0

      7
      0
      0
      0
      0
      1
      1
      1

      8
      1
      0
      0
      1
      0
      0
      0

      9
      1
      0
      0
      1
      0
      0
      1

      10
      1
      0
      0
      1
      0
      1
      0

      11
      1
      0
      0
      1
      0
      1
      1

      12
      1
      0
      0
      1
      1
      0
      0

      13
      1
      0
      0
      1
      1
      0
      1

      14
      1
      0
      0
      1
      1
      1
      0

      15
      1
      0
      0
      1
      1
      1
      1

      16
      0
      0
      1
      0
      0
      0
      0

      17
      0
      0
      1
      0
      0
      0
      1

      18
      0
      0
      1
      0
      0
      1
      0

      19
      0
      0
      1
      0
      0
      1
      1

      20
      1
      0
      1
      0
      1
      0
      0

      21
      1
      0
      1
      0
      1
      0
      1

      22
      1
      0
      1
      0
      1
      1
      0

      23
      1
      0
      1
      0
      1
      1
      1

      24
      0
      0
      1
      1
      0
      0
      0

      25
      0
      0
      1
      1
      0
      0
      1

      26
      0
      0
      1
      1
      0
      1
      0

      27
      0
      0
      1
      1
      0
      1
      1

      28
      1
      0
      1
      1
      1
      0
      0

      29
      1
      0
      1
      1
      1
      0
      1

      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...

      34
      1
      1
      0
      0
      0
      1
      0

      35
      1
      1
      0
      0
      0
      1
      1

      36
      0
      1
      0
      0
      1
      0
      0

      37
      0
      1
      0
      0
      1
      0
      1

      38
      1
      1
      0
      0
      1
      1
      0

      39
      1
      1
      0
      0
      1
      1
      1

      40
      0
      1
      0
      1
      0
      0
      0

      41
      0
      1
      0
      1
      0
      0
      1

      42
      1
      1
      0
      1
      0
      1
      0

      43
      1
      1
      0
      1
      0
      1
      1

      44
      0
      1
      0
      1
      1
      0
      0

      45
      0
      1
      0
      1
      1
      0
      1

      46
      1
      1
      0
      1
      1
      1
      0

      47
      1
      1
      0
      1
      1
      1
      1

      48
      0
      1
      1
      0
      0
      0
      0

      49
      1
      1
      1
      0
      0
      0
      1

      50
      0
      1
      1
      0
      0
      1
      0

      51
      1
      1
      1
      0
      0
      1
      1

      52
      0
      1
      1
      0
      1
      0
      0

      53
      1
      1
      1
      0
      1
      0
      1

      54
      0
      1
      1
      0
      1
      1
      0

      55
      1
      1
      1
      0
      1
      1
      1

      56
      0
      1
      1
      1
      0
      0
      0

      57
      1
      1
      1
      1
      0
      0
      1

      58
      0
      1
      1
      1
      0
      1
      0

      59
      1
      1
      1
      1
      0
      1
      1

      60
      0
      1
      1
      1
      1
      0
      0

      61
      1
      1
      1
      1
      1
      0
      1

      62
      0
      1
      1
      1
      1
      1
      0

      63
      1
      1
      1
      1
      1
      1
      1

64 rows × 7 columns

### myHDL 4:1 MUX Gate Level HDL Synthesis

```python
sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out=[Signal(bool(0)) for _ in range(7)]

toVerilog(MUX41_gates, sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)
#toVHDL(MUX41_gates, sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)
_=VerilogTextReader('MUX41_gates')
```

    ***Verilog modual from MUX41_gates.v***

     // File: MUX41_gates.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:21:50 2017

    `timescale 1ns/10ps

    module MUX41_gates (
        sel_1,
        sel_2,
        x_1in,
        x_2in,
        x_3in,
        x_4in,
        f_out
    );

    input sel_1;
    input sel_2;
    input x_1in;
    input x_2in;
    input x_3in;
    input x_4in;
    output f_out;
    wire f_out;

    assign f_out = (((!sel_1) && (!sel_2) && x_1in) || ((!sel_1) && sel_2 && x_2in) || (sel_1 && (!sel_2) && x_3in) || (sel_1 && sel_2 && x_4in));

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL 4:1 MUX Gate level verilog code

## 4:1 Multiplexer Behavioral

As one can clearly see this is not sustainable and thus 'if' Statements need to be used via behavioral logic modeling

### myHDL 4:1 MUX Behavioral Level and Testing

```python
def MUX41_behavioral(sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out):
    @always_comb
    def logic():
        if (not sel_1) and (not sel_2):
            f_out.next=x_1in

        elif (not sel_1) and sel_2:
            f_out.next=x_2in

        elif sel_1 and (not sel_2):
            f_out.next=x_3in

        else:
            f_out.next=x_4in

    return logic
```

```python
Peeker.clear()

sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out=[Signal(bool(0)) for _ in range(7)]
Peeker(sel_1, 'sel_1'); Peeker(sel_2, 'sel_2');
Peeker(x_1in, 'x_1in'); Peeker(x_2in, 'x_2in'); Peeker(x_3in, 'x_3in'); Peeker(x_4in, 'x_4in')
Peeker(f_out, 'f_out')

DUT=MUX41_behavioral(sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)

inputs=[sel_1, sel_2, x_1in, x_2in, x_3in, x_4in]
sim=Simulation(DUT, Combo_TB(inputs), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=0, stop_time=2*2**len(inputs), tock=True,
                  title='MUX 4:1 behavioral type simulation',
                  caption=f'after clock cycle {2**len(inputs)-1} ->random input')

```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson(start_time=0, stop_time=2**len(inputs) -1))
```

      f_out
      sel_1
      sel_2
      x_1in
      x_2in
      x_3in
      x_4in

      0
      0
      0
      0
      0
      0
      0
      0

      1
      0
      0
      0
      0
      0
      0
      1

      2
      0
      0
      0
      0
      0
      1
      0

      3
      0
      0
      0
      0
      0
      1
      1

      4
      0
      0
      0
      0
      1
      0
      0

      5
      0
      0
      0
      0
      1
      0
      1

      6
      0
      0
      0
      0
      1
      1
      0

      7
      0
      0
      0
      0
      1
      1
      1

      8
      1
      0
      0
      1
      0
      0
      0

      9
      1
      0
      0
      1
      0
      0
      1

      10
      1
      0
      0
      1
      0
      1
      0

      11
      1
      0
      0
      1
      0
      1
      1

      12
      1
      0
      0
      1
      1
      0
      0

      13
      1
      0
      0
      1
      1
      0
      1

      14
      1
      0
      0
      1
      1
      1
      0

      15
      1
      0
      0
      1
      1
      1
      1

      16
      0
      0
      1
      0
      0
      0
      0

      17
      0
      0
      1
      0
      0
      0
      1

      18
      0
      0
      1
      0
      0
      1
      0

      19
      0
      0
      1
      0
      0
      1
      1

      20
      1
      0
      1
      0
      1
      0
      0

      21
      1
      0
      1
      0
      1
      0
      1

      22
      1
      0
      1
      0
      1
      1
      0

      23
      1
      0
      1
      0
      1
      1
      1

      24
      0
      0
      1
      1
      0
      0
      0

      25
      0
      0
      1
      1
      0
      0
      1

      26
      0
      0
      1
      1
      0
      1
      0

      27
      0
      0
      1
      1
      0
      1
      1

      28
      1
      0
      1
      1
      1
      0
      0

      29
      1
      0
      1
      1
      1
      0
      1

      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...

      34
      1
      1
      0
      0
      0
      1
      0

      35
      1
      1
      0
      0
      0
      1
      1

      36
      0
      1
      0
      0
      1
      0
      0

      37
      0
      1
      0
      0
      1
      0
      1

      38
      1
      1
      0
      0
      1
      1
      0

      39
      1
      1
      0
      0
      1
      1
      1

      40
      0
      1
      0
      1
      0
      0
      0

      41
      0
      1
      0
      1
      0
      0
      1

      42
      1
      1
      0
      1
      0
      1
      0

      43
      1
      1
      0
      1
      0
      1
      1

      44
      0
      1
      0
      1
      1
      0
      0

      45
      0
      1
      0
      1
      1
      0
      1

      46
      1
      1
      0
      1
      1
      1
      0

      47
      1
      1
      0
      1
      1
      1
      1

      48
      0
      1
      1
      0
      0
      0
      0

      49
      1
      1
      1
      0
      0
      0
      1

      50
      0
      1
      1
      0
      0
      1
      0

      51
      1
      1
      1
      0
      0
      1
      1

      52
      0
      1
      1
      0
      1
      0
      0

      53
      1
      1
      1
      0
      1
      0
      1

      54
      0
      1
      1
      0
      1
      1
      0

      55
      1
      1
      1
      0
      1
      1
      1

      56
      0
      1
      1
      1
      0
      0
      0

      57
      1
      1
      1
      1
      0
      0
      1

      58
      0
      1
      1
      1
      0
      1
      0

      59
      1
      1
      1
      1
      0
      1
      1

      60
      0
      1
      1
      1
      1
      0
      0

      61
      1
      1
      1
      1
      1
      0
      1

      62
      0
      1
      1
      1
      1
      1
      0

      63
      1
      1
      1
      1
      1
      1
      1

64 rows × 7 columns

### myHDL 4:1 MUX Behavioral Level HDL Synthesis

```python
sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out=[Signal(bool(0)) for _ in range(7)]

toVerilog(MUX41_behavioral, sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)
#toVHDL(MUX41_behavioral, sel_1, sel_2, x_1in, x_2in, x_3in, x_4in,  f_out)
_=VerilogTextReader('MUX41_behavioral')
```

    ***Verilog modual from MUX41_behavioral.v***

     // File: MUX41_behavioral.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:21:53 2017

    `timescale 1ns/10ps

    module MUX41_behavioral (
        sel_1,
        sel_2,
        x_1in,
        x_2in,
        x_3in,
        x_4in,
        f_out
    );

    input sel_1;
    input sel_2;
    input x_1in;
    input x_2in;
    input x_3in;
    input x_4in;
    output f_out;
    reg f_out;

    always @(x_1in, sel_2, sel_1, x_2in, x_4in, x_3in) begin: MUX41_BEHAVIORAL_LOGIC
        if (((!sel_1) && (!sel_2))) begin
            f_out = x_1in;
        end
        else if (((!sel_1) && sel_2)) begin
            f_out = x_2in;
        end
        else if ((sel_1 && (!sel_2))) begin
            f_out = x_3in;
        end
        else begin
            f_out = x_4in;
        end
    end

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL behavioral level 4:1 MUX's verilog code

## 4:1 Multiplexer Behavioral with bitvectors

taking this a step further using bytes we can implement the behavioral using vector inputs instead of single bit inputs as follows

### How bit vectors work in myHDL and in Verilog/VHDL

###  Understanding BitVector bit selection in myHDL

```python
sel=intbv(1)[2:]; x_in=intbv(7)[4:]; f_out=bool(0)
```

```python
for i in x_in:
    print(i)
```

    False
    True
    True
    True

```python
for i in range(4):
    print(x_in[i])
```

    True
    True
    True
    False

### myHDL 4:1 MUX Behavioral with BitVecters and Testing
#### !? This needs to be checked

```python
def MUX41_behavioralVec(sel, x_in, f_out):
    @always_comb
    def logic():
        f_out.next=x_in[sel]
    return logic
```

```python
Peeker.clear()
sel=Signal(intbv(0)[2:]); Peeker(sel, 'sel')
x_in=Signal(intbv(0)[4:]); Peeker(x_in, 'x_in')
f_out=Signal(bool(0)); Peeker(f_out, 'f_out')

DUT=MUX41_behavioralVec(sel, x_in, f_out)

def MUX41_behavioralVec_TB(sel, x_in):

    selLen=len(sel); x_inLen=len(x_in)

    for i in range(2**x_inLen):
        x_in.next=i
        for j in range(selLen):
            sel.next=j

            yield delay(1)
            now()

```

```python
im=Simulation(DUT, MUX41_behavioralVec_TB(sel, x_in), *Peeker.instances()).run()
Peeker.to_wavedrom(tock=True,
                  title='MUX 4:1 behavioral vectype simulation')
```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson())
```

      f_out
      sel
      x_in

      0
      0
      =
      =

      1
      0
      =
      =

      2
      1
      =
      =

      3
      0
      =
      =

      4
      0
      =
      =

      5
      1
      =
      =

      6
      1
      =
      =

      7
      1
      =
      =

      8
      0
      =
      =

      9
      0
      =
      =

      10
      1
      =
      =

      11
      0
      =
      =

      12
      0
      =
      =

      13
      1
      =
      =

      14
      1
      =
      =

      15
      1
      =
      =

      16
      0
      =
      =

      17
      0
      =
      =

      18
      1
      =
      =

      19
      0
      =
      =

      20
      0
      =
      =

      21
      1
      =
      =

      22
      1
      =
      =

      23
      1
      =
      =

      24
      0
      =
      =

      25
      0
      =
      =

      26
      1
      =
      =

      27
      0
      =
      =

      28
      0
      =
      =

      29
      1
      =
      =

      30
      1
      =
      =

      31
      1
      =
      =

### myHDL 4:1 MUX Behavioral with BitVecters HDL Synthesis

```python
sel=Signal(intbv(0)[2:]); x_in=Signal(intbv(0)[4:]);
f_out=Signal(bool(0))
toVerilog(MUX41_behavioralVec,sel, x_in, f_out)
#toVHDL(MUX41_behavioralVec,sel, x_in, f_out)
_=VerilogTextReader('MUX41_behavioralVec')
```

    ***Verilog modual from MUX41_behavioralVec.v***

     // File: MUX41_behavioralVec.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:21:58 2017

    `timescale 1ns/10ps

    module MUX41_behavioralVec (
        sel,
        x_in,
        f_out
    );

    input [1:0] sel;
    input [3:0] x_in;
    output f_out;
    wire f_out;

    assign f_out = x_in[sel];

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL behavioral level 4:1 MUX using Bitvecters verilog code

## Generic Expressions via MUXs
(clean this and find harder exsample)

while shannon's theorem did not prove very much  useful in designing a 4:1 MUX it's true power lies converting boolean logic expression from and or gates to MUX's

using example 4.5 from Brown & Vranesic 3rd Ed

```python
w1, w2, w3=symbols('w_1, w_2, w_3')
f=(~w1&~w3)|(w1&w2)|(w1&w3)
f
```

```python
s1=w1
fp, fp0, fp1=shannon_exspanson(f, s1)
fp, fp0, fp1
```

```python
s2=w2
fpp0, fpp00, fpp01=shannon_exspanson(fp0, s2)
fpp1, fpp10, fpp11=shannon_exspanson(fp1, s2)

fpp0, fpp00, fpp01, fpp1, fpp10, fpp11
```

### myHDL Generic Expression via MUXs and Testing

```python
def Shannon21MUX(s1, s2, w_3in, f_out):
    @always_comb
    def logic():
        if (not s1) and (not s2):
            f_out.next=not w_3in
        elif (not s1) and ( s2):
            f_out.next=not w_3in
        elif ( s1) and (not s2):
            f_out.next= w_3in
        else:
            f_out.next=1
    return logic

```

```python
Peeker.clear()

s1, s2, w_3in,  f_out=[Signal(bool(0)) for _ in range(4)]
Peeker(s1, 's1'); Peeker(s2, 's2');
Peeker(w_3in, 'w_3in')
Peeker(f_out, 'f_out')

DUT=Shannon21MUX(s1, s2, w_3in, f_out)

inputs=[s1, s2, w_3in, f_out]
sim=Simulation(DUT, Combo_TB(inputs), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=0, stop_time=2*2**len(inputs), tock=True,
                  title='Shannon 2:1 MUX gate type simulation',
                  caption=f'after clock cycle {2**len(inputs)-1} ->random input')

```

    <class 'myhdl.StopSimulation'>: No more events

```python
MakeDFfromPeeker(Peeker.to_wavejson(start_time=0, stop_time=2**len(inputs) -1))
```

      f_out
      s1
      s2
      w_3in

      0
      1
      0
      0
      0

      1
      1
      0
      0
      0

      2
      0
      0
      0
      1

      3
      1
      0
      0
      1

      4
      1
      0
      1
      0

      5
      1
      0
      1
      0

      6
      0
      0
      1
      1

      7
      1
      0
      1
      1

      8
      0
      1
      0
      0

      9
      1
      1
      0
      0

      10
      1
      1
      0
      1

      11
      1
      1
      0
      1

      12
      1
      1
      1
      0

      13
      1
      1
      1
      0

      14
      1
      1
      1
      1

      15
      1
      1
      1
      1

### myHDL Generic Expression via MUXs HDL Synthesis

```python
s1, s2, w_3in,  f_out=[Signal(bool(0)) for _ in range(4)]

toVerilog(Shannon21MUX,s1, s2, w_3in, f_out)
#toVHDL(Shannon21MUX, s1, s2, w_3in, f_out)
_=VerilogTextReader('Shannon21MUX')
```

    ***Verilog modual from Shannon21MUX.v***

     // File: Shannon21MUX.v
    // Generated by MyHDL 0.9.0
    // Date: Mon Oct  9 19:22:03 2017

    `timescale 1ns/10ps

    module Shannon21MUX (
        s1,
        s2,
        w_3in,
        f_out
    );

    input s1;
    input s2;
    input w_3in;
    output f_out;
    reg f_out;

    always @(s2, s1, w_3in) begin: SHANNON21MUX_LOGIC
        if (((!s1) && (!s2))) begin
            f_out = (!w_3in);
        end
        else if (((!s1) && s2)) begin
            f_out = (!w_3in);
        end
        else if ((s1 && (!s2))) begin
            f_out = w_3in;
        end
        else begin
            f_out = 1;
        end
    end

    endmodule

The following shows the **Xilinx**'s _Vivado 2016.1_ RTL generated schematic of our myHDL 2:1 Mux expansion of $f$ using Shannon's Expansion Theorom

# Demultiplexers

# Encoders

# Decoders


```python
import numpy as np
import pandas as pd
import sympy

import weldx
import weldx.measurement as msm
import weldx.transformations as tf
from weldx import Q_
from weldx import util
```

```python
time = pd.timedelta_range(start="0s", end="10s", freq="1ms")
I_ts = util.sine(f=Q_(10, "1/s"), amp=Q_(20, "A"), bias=Q_(300, "A"))
I = I_ts.interp_time(time)
print(I)
I["time"] = I["time"]+pd.Timestamp("2020-01-01")
print(I)
print(len(I))
unit = I[0].data.units
value = I[0].data.magnitude
print(value)
```

    <xarray.DataArray (time: 10001)>
    <Quantity([300.         301.25581039 302.50666467 ... 297.49333533 298.74418961
     300.        ], 'ampere')>
    Coordinates:
      * time     (time) timedelta64[ns] 00:00:00 00:00:00.001000 ... 00:00:10
    <xarray.DataArray (time: 10001)>
    <Quantity([300.         301.25581039 302.50666467 ... 297.49333533 298.74418961
     300.        ], 'ampere')>
    Coordinates:
      * time     (time) datetime64[ns] 2020-01-01 ... 2020-01-01T00:00:10
    10001
    300.0

```python
I.to_pandas().to_csv("current_data.csv", header=["value"])
```

```python
df = pd.read_csv("current_data.csv", parse_dates=["time"], nrows=5)
```

```python
import ipywidgets as widgets
```

```python
df.dtypes
```

    time     datetime64[ns]
    value           float64
    dtype: object

```python
out = widgets.Output()
```

```python
hbox = widgets.HBox([out])
```

```python
with out:
    df
```

```python
hbox
```

    HBox(children=(Output(outputs=({'name': 'stdout', 'text': '                     time       value\n0 2020-01-01…

```python
from ipydatagrid import DataGrid

dg = DataGrid(df)
```

```python
dg
```

    DataGrid(column_widths={'value': 206, 'time': 187}, default_renderer=TextRenderer(), header_renderer=None)

```python
for c in df.columns:
    print(c)
```

    time
    value

```python
for k, v in df.dtypes.items():
    print(k)
    print(v)
```

    time
    datetime64[ns]
    value
    float64

```python
I.to_pandas().to_csv("current_data_DE.csv", header=["Wert"], index_label="Zeit", float_format='{:.4f}'.format)
```

```python
import xarray as xr
import pint

ureg = pint.UnitRegistry()
Q_ = ureg.Quantity
```

```python
values = Q_(np.array(df.value), 'volt')
print(values)
ds = xr.DataArray(values, coords=[df.time], dims=["time"])
```

    [300.0 301.25581039058625 302.5066646712861 303.7476262917145 304.9737977432971] volt

```python
print(ds)
```

    <xarray.DataArray (time: 5)>
    <Quantity([300.         301.25581039 302.50666467 303.74762629 304.97379774], 'volt')>
    Coordinates:
      * time     (time) datetime64[ns] 2020-01-01 ... 2020-01-01T00:00:00.004000

```python
ureg = pint.UnitRegistry()
```

```python
print(ureg('oschi'))
```

```python

```