# Parameter Identification Technique (PIT) on a linear VMM model

# Purpose
Show the general workflow by applying PIT regression on a very simple model

# Methodology
* Load time series from a ZigZag test
* Determine yaw rate and acceleration (compare with accelerometers from test).
* Find the best parameter values in the Nomoto model using OLS linear regression.

# Setup


```python
# %load imports.py
%matplotlib inline
%load_ext autoreload
%autoreload 2
%config Completer.use_jedi = False  ## (To fix autocomplete)

## External packages:
import pandas as pd
pd.options.display.max_rows = 999
pd.options.display.max_columns = 999
pd.set_option("display.max_columns", None)

import numpy as np
import os
import matplotlib.pyplot as plt
#if os.name == 'nt':
#    plt.style.use('presentation.mplstyle')  # Windows

import plotly.express as px 
import plotly.graph_objects as go

import seaborn as sns
import sympy as sp
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame,
                                      Particle, Point)
from sympy.physics.vector.printing import vpprint, vlatex
from IPython.display import display, Math, Latex
from src.substitute_dynamic_symbols import run, lambdify

import pyro

import sklearn
import pykalman
from statsmodels.sandbox.regression.predstd import wls_prediction_std
import statsmodels.api as sm

from scipy.integrate import solve_ivp

## Local packages:
from src.data import mdl
from src.models import linear_vmm
import src.linear_vmm_equations as eq
from src.symbols import *
import src.symbols as symbols
from src.models import regression
```

## Load test


```python
df_runs = mdl.runs()
```


```python
#id=22773
id=22616
#id=22774
#id=22770


df, units, meta_data = mdl.load(id=id, dir_path='../data/processed/kalman')
df.index = df.index.total_seconds()
df = df.iloc[0:-100].copy()
df.index-=df.index[0]
df.sort_index(inplace=True)
```


```python
meta_data['rho']=1000
meta_data['mass'] = meta_data['Volume']*meta_data['rho']
meta_data.dropna()
```


```python
df.head()
```


```python
from src.visualization.plot import track_plot
fig,ax=plt.subplots()
#fig.set_size_inches(10,10)
track_plot(df=df, lpp=meta_data.lpp, x_dataset='x0', y_dataset='y0',  psi_dataset='psi', beam=meta_data.beam, ax=ax);
```

## Yaw rate


```python
px.line(data_frame=df, y=['r'], width=1400, height=300,)
```

# Linear VMM

## N


```python
Math(vlatex(eq.N_eq))
```


```python
N_eq = eq.N_eq.copy()
N_eq = N_eq.subs([
    (x_G,0),  # Assuming or moving to CG=0
    #(I_z,1),  # Removing inertia
    #(eq.p.Nrdot,0),  # Removing added mass
    (eq.p.Nvdot,0),  # Removing added mass
    (eq.p.Nudot,0),  # Removing added mass
    
])  

solution = sp.solve(N_eq,r1d)[0]
N_eq = sp.Eq(r1d, solution*(I_z-eq.p.Nrdot))  # Putting r1d on the LHS
```


```python
#Math(vlatex(N_eq))
```


```python
diff_eq_N = regression.DiffEqToMatrix(ode=N_eq, label=r1d, base_features=[delta,u,v,r])
```


```python
Math(vlatex(diff_eq_N.acceleration_equation))
```

$ y = X \cdot \beta + \epsilon $


```python
Math(vlatex(diff_eq_N.acceleration_equation_x))
```


```python
Math(vlatex(diff_eq_N.eq_y))
```


```python
diff_eq_N.eq_beta
```


```python
Math(vlatex(diff_eq_N.eq_X))
```


```python
diff_eq_N.X_lambda
```


```python
from statsmodels.sandbox.regression.predstd import wls_prediction_std
def show_pred(X,y,results, label):
    
    display(results.summary())
    
    X_ = X
    y_ = y
    y_pred = results.predict(X_)
    
    prstd, iv_l, iv_u = wls_prediction_std(results, exog=X_, alpha=0.05)
    #iv_l*=-1 
    #iv_u*=-1
    
    fig,ax=plt.subplots()
    ax.plot(X_.index,y_, label='Numerical gradient from model test')
    ax.plot(X_.index,y_pred, '--', label='OLS')
    ax.set_ylabel(label)
    
    ax.fill_between(X_.index, y1=iv_l, y2=iv_u, zorder=-10, color='grey', alpha=0.5, label=r'5\% confidence')
    ax.legend();
```


```python
X = diff_eq_N.calculate_features(data=df)
y = diff_eq_N.calculate_label(y=df['r1d'])

model_N = sm.OLS(y,X)
results_N = model_N.fit()

show_pred(X=X,y=y,results=results_N, label=r'$\dot{r}$')
```

## Y


```python
Y_eq = eq.Y_eq.copy()
Y_eq = Y_eq.subs([
    #(eq.p.Yvdot,1),  # Removing added mass
    (eq.p.Yudot,0),  # Removing added mass
    (eq.p.Yrdot,0),  # Removing added mass  
    (x_G,0),  
    
])  
solution = sp.solve(Y_eq,v1d)[0]
solution = solution*(-eq.p.Yvdot+m)
solution = solution + U*m*r# (adding u*m*r to the measurement fy instead)  
Y_eq = sp.simplify(sp.Eq(v1d, solution))  # Putting r1d on the LHS
```


```python
Math(vlatex(Y_eq))
```


```python
diff_eq_Y = regression.DiffEqToMatrix(ode=Y_eq, label=v1d, base_features=[delta,u,v,r])
```


```python
diff_eq_Y.eq_beta
```


```python
diff_eq_Y.acceleration_equation_x
```


```python
X = diff_eq_Y.calculate_features(data=df, simplify_names=True)
y = diff_eq_Y.calculate_label(y=df['v1d'])
#y+=df['u']*df['r']  # adding centrifugal force

model_Y = sm.OLS(y,X)
results_Y = model_Y.fit()

show_pred(X=X,y=y,results=results_Y, label=r'$\dot{v}$')
```


```python
X.head()
```

## X


```python
X_eq = eq.X_eq.copy()
X_eq = X_eq.subs([
    #(eq.p.Xudot,1),  # Removing added mass
    (eq.p.Xvdot,0),  # Removing added mass
    (eq.p.Xrdot,0),  # Removing added mass
    
    #(m,0),  # mass
    
])  

solution = sp.solve(X_eq,u1d)[0]
X_eq = sp.Eq(u1d, solution*(-eq.p.Xudot+m))  # Putting r1d on the LHS
```


```python
Math(vlatex(X_eq))
```


```python
diff_eq_X = regression.DiffEqToMatrix(ode=X_eq, label=u1d, base_features=[delta,u,v,r])
```


```python
X = diff_eq_X.calculate_features(data=df)
y = diff_eq_X.calculate_label(y=df['u1d'])

model_X = sm.OLS(y,X)
results_X = model_X.fit()

show_pred(X=X,y=y,results=results_X, label=r'$\dot{u}$')
```


```python
results_summary_X = regression.results_summary_to_dataframe(results_X)
results_summary_Y = regression.results_summary_to_dataframe(results_Y)
results_summary_N = regression.results_summary_to_dataframe(results_N)
```

## Simulation


```python
eqs = [diff_eq_X.acceleration_equation, diff_eq_Y.acceleration_equation, diff_eq_N.acceleration_equation]
solution = sp.solve(eqs, u1d, v1d, r1d, dict=True)

## Decouple the equations:
u1d_eq = sp.Eq(u1d, solution[0][u1d]) 
v1d_eq = sp.Eq(v1d, solution[0][v1d]) 
r1d_eq = sp.Eq(r1d, solution[0][r1d]) 

## Lambdify:
subs = {value:key for key,value in eq.p.items()}
u1d_lambda = lambdify(u1d_eq.subs(subs).rhs)
v1d_lambda = lambdify(v1d_eq.subs(subs).rhs)
r1d_lambda = lambdify(r1d_eq.subs(subs).rhs)
```


```python
display(Math(vlatex(u1d_eq)))
display(Math(vlatex(v1d_eq)))
display(Math(vlatex(r1d_eq)))
```


```python
from scipy.spatial.transform import Rotation as R

def step(t, states, parameters, df_control, meta_data):
    
    u,v,r,x0,y0,psi = states

    states_dict = {
        
        'u':u,
        'v':v,
        'r':r,
        
        'x0':x0,
        'y0':y0,
        'psi':psi,
        
        }
        
    inputs = dict(parameters)
    inputs.update(states_dict)
    
    index = np.argmin(np.array(np.abs(df_control.index - t)))
    control = dict(df_control.iloc[index])
    #print(f't:{t}, i:{index}')
    inputs.update(control)
    
    inputs['U'] = np.sqrt(u**2 + v**2)  #Instantanious velocity
    
    if not 'u1d' in control:
        u1d = -run(function=u1d_lambda, inputs=inputs)
    else:
        u1d = control['u1d']
    
    mass = meta_data['mass']
    v1d = run(function=v1d_lambda, inputs=inputs) - u*r
    #v1d = run(function=v1d_lambda, inputs=inputs)
    
    r1d = run(function=r1d_lambda, inputs=inputs)
    
    
    rotation = R.from_euler('z', psi, degrees=False)
    w = 0
    velocities = rotation.apply([u,v,w])
    x01d = velocities[0]
    y01d = velocities[1]
    psi1d = r    

    dstates = [
        u1d,
        v1d,
        r1d,
        x01d,
        y01d,
        psi1d,
    ]    
    
    #print(t)
    
    return dstates


```


```python
parameters = dict(results_summary_X['coeff'])  # Fix (-)
parameters.update(dict(results_summary_Y['coeff']))   # Fix (-)
parameters.update(dict(results_summary_N['coeff']))

states = ['u','v','r','x0','y0','psi']
states0 = [float(df[key].iloc[0]) for key in states]
#df_control = df[['delta','u1d','u']]
df_control = df[['delta']].copy()
#df_control['delta']+=np.deg2rad(0.5)
```


```python
t_ = 10
index = np.argmin(np.array(np.abs(df_control.index - t_)))
df_control.iloc[index]
```


```python
(np.diff(df_control.index) > 0).all()
```


```python
## Simulate:
df_ = df.copy()
t = np.array(df_.index)
t_span = [t[0],t[-1]]
solution = solve_ivp(fun=step, t_span=t_span, y0=states0, t_eval=t, args=(parameters, df_control, meta_data), method='RK45')
```


```python
df_result = pd.DataFrame(data=solution.y.T, columns=states)
df_result.index=t
```


```python
fig,ax=plt.subplots()
#fig.set_size_inches(15,15)
track_plot(df=df_, lpp=meta_data['lpp'], beam=meta_data['beam'], ax=ax, color='k', label='model test');
track_plot(df=df_result, lpp=meta_data['lpp'], beam=meta_data['beam'], ax=ax, color='g', label='simulation');
ax.legend()
```


```python
fig,ax=plt.subplots()
df_['-delta'] = -df_['delta']
df_.plot(y='-delta', style='--', ax=ax)
df_.plot(y='psi', ax=ax, label='model test')
df_result.plot(y='psi', ax=ax, label='simulation')
ax.legend()


for key in ['u','v','r']:
    
    fig,ax=plt.subplots()
    df_.plot(y=key, ax=ax, label='model test')
    df_result.plot(y=key, ax=ax, label='simulation')
    ax.set_ylabel(key)

```


```python

```

1ca22d43125fffe414d907bd439ee74dd5931b1a
ipynb
Jupyter Notebook
notebooks/04.02_PIT_linear_VMM.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/04.02_PIT_linear_VMM.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/04.02_PIT_linear_VMM.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
Qwen/Qwen-72B
1. YES
2. YES