# Extended Kalman filter with Parameter Identification ($T_1$ and $K$) for Nomoto model
An Extended Kalman filter with a Nomoto model as the predictor will be developed. 
The filter should also estimate the parameters in the Nomoto model. This is done by setting up a system where the parameters are defined as some of the states in the model 
The filter is run on simulated data as well as real model test data.


```python
%load_ext autoreload
%autoreload 2

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from numpy.linalg import inv
import sympy as sp

import src.visualization.book_format as book_format
book_format.set_style()
from src.substitute_dynamic_symbols import lambdify
from sympy import Matrix
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame,
                                      Particle, Point)
from IPython.display import display, Math, Latex
from src.substitute_dynamic_symbols import run, lambdify
from sympy.physics.vector.printing import vpprint, vlatex
from src.data import mdl
from src.kalman_filter import extended_kalman_filter
```

## Nomoto model for ship manoeuvring dynamics
The Nomoto model can be written as:


```python
r,r1d,r2d = sp.symbols('r \dot{r} \ddot{r}')
psi,psi1d = sp.symbols('psi \dot{\psi}')
h,u = sp.symbols('h u')
x, x1d = sp.symbols('x \dot{x}')
A,B,C,D,E, Phi = sp.symbols('A B C D E Phi')
w = sp.symbols('w')

K, delta, T_1, T_2 = sp.symbols('K delta T_1 T_2')

eq_nomoto = sp.Eq(K*delta,
                 r + T_1*r1d + T_2*r2d)
Math(vlatex(eq_nomoto))
```

where $r$ is yaw rate with its time derivatives and $\delta$ is the rudder angle. $K$, $T_{1}$
 and $T_{1}$ are the coefficients describing the hydrodynamics of the ship.
 
For slow manoeuvres this equation can be further simplified by removing the $\ddot{r}$ term into a first order Nomoto model:


```python
eq_nomoto_simple = eq_nomoto.subs(r2d,0)
Math(vlatex(eq_nomoto_simple))
```

### Simulation model


```python
f_hat = sp.Function('\hat{f}')(x,u,w)
eq_system = sp.Eq(x1d, f_hat)
eq_system
```

Where the state vector $x$:


```python
eq_x = sp.Eq(x, sp.UnevaluatedExpr(Matrix([psi,r])))
eq_x
```

and input vector $u$:

and $w$ is zero mean Gausian process noise

For the nomoto model the time derivatives for the states can be expressed as:


```python
eq_psi1d = sp.Eq(psi1d,r)
eq_psi1d
```


```python
eq_r1d = sp.Eq(r1d,sp.solve(eq_nomoto_simple,r1d)[0])
eq_r1d
```


```python
def lambda_f_constructor(K, T_1):
    def lambda_f(x, u):
        delta = u
        f = np.array([[x[1], (K*delta-x[1])/T_1]]).T
        return f

    return lambda_f
```

## Simulation
Simulation with this model where rudder angle shifting between port and starboard


```python
T_1_ = 1.8962353076056344
K_ = 0.17950970687951323
h_ = 0.01

lambda_f = lambda_f_constructor(K=K_, T_1=T_1_)
```


```python
def simulate(E, ws, t, us):
    
    simdata = []
    x_=np.deg2rad(np.array([[0,0]]).T)
    
    for u_,w_ in zip(us,ws):
               
        x_=x_ + h_*lambda_f(x=x_.flatten(), u=u_)
    
        simdata.append(x_.flatten())
        
    simdata = np.array(simdata)
    df = pd.DataFrame(simdata, columns=["psi","r"], index=t)
    df['delta'] = us
    
    return df
```


```python
N_ = 8000

t_ = np.arange(0,N_*h_,h_)
    
us = np.deg2rad(np.concatenate((-10*np.ones(int(N_/4)),
                                10*np.ones(int(N_/4)),
                                -10*np.ones(int(N_/4)),
                                10*np.ones(int(N_/4)))))

np.random.seed(42)
E = np.array([[0, 1]]).T
process_noise = np.deg2rad(0.01)
ws = process_noise*np.random.normal(size=N_)
df = simulate(E=E, ws=ws, t=t_, us=us)

measurement_noise = np.deg2rad(0.5)
df['epsilon'] = measurement_noise*np.random.normal(size=N_)
df['psi_measure'] = df['psi'] + df['epsilon']
df['psi_deg'] = np.rad2deg(df['psi'])
df['psi_measure_deg'] = np.rad2deg(df['psi_measure'])
df['delta_deg'] = np.rad2deg(df['delta'])

```


```python
fig,ax=plt.subplots()
df.plot(y='psi_deg', ax=ax)
df.plot(y='psi_measure_deg', ax=ax, zorder=-1)
df.plot(y='delta_deg', ax=ax, zorder=-1)
df.plot(y='r')
ax.set_title('Simulation with measurement and process noise')
ax.set_xlabel('Time [s]');


```

## Kalman filter
Implementation of the Kalman filter. The code is inspired of this Matlab implementation: [ExEKF.m](https://github.com/cybergalactic/MSS/blob/master/mssExamples/ExEKF.m).


```python
jac = sp.eye(4,4) + Matrix([r,eq_r1d.rhs,0,0]).jacobian([psi,r,T_1,K])*h
jac
```


```python
def lambda_f_constructor2():
    def lambda_f(x, u):
        delta = u
        T_1 = x[2]  # Note! T_1 is the third state now!
        K = x[3]  # Note! K is the fourth state!
        r = x[1]
        
        f = np.array([[r, (K*delta-r)/T_1, 0, 0]]).T
        return f

    return lambda_f
```


```python
def lambda_jacobian_constructor(h):
    def lambda_jacobian(x, u):

        T_1 = x[2]  # Note! T_1 is the third state now!
        K = x[3]  # Note! K is the fourth state!
        delta = u
        r = x[1]

        jac = np.array(
            [
                [1, h, 0, 0],
                [0, 1 - h / T_1, -h * (K * delta - r) / T_1 ** 2, delta*h/T_1],
                [0, 0, 1, 0],
                [0, 0, 0, 1]
            ]
        )
        return jac

    return lambda_jacobian
```


```python
lambda_jacobian = lambda_jacobian_constructor(h=h_)
lambda_f = lambda_f_constructor2()
```


```python
lambda_jacobian(x=[0,0,0.1,0.1], u=0)
```


```python
lambda_f(x=[0,0,0.1, 0.1], u=0)
```


```python
def plot(df,time_steps):

    x_hats = np.array([time_step["x_hat"] for time_step in time_steps]).T
    time = np.array([time_step["time"] for time_step in time_steps]).T
    Ks = np.array([time_step["K"] for time_step in time_steps]).T
    stds = np.sqrt(np.array([[time_step["P_hat"][0,0],
                          time_step["P_hat"][1,1],
                          time_step["P_hat"][2,2],
                          time_step["P_hat"][3,3],
                         ] for time_step in time_steps]).T)

    n=len(P_prd)
    fig,axes=plt.subplots(nrows=n)
    
    df['T_1'] = T_1_
    df['K'] = K_
    keys = ['psi','r','T_1','K']
    labels = {
        'psi' : '\Psi',
    }
    for i,key in enumerate(keys):
        
        ax=axes[i]
        
        if key=='psi':
            df.plot(y='psi_measure', ax=ax, label="Measured", zorder=-1)
        else:
            ax.plot([])
        
        df.plot(y=key, ax=ax, label="True")
        
        
        ax.plot(time, x_hats[i, :], "-", label="kalman")
        
        ylims = np.array(ax.get_ylim())
        
        std_top = x_hats[i, :] + stds[i, :]
        std_btm = x_hats[i, :] - stds[i, :]
        ax.plot(time, std_top, linestyle=':', color='k', lw=1, alpha=0.4)
        ax.plot(time, std_btm, linestyle=':', color='k', lw=1, alpha=0.4)
        ax.fill_between(time, std_top, std_btm,
                         facecolor='yellow', alpha=0.2, interpolate=True, label='+/- std')
        
        if np.all(ylims < 0):
            ax.set_ylim(ylims[0],0)
        elif np.all(ylims > 0):
            ax.set_ylim(0,ylims[1])
        else:
            ax.set_ylim(ylims)
        
        
        ylabel=labels.get(key,key)
        ax.set_ylabel(f'${ylabel}$')
        
        if i<2:
            ax.legend(loc='upper left')
        else:
            ax.legend().set_visible(False)
        
```


```python
x0=np.deg2rad(np.array([[0,0,3, 0.2]]).T)
P_prd = np.diag([np.deg2rad(1), np.deg2rad(0.1), 0.1, 0.5])

Qd = np.diag([np.deg2rad(0), 2, 1])

Rd = np.deg2rad(0.1)

ys = df['psi_measure'].values

E_ = np.array(
    [
     [0,0,0], 
     [1,0,0],
     [0,1,0],
     [0,0,1],
        
    ],
)

C_ = np.array([[1, 0, 0, 0]])

Cd_ = C_
Ed_ = h_ * E_

time_steps = extended_kalman_filter(x0=x0, P_prd=P_prd, lambda_f=lambda_f, 
                                    lambda_jacobian=lambda_jacobian,h=h_, us=us, ys=ys, E=E_, Qd=Qd, Rd=Rd, Cd=Cd_)

x_hats = np.array([time_step["x_hat"] for time_step in time_steps]).T
time = np.array([time_step["time"] for time_step in time_steps]).T
Ks = np.array([time_step["K"] for time_step in time_steps]).T
stds = np.sqrt(np.array([[time_step["P_hat"][0,0],
                      time_step["P_hat"][1,1],
                      time_step["P_hat"][2,2],
                      time_step["P_hat"][3,3],
                     ] for time_step in time_steps]).T)

```


```python
plot(df=df, time_steps=time_steps)
```


```python
fig,axes=plt.subplots(nrows=2)
for i, key in enumerate(['T_1','K']):
    ax = axes[i]
    ax.set_ylabel(key)
    ax.set_xlabel('Time [s]')
    ax.plot(time,x_hats[2+i,:],label='Kalman')
    df.plot(y=key, ax=ax, label='True')
    ax.legend()

```


```python
fig,ax=plt.subplots()
keys = ['psi','r','T_1','K']
for i,key in enumerate(keys):
    ax.plot(time,Ks[i,:],label=key)
ax.set_title('Kalman gains')
ax.legend();
ax.set_ylim(-1,1);

```


```python
fig,axes=plt.subplots(nrows=2)

ax=axes[0]
for i,key in enumerate(keys):
    ax.plot(time,stds[i,:]**2,label=key)
ax.set_title('Variances')
ax.legend();

ax=axes[1]
df.plot(y='delta',ax=ax)


```


```python
x0=np.deg2rad(np.array([[0,0,3, 0.2]]).T)
P_prd = np.diag([np.deg2rad(1), np.deg2rad(0.1), 0.1, 0.1])

Qd = np.diag([np.deg2rad(0), 2, 1])

Rd = np.deg2rad(0.1)

ys = df['psi_measure'].values

E_ = np.array(
    [
     [0,0,0], 
     [1,0,0],
     [0,1,0],
     [0,0,1],
        
    ],
)

C_ = np.array([[1, 0, 0, 0]])

Cd_ = C_
Ed_ = h_ * E_

for i in range(5):
    time_steps = extended_kalman_filter(x0=x0, P_prd=P_prd, lambda_f=lambda_f, 
                                        lambda_jacobian=lambda_jacobian,h=h_, us=us, ys=ys, E=E_, Qd=Qd, Rd=Rd, Cd=Cd_)
    
    time_step = time_steps[-1]
    x0[2:]=np.array([time_step['x_hat'][2:]]).T
    P_prd = time_step['P_hat']
```


```python
plot(df=df, time_steps=time_steps)
```

58099ec72c69d3c25e2b446dc23eccc896fefede
ipynb
Jupyter Notebook
notebooks/15.41_EKF_PIT_nomoto.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.41_EKF_PIT_nomoto.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.41_EKF_PIT_nomoto.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
Qwen/Qwen-72B
1. YES
2. YES