# Multivariate Kalman filter on linear manoeuvring data

# Purpose
* implementation of multivariate kalman filter inspired by: [https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/06-Multivariate-Kalman-Filters.ipynb](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/06-Multivariate-Kalman-Filters.ipynb)

# Methodology
* Implement a kalman filter to chase a ship at unsteady velocity

# Setup


```python
# %load imports.py
## Local packages:

%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.symbols import *
from src.parameters import *
import src.symbols as symbols
from src import prime_system
from src.models import regression
from src.visualization.regression import show_pred
from src.visualization.plot import track_plot

## Load models:
# (Uncomment these for faster loading):
import src.models.vmm_linear  as vmm
from src.data.case_0 import ship_parameters, df_parameters, ps, ship_parameters_prime
from src.data.transform import transform_to_ship

from scipy.stats import norm
import filterpy.stats as stats
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import KalmanFilter
np.set_printoptions(linewidth=150)

from numpy import zeros,dot
from numpy.linalg import inv
```

## Generate some data


```python
mask = df_parameters['prime'].notnull()
index = df_parameters.loc[mask,'prime'].index
coefficients=vmm.simulator.get_all_coefficients(sympy_symbols=False)
missing_coefficients = set(coefficients) - set(index)
missing_coefficients
```


```python
mask = df_parameters['prime'].notnull()
df_parameters.loc[mask,'prime']
```

## Simulate data


```python
parameters=df_parameters['prime'].copy()

t_ = np.arange(0,4,0.01)
df_ = pd.DataFrame(index=t_)

df_['u'] = 1
df_['v'] = 0
df_['r'] = 0
df_['x0'] = 0
df_['y0'] = 0
df_['psi'] = 0
df_['U'] = np.sqrt(df_['u']**2 + df_['v']**2)
df_['beta'] = -np.arctan2(df_['v'],df_['u'])
df_['delta'] = np.deg2rad(20)

## Simulation in prime system!
result = vmm.simulator.simulate(df_=df_, parameters=parameters, ship_parameters=ship_parameters_prime, 
                                control_keys=['delta'], primed_parameters=False,
                                prime_system=ps, method='Radau')
df_result=result.result.copy()
```


```python
result.track_plot(compare=False)
result.plot(compare=False);
```

## Measurement noise


```python
df_measure = df_result.copy()
df_measure['u1d'] = np.NaN
df_measure['v1d'] = np.NaN
df_measure['r1d'] = np.NaN

R_u_var = ((df_measure['u'].abs().max())/30)**2
R_v_var = ((df_measure['v'].abs().max())/30)**2
R_r_var = ((df_measure['r'].abs().max())/30)**2


df_measure['R_u'] = np.random.normal(loc=0, scale=np.sqrt(R_u_var), size=len(df_measure))  # measurement noise
df_measure['R_v'] = np.random.normal(loc=0, scale=np.sqrt(R_v_var), size=len(df_measure))  # measurement noise
df_measure['R_r'] = np.random.normal(loc=0, scale=np.sqrt(R_r_var), size=len(df_measure))  # measurement noise

Q_var = 0.00001

df_measure['Q'] = np.random.normal(loc=0, scale=np.sqrt(Q_var), size=len(df_measure))  # process noise              

df_measure['u']+=(df_measure['R_u']+df_measure['Q']) 
df_measure['v']+=(df_measure['R_v']+df_measure['Q']) 
df_measure['r']+=(df_measure['R_r']+df_measure['Q']) 

```


```python
A = vmm.simulator.A
b = vmm.simulator.b
acceleration = sp.matrices.MutableDenseMatrix([u1d,v1d,r1d])
eq_simulator = sp.Eq(sp.UnevaluatedExpr(A)*sp.UnevaluatedExpr(acceleration),sp.UnevaluatedExpr(b))
Math(vlatex(eq_simulator))
```


```python
A_inv = A.inv()
S = sp.symbols('S')
eq_S=sp.Eq(S,-sp.fraction(A_inv[1,1])[1])

A_inv_S = A_inv.subs(eq_S.rhs,S)
eq_acceleration_matrix_clean = sp.Eq(sp.UnevaluatedExpr(acceleration),sp.UnevaluatedExpr(A_inv_S)*sp.UnevaluatedExpr(b))
Math(vlatex(eq_acceleration_matrix_clean))
```


```python
eq_S
```


```python
eq_acceleration_matrix_clean_specific = eq_acceleration_matrix_clean.subs([
    (X_qs, vmm.simulator.X_qs_eq.rhs),
    (Y_qs, vmm.simulator.Y_qs_eq.rhs),
    (N_qs, vmm.simulator.N_qs_eq.rhs),
])

Math(vlatex(eq_acceleration_matrix_clean_specific))
```


```python
u_,v_,r_,u1d,v1d,r1d,delta_ = sp.symbols('u v r u1d v1d r1d delta')

subs = [

    (u1d,u1d),
    (v1d,v1d),
    (r1d,r1d),
    
    (u,u_),
    (v,v_),
    (r,r_),
    (delta,0),


]

A,b = sp.linear_eq_to_matrix(eq_acceleration_matrix_clean_specific.doit().subs(subs).rhs,
                       [u_,v_,r_])
```


```python
A
```


```python
subs = [

    (u1d,u1d),
    (v1d,v1d),
    (r1d,r1d),
    
    (u,0),
    (v,0),
    (r,0),
    (delta,delta_),


]
B,b = sp.linear_eq_to_matrix(eq_acceleration_matrix_clean_specific.doit().subs(subs).rhs,
                       [delta_])
#B=sp.Matrix.vstack(B*symbols.dt,B)
B=sp.Matrix.vstack(sp.zeros(3,1),B)
```


```python
B
```


```python
F = sp.Matrix.vstack(
    sp.Matrix.hstack(sp.eye(3),symbols.dt*sp.eye(3)),
    sp.Matrix.hstack(A,sp.zeros(3))
)  # transition matrix
F
```


```python
subs = {value:key for key,value in p.items()}
A_lambda = lambdify(A.subs(subs))

F_lambda = lambdify(F.subs(subs))

subs = {value:key for key,value in p.items()}
B_lambda = lambdify(B.subs(subs))

S_lambda = lambdify(eq_S.rhs.subs(subs))
```


```python
df = df_measure
```


```python
parameters=df_parameters['prime'].copy()
mask = parameters.notnull()

values = parameters.loc[mask]
parameters.loc[mask]+=0.01*values*np.random.rand(len(values))  # add some noise to the parameters
mask=(parameters==0)
parameters.loc[mask]=0.00001
```


```python
mask = parameters.notnull()
parameters.loc[mask]
```


```python
U_ = float(df.iloc[0]['U'])
dt_ = df.index[1] - df.index[0]
S_ = run(S_lambda, inputs=parameters, **ship_parameters_prime)
F_ = run(F_lambda, inputs=parameters, **ship_parameters_prime, S=S_, U=U_, dt=dt_)
B_ = run(B_lambda, inputs=parameters, **ship_parameters_prime, S=S_, U=U_, dt=dt_)
A_ = run(A_lambda, inputs=parameters, **ship_parameters_prime, S=S_, U=U_, dt=dt_)
```


```python
F_
```


```python
B_
```


```python
np.eye(3,3) + A_*dt_ + (A_*dt_)**2/np.math.factorial(2) + (A_*dt_)**3/np.math.factorial(3)
```

## Kalman filtering


```python
from scipy.linalg import inv

def predict(F,x,B,u):
    x = F @ x + B.dot(u)
    return x

def filter2(zs, x,us,P,F,H,R,Q,B):

    xs, cov = [], []
    for z,u in zip(zs,us):
        
        z = z.reshape(len(z),1)
        
        # predict
        x = predict(F=F,x=x,B=B,u=u)
        P = F @ P @ F.T + Q
        
        #update
        S = H @ P @ H.T + R
        K = P @ H.T @ inv(S)
        y = z - H @ x
        x += K @ y
        P = P - K @ H @ P
        
        xs.append(x)
        cov.append(P)
    
    xs, cov = np.array(xs), np.array(cov)
    return xs, cov
```


```python
x=np.array([list(df.iloc[0][['u','v','r']].values) + [0,0,0]]).T  # state mean

state_keys = ['u','v','r','u1d','v1d','r1d']

est_cov = (df_result[state_keys].abs().max()/3)**2
mask = est_cov==0
est_cov[mask] = np.mean(est_cov)

P = np.diag(est_cov)         # state covariance

F = F_       # measurement function         # transition matrix

H = np.concatenate([np.eye(3),np.zeros((3,3))],axis=1)       # measurement function
R_var = 100
R = np.diag(((df[['u','v','r']].abs().max()/3)**2).values)        # measurement covariance
Q = 0  # process covariance
B = B_     # control transition matrix

zs=df[['u','v','r']].values

us= df['delta'].values
#us=np.zeros(len(df))
```


```python
np.linalg.inv(P)
```


```python
P.shape
```


```python
x
```


```python
zs=df[['u','v','r']].values
us= df['delta'].values

x=np.array([df_result.iloc[0][state_keys].values]).T  # state mean

xs, cov = [], []
x_prime = np.zeros(x.shape)
L_ = ship_parameters['L']

for z,u in zip(zs,us):
    
    z = z.reshape(len(z),1)
    
    x = predict(F=F,x=x,B=B,u=u)
    xs.append(x)
   
xs = np.array(xs)

df_pred = pd.DataFrame(data=xs.reshape((len(df),6)), 
                       columns=['u','v','r','u1d','v1d','r1d'], index=df.index)
```


```python
x=np.array([list(df.iloc[0][['u','v','r']].values) + [0,0,0]]).T  # state mean
u_=us[0]
predict(F=F,x=x,B=B,u=u_)
```


```python
B.dot(u)
```


```python
B
```


```python
for key in ['u','v','r','u1d','v1d','r1d']:
    fig,ax=plt.subplots()
    fig.set_size_inches(10,2)
    df_result.plot(y=key, ax=ax, label='real')
    df_measure.plot(y=key, style='.', ax=ax, label='measurement')
    
    df_pred.plot(y=key, style='--', ax=ax, label='prediction')
    ax.set_ylabel(key)
```


```python
x
```


```python
#x=np.array([list(df.iloc[0][['u','v','r']].values) + [0,0,0]]).T  # state mean
x=np.array([list(df_result.iloc[0][['u','v','r','u1d','v1d','r1d']].values)]).T  # state mean

xs,cov=filter2(zs=zs, x=x, us=us, P=P, F=F, H=H, R=R, Q=Q, B=B)
df_pred = pd.DataFrame(data=xs.reshape((len(df),6)), 
                       columns=['u','v','r','u1d','v1d','r1d'], index=df.index)
```


```python
for key in ['u','v','r','u1d','v1d','r1d']:
    fig,ax=plt.subplots()
    fig.set_size_inches(10,2)
    df_result.plot(y=key, ax=ax, label='real')
    df.plot(y=key, style='.', ax=ax, label='measure')
    
    df_pred.plot(y=key, style='--', ax=ax, label='filter')
    #df_smooth.plot(y=key, style='--', ax=ax, label='smoother')
    ax.set_ylabel(key)


```


```python
Xs = xs.reshape(len(zs),len(x))
Ps = cov
n = Xs.shape[0]
Fs = [F]*n
Qs = [Q]*n
```


```python
Xs.shape
```


```python
def rts_smoother(Xs, Ps, F, Q):
    n, dim_x = Xs.shape

    # smoother gain
    K = zeros((n,dim_x, dim_x))
    x, P, Pp = Xs.copy(), Ps.copy(), Ps.copy()

    for k in range(n-2,-1,-1):
        Pp[k] = F @ P[k] @ F.T + Q # predicted covariance

        try:
            K[k]  = P[k] @ F.T @inv(Pp[k])
        except:
            # I don't understand why this happens...
            break
        
        x[k] += K[k] @ (x[k+1] - (F @ x[k]))     
        P[k] += K[k] @ (P[k+1] - Pp[k]) @ K[k].T
    
    return (x, P, K, Pp)
```


```python
(x, P, K, Pp) = rts_smoother(Xs=Xs, Ps=Ps, F=F, Q=Q)

df_smooth = pd.DataFrame(data=x.reshape((len(df),6)), 
                       columns=['u','v','r','u1d','v1d','r1d'], index=df.index)
```

## PyKalman


```python
B
```


```python
F2 = np.concatenate([F,B], axis=1)
F2 = np.concatenate([F2,np.zeros((1,F2.shape[1]))], axis=0)
F2[F2.shape[0]-1,F2.shape[1]-1]=1
print(F2)
```


```python
H2 = np.concatenate([H,np.zeros((H.shape[0],1))], axis=1)
H2 = np.concatenate([H2,np.zeros((1,H2.shape[1]))], axis=0)
H2[H2.shape[0]-1,H2.shape[1]-1]=1
H2
```


```python
delta_ = df.iloc[0]['delta']

x=np.array([list(df.iloc[0][['u','v','r']].values) + [0,0,0] + [delta_]]).T  # state mean

P2 = np.diag(list((df_result[state_keys].abs().max()/3)**2) + [0])         # state covariance

R_var = 100
R2 = np.diag(list(((df[['u','v','r']].abs().max()/3)**2).values) + [0])        # measurement covariance

Q_var = 0.01
Q = 0  # process covariance
B = B_         # control transition matrix

zs=df[['u','v','r','delta']].values

kf = KalmanFilter(transition_matrices=F2,
                  #observation_matrices=H2, 
                  #transition_covariance=Q, 
                  #observation_covariance=R2,
                  initial_state_mean=x[:,0],
                  #initial_state_covariance=P2
             )
```


```python
F2.shape
```


```python
H2.shape
```


```python
Q
```


```python
R2.shape
```


```python
P2.shape
```


```python
x.shape
```


```python
kf.filter(zs)
```


```python

```

f64aff70210a9288a5fb183a0ff935f4fd715c9b
ipynb
Jupyter Notebook
notebooks/15.11_kalman_filter_linear_manoeuvring.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.11_kalman_filter_linear_manoeuvring.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.11_kalman_filter_linear_manoeuvring.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
Qwen/Qwen-72B
1. YES
2. YES