# We implement an Extended Kalman Filter (EKF) on a double integrator $\ddot{x} = \epsilon$ with range/bearing measurements
We follow https://www.intechopen.com/books/introduction-and-implementations-of-the-kalman-filter/introduction-to-kalman-filter-and-its-applications section 3

### Process Model
\begin{align}
x_{k+1} &= \pmatrix{I_{3 \times 3} & I_{3 \times 3} \Delta t \\ 0_{3 \times 3} & I_{3 \times 3}} x_{k} + v_k \\
   &= Ax_k + v_k
\end{align}
   

where

$ x_k = [p_k^T, v_k^T] $ 

$v_k \sim N(0, Q)$

$ Q = \pmatrix{ 0_{3 \times 3} & & 0_{3 \times 3} \\
                                & \sigma_x^2 & 0 & 0 \\
                 0_{3 \times 3} & 0 & \sigma_y^2 & 0 \\
                                & 0 & 0 & \sigma_z^2} $

#### Prediction step
$ \hat{x}_{k+1}^- = A\hat{x}_k^+$

$ P_{k+1}^- = AP_k^+ A^T + Q_m$

where

$Q_m = \pmatrix{ 0_{3 \times 3} & 0_{3 \times 3} \\
                 0_{3 \times 3} & I_{3 \times 3} \sigma_v^2 } $ is the model's Q matrix (note that its different from the real Q here)

### Measurement model

 <div style="width: 100%;">
     <div style="width: 40%; float: left;">
      
    </div>
     <div style="width: 49%; float: left;">
\begin{align}
    z_k = \pmatrix{\phi \\ \theta \\ r} = 
    \pmatrix{\text{atan}\left(\frac{x_t-x_s}{y_t - y_s}\right) \\
         \text{atan}\left(\frac{z_t - z_s}{r_{xy}}\right) \\
         r_{xyz}} + w_k
\end{align}
    </div>
</div>
<br style="clear: left;">

where

$ r_{xy} = \sqrt{(x_t-x_s)^2 + (y_t-y_s)^2} $, 
$ r_{xyz} = \sqrt{(x_t-x_s)^2 + (y_t-y_s)^2 + (z_t-y_s)^2} $

$w_k \sim N(0, R)$

$R = \pmatrix{0.02^2 & 0 & 0 \\
              0 & 0.02^2 & 0 \\
              0 & 0 & 1.0^2 } $

#### Update step
$$ 
H_k = \frac{\partial z_k}{\partial x_k}\Big|_{\hat{x}_k^-} =
   \pmatrix{ \frac{-y}{x^2+y^2} & \frac{x}{x^2+y^2} & 0 \\
             \frac{-xz}{r_{xyz}^2 r_{xy}} & \frac{-yz}{r_{xyz}^2 r_{xy}} & \frac{1}{r_{xy}} & 0_{3 \times 3}\\
             \frac{x}{r_{xyz}} & \frac{y}{r_{xyz}} & \frac{z}{r_{xyz}}}
$$

$ \tilde{y}_k = z_k - h(\hat{x}_k^-)$

$ K_k = P_k^- H_k^T (R_m + H_k P_k^- H_k^T)^{-1} $

$ \hat{x}_k^+ = \hat{x}_k^- + K_k \tilde{y} $

$ P_k^+ = (I - K_k H_k)P_k^- $

where

$R_m = \pmatrix{0.02^2 & 0 & 0 \\
              0 & 0.02^2 & 0 \\
              0 & 0 & 1.0^2 } $ is the model's measurement noise covariance (note here we use the same as the actual measurement's covariance $Q$)


```python
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
from dymoesco.dynamics.doubleintegrator import DoubleIntegrator
from dymoesco.estimation.filters import EKF
np.set_printoptions(suppress=True, precision=3)
%load_ext autoreload
%autoreload 2
```


```python
# PARAMS
v_std = 0.5
# For now, we set them all equal
x_std = v_std
y_std = v_std
z_std = v_std

x0 = np.array([2,-2,0,5,5.1,0.1])
P0 = np.block([[np.eye(3) * 4**2, np.zeros((3,3))],
               [np.zeros((3,3)), np.eye(3) * 0.4**2]])
a = np.zeros(3)

N = 20
t_span = (0,N)
dt = 1.0
t = np.arange(*t_span, dt)
```


```python
def _g(x):
    return np.linalg.norm(x)
di = DoubleIntegrator(dim=3)
di._g = _g
ddi = di.discretize(1.0)
ddi._g = _g
```


```python
traj = di.simulate(lambda t: a, t_span, x0, t_eval=t, u_std=v_std, y_std=y_std)
```

#### First we write the EKF by calculating the jacobians by hand. Then we'll compare with an autograd implementation


```python
# KEEP THIS!
class HandEKF():
    
    def __init__(self, x0, P0, p_std, v_std, a_std, dt):
        self.x = x0
        self.P = P0
        self.dt = dt
        self.Q = np.block([[1/4 * dt**4 * np.eye(3), 1/2 * dt**3 * np.eye(3)],
                           [1/2 * dt**3 * np.eye(3), dt**2 * np.eye(3)]])
        self.R = a_std**2 *np.block([[np.eye(3) * p_std**2, np.zeros((3,3))],
                                     [np.zeros((3,3)), np.eye(3) * v_std**2]])
        self.A = np.block([[np.eye(3), np.eye(3)*dt],
                           [np.zeros((3,3)), np.eye(3)]])
        self.B = np.block([[1/2 * np.eye(3) * dt**2],
                           [np.eye(3) * dt]])
        
    def predict(self, a):
        self.x = self.A @ self.x + self.B @ a
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x
        
    def update(self, z):
        y = z - self.x
        K = self.P @ np.linalg.pinv(self.R + self.P)
        self.x = self.x + K@y
        self.P = (np.eye(6) - K) @ self.P
        return self.x
```

#### Here we use the autograd EKF


```python
Q = np.block([[np.zeros((3,3)), np.zeros((3,3))],
              [np.zeros((3,3)), v_std * np.eye(3)]])
ekf = EKF(ddi.f, ddi.g, Q, np.eye(1))
```


```python
preds = np.zeros((len(t), 6))
preds[0] = x0
x = x0; P = P0
for i in range(1, len(t)):
    x, P = ekf.predict(x, P, a)
    preds[i] = x
```


```python
# KF with pred + update
Ps = np.zeros((N,) + P0.shape)
Ps[0] = P0
filtered = np.zeros((N,6))
filtered[0] = x0
x = x0; P = P0;
for i in range(1,len(t)):
    x, P = ekf.predict(x, P, a)
    x, P = ekf.update(x, P, traj.y[i])
    filtered[i] = x
    Ps[i] = P
```


```python
from ipywidgets import interact
@interact(ground_truth=True, observations=True, pred=False, pred_and_update=True)
def f(ground_truth, observations, pred, pred_and_update):
    fig, ax = plt.subplots()
    if ground_truth:
        di.plot_phase(traj, label="ground truth", ax=ax, color="black")
    if pred:
        plt.plot(*preds.T, label="KF pred", color="blue")
    #if pred_and_update:
        # TODO: fix this (should only take first 3 dimensions of cov matrix)
        #ekf.plot(filtered, Ps, ax=ax, label="pred+update", color="orange")
    plt.legend()
```


    interactive(children=(Checkbox(value=True, description='ground_truth'), Checkbox(value=True, description='obse…



```python

```

bbfe2837a80c983a90eeb76e8b8fcd2da9df6e66
ipynb
Jupyter Notebook
examples/estimation/3d_di_radar_ekf.ipynb
samlaf/dymoesco
1695333aab8171f7a26062eb8ad7b0be38493d3d
MIT
examples/estimation/3d_di_radar_ekf.ipynb
samlaf/dymoesco
1695333aab8171f7a26062eb8ad7b0be38493d3d
MIT
examples/estimation/3d_di_radar_ekf.ipynb
samlaf/dymoesco
1695333aab8171f7a26062eb8ad7b0be38493d3d
MIT
Qwen/Qwen-72B
1. YES
2. YES