# Extended Kalman filter
This is an implementation of the example Kalman filter: [ExEKF.m](https://github.com/cybergalactic/MSS/blob/master/mssExamples/ExEKF.m).

ExEKF Discrete-time extended Kalman filter (EKF) implementation demonstrating
how the "predictor-corrector representation" can be applied to the
nonlinear model:

$dx_1/dt = x_2$ <br>
$dx_2/dt = a * x_2 * abs(x_2) + b * u + white noise $ <br>
$y = x_1 + white noise$ <br>

The GNSS position measurement frequency f_gnss [Hz] can be chosen smaller or
equal to the  sampling frequency f_s [Hz]. The ratio between the 
frequencies must be an integer:

Integer:  Z = f_s/f_gnss >= 1 

Author:    Thor I. Fossen <br>
Date:      17 Oct, 2018 <br>
Revisions: 28 Feb. 2020, minor updates of notation <br>



```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
```


```python
x_1,x_2, a, b, u, w, h = sp.symbols("x_1,x_2, a, b, u, w, h")
jac = sp.eye(2) + Matrix([x_2,
       a * x_2 * x_2 + b * u + w]).jacobian([x_1,x_2])*h
jac
```


```python
lambda_jacobian = lambdify(jac)
```


```python
lambda_jacobian(a=1,h=0.1,x_2=3)
```


```python
f = Matrix([x_2,
       a * x_2 * sp.Abs(x_2) + b * u + w])
lambda_f = lambdify(f)
```


```python
lambda_f
```


```python
from src.kalman_filter import (extended_kalman_filter_example, 
        extended_kalman_filter_parameter_estimation_example, simulate_model)
```


```python
# simulation parameters
N = 100  # no. of iterations
f_m = 1  # sampling frequency [Hz]
h_m = 1 / f_m  # sampling time: h  = 1/f_s (s)
t = np.arange(0, N * h_m, h_m)

# initial values for x
x0 = np.array([[0, 0]]).T
us = 0.1 * np.sin(0.1 * t)  # inputs
np.random.seed(42)
a_ = -0.9
ws = 0.1 * np.random.normal(scale=1, size=N)  # process noise

df = simulate_model(x0=x0, us=us, ws=ws, t=t, a=a_)

fig, axes = plt.subplots(nrows=3)
df.plot(y="u", label="u (input)", ax=axes[0])
df.plot(y="x_1", ax=axes[1])
df.plot(y="x_2", ax=axes[2])
plt.show()

```


```python
## Measured yaw angle:
df["epsilon"] = 0.1 * np.random.normal(scale=3, size=N)  # measurement noise
df["y"] = df["x_1"] + df["epsilon"]
ys = np.zeros((N, 1))  # 1!
ys[:, 0] = df["y"].values
```


```python
## Discretisized system matrixes:
f_s = 10  # sampling frequency [Hz]
h_s = 1 / f_s  # sampling time: h  = 1/f_s (s)
    
# initialization of Kalman filter
x0 = np.array([[3.5, 0]]).T
P_prd = np.diag([1, 1])
Qd = 1
Rd = 10
df2 = extended_kalman_filter_example(
    x0=x0, P_prd=P_prd, lambda_f=lambda_f, 
    lambda_jacobian=lambda_jacobian, h_m=h_m, h=h_s, us=us, ys=ys, Qd=Qd, Rd=Rd
    )
```


```python
df2.head()
```


```python
fig,axes=plt.subplots(nrows=3)
df.plot(y='u', label='u (input)', ax=axes[0])
axes[1].set_ylabel('$x_1$')
df.plot(y='y', style='.', alpha=0.7, ax=axes[1])
df.plot(y='x_1', label='model', ax=axes[1])
df2.plot(y='x_1 predictor', label='predictor', style='--', ax=axes[1])
df2.plot(y='x_1', label='kalman', style=':', ax=axes[1])


axes[2].set_ylabel('$x_2$')
df.plot(y='x_2', label='model', ax=axes[2]);
df2.plot(y='x_2 predictor', label='predictor', style='--', ax=axes[2]);
df2.plot(y='x_2', label='kalman', style=':', ax=axes[2])
```

## Parameter etimation
The extended Kalman Filter can also be used to estimate model parameters during the filtering.

### estimating $a$


```python
x_1,x_2, a, b, u, w, h = sp.symbols("x_1,x_2, a, b, u, w, h")
jac_a = sp.eye(3) +  Matrix([x_2,
       a * x_2 * x_2 + b * u + w, 0]).jacobian([x_1,x_2,a])*h
jac_a
```


```python
lambda_jacobian_a = lambdify(jac_a)
```


```python
lambda_jacobian_a
```


```python
def lambda_jacobian_a(a, h, x_2):
    jac = np.array([
        [1, h, 0],
        [0, 2*a*h*np.abs(x_2) + 1, h*x_2*np.abs(x_2)],
        [0,0,1]
    ])
    return jac
```


```python
f_a = Matrix([x_2,
       a * x_2 * sp.Abs(x_2) + b * u + w,0])
lambda_f_a = lambdify(f_a)
```


```python
f_a
```


```python
# simulation parameters
N = 4000  # no. of iterations
#f_m = 1  # sampling frequency [Hz]
h_m = 0.05  # sampling time: h  = 1/f_s (s)
t = np.arange(0, N * h_m, h_m)

# initial values for x
x0 = np.array([[0, 0]]).T
us = 0.1 * np.sin(0.1 * t)  # inputs
np.random.seed(42)
process_noise = 0
ws = process_noise * np.random.normal(scale=1, size=N)  # process noise
a_ = -0.9
df = simulate_model(x0=x0, us=us, ws=ws, t=t, a=a_)
```


```python
## Measured yaw angle:
noise = 0
df["epsilon"] = noise * np.random.normal(scale=3, size=N)  # measurement noise
df["y"] = df["x_1"] + df["epsilon"]
ys = np.zeros((N, 1))  # 1!
ys[:, 0] = df["y"].values
```


```python
## Discretisized system matrixes:
h_s = h_m

e=1
E = np.array([[0,0],
              [e,0],
              [0,e]])



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

# initialization of Kalman filter
x0 = np.array([[0, 0, 0]]).T
P_prd = np.diag([1, 1, 1])

Qd = np.diag([1, 0.1]) # Q = diag( Q_x2  Q_a )
Rd = 10 # R = diag( R_x1 )

time_steps = extended_kalman_filter_parameter_estimation_example(
    x0=x0, P_prd=P_prd, lambda_f=lambda_f_a, 
    lambda_jacobian=lambda_jacobian_a, h=h_s, us=us, ys=ys, Qd=Qd, Rd=Rd, E=E, Cd=Cd
    )
```


```python
x_hats = np.array([time_step['x_hat'] for time_step in time_steps]).T
x_hats.shape
```


```python
kalman_gains = np.array([time_step['K'] for time_step in time_steps]).T
kalman_gains.shape
```


```python
time = np.array([time_step['time'] for time_step in time_steps]).T
time.shape
```


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

ax=axes[0]
ax.set_ylabel('$x_1$')
df.plot(y='y', style='-', alpha=0.7, ax=ax)
df.plot(y='x_1', label='model', ax=ax)
ax.plot(time,x_hats[0,:], '--', label='kalman')
ax.legend()


ax=axes[1]
ax.set_ylabel('$x_2$')
df.plot(y='x_2', label='model', ax=ax);
ax.plot(time,x_hats[1,:], '--', label='kalman')
ax.legend()

ax=axes[2]
ax.set_ylabel('$a$')
ax.plot(time,x_hats[2,:], '--', label='kalman')
ax.plot([time[0], time[-1]], [a_,a_], label='$a$')
ax.legend()


```


```python
fig,ax=plt.subplots()
for k in kalman_gains:
    ax.plot(time, k)

ax.legend(['$x_1$','$x_2$','$a$'])
ax.set_title('Kalman gains')
```

f1844f47136022c27405cbaee5ee49da2325171b
ipynb
Jupyter Notebook
notebooks/15.31_EKF_fossen.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.31_EKF_fossen.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
notebooks/15.31_EKF_fossen.ipynb
martinlarsalbert/wPCC
16e0d4cc850d503247916c9f5bd9f0ddb07f8930
MIT
Qwen/Qwen-72B
1. YES
2. YES