# Multivariate Kalman filter

# 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

%matplotlib inline
%load_ext autoreload
%autoreload 2

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
import seaborn as sns
from scipy.stats import norm
import filterpy.stats as stats
```


```python
import math
import numpy as np
from numpy.random import randn

def compute_dog_data(z_var, process_var, count=1, dt=1.):
    "returns track, measurements 1D ndarrays"
    x, vel = 0., 1.
    z_std = math.sqrt(z_var) 
    p_std = math.sqrt(process_var)
    xs, zs = [], []
    for _ in range(count):
        v = vel + (randn() * p_std)
        
        x += v*dt        
        xs.append([x,v])
        
        zs.append(x + randn() * z_std)  
        
    return np.array(xs), np.array(zs), 
```


```python
from filterpy.common import Q_discrete_white_noise

dt = 1.
R_var = 10
Q_var = 0.01

count = 50
track, zs = compute_dog_data(R_var, Q_var, count)

```


```python
from scipy.linalg import inv

def filter(zs,x,P,F,H,R,Q):

    xs, cov = [], []
    for z in zs:
        # predict
        x = F @ x
        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([[0, 1]]).T  # state mean
P = np.diag([500, 49])         # state covariance
F = np.array([[1, dt],         # transition function
              [0,  1]])
H = np.array([[1., 0.]])       # measurement function
R = np.array([[R_var]])        # measurement covariance
# z : measurement mean
Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)  # process covariance

xs,cov=filter(zs=zs, x=x, P=P, F=F, H=H, R=R, Q=Q)
```


```python
fig,ax=plt.subplots()

ax.plot(track[:,0], label='real')
ax.plot(zs, '.', label='measurement')
ax.plot(xs[:,0], label='filter')
ax.set_ylabel('x [m]')
ax.legend()

fig,ax=plt.subplots()

ax.plot(track[:,1], label='real')
ax.plot(xs[:,1], label='filter')
ax.set_ylabel('v [m/s]')
ax.legend()
```


```python
from scipy.stats import multivariate_normal

ncols = 4
fig,axes=plt.subplots(ncols=ncols)
fig.set_size_inches(15,5)

for n,i in enumerate(range(0, len(xs), int(len(xs)/(ncols-1)))):

    mus = xs[i].flatten()
    rv = multivariate_normal(mean=mus, cov=cov[i])
    
    x_ = np.linspace(np.min(xs[:,0]), np.max(xs[:,0]),50)
    y_ = np.linspace(np.min(xs[:,1]), np.max(xs[:,1]),50)
    
    xx_, yy_ = np.meshgrid(x_, y_)
    pos = np.dstack((xx_, yy_))
    
    ax=axes[n]
    ax.contourf(xx_, yy_, rv.pdf(pos));
```

## Smoother


```python
from numpy import zeros
from numpy.linalg import inv

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

        K[k]  = P[k] @ F.T @inv(Pp[k])
        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
xs,cov=filter(zs=zs, x=x, P=P, F=F, H=H, R=R, Q=Q)
xs = xs.reshape((len(zs),len(x)))

xs_reverse = np.flipud(xs)
P_reverse = np.flipud(cov)
```


```python
def filter3(zs,x,Ps,F,H,R,Q):

    xs, cov = [], []
    for z,P in zip(zs,Ps):
        # predict
        x = F @ x
        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
xs_,cov_ = filter3(zs=xs_reverse[:,0], x=xs_reverse[0,:], Ps=P_reverse, F=F, H=H, R=R, Q=Q)
xs_ = np.flipud(xs_)
xs_ = xs_.reshape(xs.shape)
cov_ = np.flipud(cov_)

fig,ax=plt.subplots()

ax.plot(track[:,0], label='real')
ax.plot(zs, '.', label='measurement')
ax.plot(xs[:,0], label='filter')
ax.plot(x_[:,0], label='smoother')
ax.set_ylabel('x [m]')

```


```python
xs_reverse.shape
```


```python
fig,ax=plt.subplots()

ax.plot(track[:,0], label='real')
ax.plot(zs, '.', label='measurement')
ax.plot(xs[:,0], label='filter')
ax.plot(x_smooth[:,0], label='smoother')
ax.set_ylabel('x [m]')
ax.legend()

fig,ax=plt.subplots()

ax.plot(track[:,1], label='real')
ax.plot(xs[:,1], label='filter')
ax.plot(x_smooth[:,1], label='smoother')
ax.set_ylabel('v [m/s]')
ax.legend()
```

## PyKalman


```python
from filterpy.kalman import KalmanFilter
kf = KalmanFilter(dim_x=2, dim_z=1)

kf.x = np.array([[0, 1]]).T  # state mean
kf.P = np.diag([500, 49])         # state covariance
kf.F = np.array([[1, dt],         # transition function
              [0,  1]])
kf.H = np.array([[1., 0.]])       # measurement function
kf.R = np.array([[R_var]])        # measurement covariance
# z : measurement mean
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)  # process covariance
```


```python
(mu, cov, _, _) = kf.batch_filter(zs=zs)

fig,ax=plt.subplots()
ax.plot(track[:,0], label='real')
ax.plot(zs, '.', label='measurement')
ax.plot(mu[:,0], label='filter')
ax.set_ylabel('x [m]')
ax.legend()

fig,ax=plt.subplots()
ax.plot(track[:,1], label='real')
ax.plot(mu[:,1], label='filter')
ax.set_ylabel('v [m/s]')
ax.legend()
```

## Ball drop

### Example
Let's exemplify this with a simple problem, simulated a ball being dropped in air.
The forces acting on this ball will be the drag from the air, which is modelled as: $C_d \cdot \dot{x}^2$ and the gravity, which is modelled as $g \cdot m$:


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

x = dynamicsymbols('x')
m,F,g = sp.symbols('m F g')

eq_kinetics = sp.Eq(F, -m*g)
Math(vlatex(eq_kinetics))
```

The kinematics connecting the forces to motion can be described with Newtons 2nd law:


```python
eq_kinematics = sp.Eq(F, m*x.diff().diff())
Math(vlatex(eq_kinematics))
```

The acceleration can then be calculated using these two equations:


```python
eqs=[eq_kinetics,eq_kinematics]
solution = sp.solve(eqs, x.diff().diff(), F, dict=True)[0]
eq_acc = sp.Eq(x.diff().diff(),solution[x.diff().diff()])
Math(vlatex(eq_acc))
```

This equation can be used to simulate the motion of the ball, by doing a time step integration of this initial value problem.


```python
from scipy.integrate import solve_ivp

acceleration_lambda = lambdify(eq_acc.rhs)

inputs={
'g' : 9.81,
}

def step(t,states, inputs):

    x1d = states[1]
    x2d = acceleration_lambda(**inputs)

    dstates = [x1d, x2d]
    return dstates

t_ = np.linspace(0,10,100)
y0 = [
    0,0
]
solution = solve_ivp(fun=step, y0=y0, t_span=[t_[0],t_[-1]], t_eval=t_, args=(inputs,))

df_result = pd.DataFrame(solution.y.T, index=solution.t, columns=['x','x1d'])
df_result['x2d'] = acceleration_lambda(**inputs)

fig,axes=plt.subplots(nrows=3)
ax=axes[0]
df_result.plot(y='x', label='$x$ [m]', ax=ax)

ax.set_title('Ball position')

ax=axes[1]
df_result.plot(y='x1d', label='$\dot{x}$ [m/s]', ax=axes[1])
ax.set_title('Ball velocity [m/s]')
ax.set_xlabel('time [s]')

ax=axes[2]
df_result.plot(y='x2d', label='$\ddot{x}$ [m/s^2]', ax=axes[2])
ax.set_title('Ball acceleration [m/s]')
ax.set_xlabel('time [s]')

for ax in axes:
    ax.grid(True)
plt.tight_layout()
```


```python
df_measure = df_result.copy()
df_measure['x1d'] = np.NaN
df_measure['x2d'] = np.NaN

R_var = 100
Q_var = 0.01


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

df_measure['x']+=(df_measure['R']+df_measure['Q']) 
                  

```


```python
dt = float(df_measure.index[1]-df_measure.index[0] )
x = np.array([[0, 0]]).T  # state mean
P = np.diag([500, 49])         # state covariance
F = np.array([[1, dt],         # transition matrix
              [0,  1]])
H = np.array([[1., 0.]])       # measurement function
R = np.array([[R_var]])        # measurement covariance
Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)  # process covariance
xs,cov=filter(zs=df_measure['x'], x=x, P=P, F=F, H=H, R=R, Q=Q)
df_pred2 = pd.DataFrame(data=xs.reshape((len(df_result),2)), index=df_result.index, columns=['x','x1d'])

x_smooth, P_smooth, K_smooth, Pp_smoot = rts_smoother(xs, cov, F, Q)
df_pred2_smooth= pd.DataFrame(data=x_smooth.reshape((len(df_measure),2)), columns=['x','x1d'], index=df_measure.index)

```

## Control


```python
from scipy.linalg import inv

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

    xs, cov = [], []
    for z in zs:
        # predict
        x = F @ x + B.dot(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
def rts_smoother_control(Xs, Ps, F, Q, B, u):
    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

        K[k]  = P[k] @ F.T @inv(Pp[k])
        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
dt = float(df_measure.index[1]-df_measure.index[0] )
x = np.array([[0, 0]]).T  # state mean
P = np.diag([500, 49])         # state covariance
F = np.array([[1, dt],         # transition matrix
              [0,  1]])
H = np.array([[1., 0.]])       # measurement function
R = np.array([[R_var]])        # measurement covariance
Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)  # process covariance
B = np.array([[1/2*dt**2],         # control transition matrix
                  [dt]])

a = -9.81

xs,cov=filter2(zs=df_measure['x'], x=x, u=a, P=P, F=F, H=H, R=R, Q=Q, B=B)
df_pred_control = pd.DataFrame(data=xs.reshape((len(df_measure),2)), columns=['x','x1d'], index=df_measure.index)

x_smooth, P_smooth, K_smooth, Pp_smoot = rts_smoother_control(xs, cov, F, Q, B=B, u=a)
df_pred_control_smooth= pd.DataFrame(data=x_smooth.reshape((len(df_measure),2)), columns=['x','x1d'], index=df_measure.index)

```


```python
fig,ax=plt.subplots()
fig.set_size_inches(15,8)

df_result.plot(y='x', ax=ax, label='real')
df_measure.plot(y='x', style='.', ax=ax, label='measure')
df_pred2.plot(y='x', style='--', ax=ax, label='filter')
df_pred2_smooth.plot(y='x', style='--', ax=ax, label='smoother')
df_pred_control.plot(y='x', style='--', ax=ax, label='filter control')
df_pred_control_smooth.plot(y='x', style='--', ax=ax, label='smoother control ')


fig,ax=plt.subplots()
fig.set_size_inches(15,8)
df_result.plot(y='x1d', ax=ax, label='real')
df_pred2.plot(y='x1d', style='--', ax=ax, label='filter')
df_pred2_smooth.plot(y='x1d', style='--', ax=ax, label='smoother')
df_pred_control.plot(y='x1d', style='--', ax=ax, label='filter control')
df_pred_control_smooth.plot(y='x1d', style='--', ax=ax, label='smoother control ')

```

### statsmodels


```python
import numpy as np
import statsmodels.api as sm
import matplotlib.pyplot as plt

# --- True values --- #
init_state = 1.
# covariance matrix
state_noise = 0.02
# measurement noise
measure_noise = 0.8

# --- Generate Data --- #
np.random.seed(1234)
numsteps = 80

# control inputs
controls = np.array([0]*1 + [0]*19 + [3]*40 + [0]*20)
# get true states
true_states = np.zeros(numsteps)
true_states[0] = init_state 
true_states += controls
# state noise
true_states += np.random.normal(0, state_noise, numsteps)

# measurements
measurements = np.array([(s + np.random.normal(0, measure_noise)) for s in true_states])

# --- Create the model --- #
mod = sm.tsa.statespace.MLEModel(measurements, k_states=1, k_posdef=1)
mod['design'] = [1.]
mod['obs_cov'] = [measure_noise]
mod['state_intercept'] = np.r_[controls[1:], np.nan][None,:]
mod['selection'] = [1.]
mod['state_cov'] = [state_noise]
mod.initialize_known(1+controls[0:1], [[0.02]])
res = mod.filter(init_state)

# --- Plot the results --- #
fig, axes = plt.subplots(2, 1, figsize=(10,6))
time = range(numsteps)
axes[0].plot(time, measurements, 'b', label='measured')
axes[0].plot(time, true_states, 'r', label='true')
axes[0].plot(time, res.filtered_state[0], 'g', label='estimates')
#axes[0].plot(time, res.forecasts[0], 'g', label='estimates')
axes[0].legend()

axes[1].plot(time, res.filtered_state_cov[0,0,:], 'b', label='covariance')
axes[1].legend();
```


```python

```

cee1a478f0217fc357d70696e7a5b015606f4f12
ipynb
Jupyter Notebook
notebooks/15.10_kalman_filter_multivariate.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.10_kalman_filter_multivariate.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.10_kalman_filter_multivariate.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
Qwen/Qwen-72B
1. YES
2. YES