<jupyter_start><jupyter_text>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<jupyter_code># %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<jupyter_output><empty_output><jupyter_text>Generate some data<jupyter_code>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
mask = df_parameters['prime'].notnull()
df_parameters.loc[mask,'prime']<jupyter_output><empty_output><jupyter_text>Simulate data<jupyter_code>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()
result.track_plot(compare=False)
result.plot(compare=False);<jupyter_output><empty_output><jupyter_text>Measurement noise<jupyter_code>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']) 

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))
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))
eq_S
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))
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_])
A
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)
B
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
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))
df = df_measure
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
mask = parameters.notnull()
parameters.loc[mask]
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_)
F_
B_
np.eye(3,3) + A_*dt_ + (A_*dt_)**2/np.math.factorial(2) + (A_*dt_)**3/np.math.factorial(3)<jupyter_output><empty_output><jupyter_text>Kalman filtering<jupyter_code>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
x=np.array([list(df.iloc[0][['u','v','r']].values) + [0,0,0]]).T  # state mean

state_keys = ['<KEY>']

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))
np.linalg.inv(P)
P.shape
x
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)
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_)
B.dot(u)
B
for key in ['<KEY>']:
    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)
x
#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)
for key in ['<KEY>']:
    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)


Xs = xs.reshape(len(zs),len(x))
Ps = cov
n = Xs.shape[0]
Fs = [F]*n
Qs = [Q]*n
Xs.shape
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)
(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)<jupyter_output><empty_output><jupyter_text>PyKalman<jupyter_code>B
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)
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
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
             )
F2.shape
H2.shape
Q
R2.shape
P2.shape
x.shape
kf.filter(zs)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1828978622327791, 'fraction_numerical': 0.019681031557516118, 'mean_word_length': 4.09244060475162, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 28, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '6', 'dir': 'jupyter-structured-clean-dedup', 'id': '432158', 'license': 'MIT', 'n_tokens_mistral': 5001, 'n_tokens_neox': 4843, 'n_words': 796, 'path': 'notebooks/15.11_kalman_filter_linear_manoeuvring.ipynb', 'repo_name': 'martinlarsalbert/wPCC', 'size': '11854'}