# Dump "Complementary filter"

Attitude estimation is determined from attitude propagation based on Gyro and measurements of tilt, roll and yaw
based on accelerometer and magnetometer, following [ahrs_python:doc](https://ahrs.readthedocs.io/en/latest/filters/complementary.html).

Basic idea is to combine attitude propagation (gyro, $\mathbf{q}_\mathrm{\omega}$) and attitude measurement
(magnetometer and accelormehter, $\mathbf{q}_\mathrm{am}$ with a complementary factor $\alpha$:

$
\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}, \quad \alpha \in (0, 1)
$

* yaw angle $\psi$: for North east down frame the angle between north and x axis in horizontal place
* pitch angle $\phi$: angle between horizontal plane and x axis
* roll angle $\theta$:




```python
from matplotlib import pyplot as plt
import numpy as np
import sympy
import quaternionic as quat
from scipy.integrate import cumtrapz
from sympy import sin, cos, symbols
from sympy.matrices import rot_axis1, rot_axis2, rot_axis3
from sympy.algebras.quaternion import Quaternion
```

## Configuration parameters


```python

```


```python
# num of samples
T = 15 #s
n = 50000

# Gyro
gyro_noise = 0.003
gyro_bias = [0.002, 0.02, 0.01]

# Accelerometer
acc_noise = 0.02
acc_bias = [0.1, 0.07, 0.1]

# Magnetometer
mag_noise = 0.01
mag_bias = np.zeros(3) #[0.03, 0.02, 0.04]
```


```python
# timesteps between samples
Δt = T / n

# rotation velocities in body frame
ωs = np.zeros((n, 3), dtype=float)
ωs[:,1] = np.sin(np.linspace(0, 2*np.pi, n))/25
```


```python
θs = cumtrapz(ωs, dx=Δt, axis=0, initial=0.0)

plt.plot(θs[:,0],'-')
plt.plot(θs[:,1],'--')
plt.plot(θs[:,2],'x', markevery=n//20)
```


```python
_tmp = np.array([[0.0, 1.0, 0.0]] * n) * θs[:,1][:,np.newaxis]
qs = quat.array.from_axis_angle(_tmp)
```

## Simulated Measurements

### Gyro


```python
ωs_ = ωs + np.random.normal(gyro_bias, scale=gyro_noise, size=(n,3))
```


```python
plt.plot(ωs_[:,0])
plt.plot(ωs_[:,1], '--')
plt.plot(ωs_[:,2], 'x', markevery=n//24)
```

### Accelerometer


```python
g = np.array([0,0,-1])

a = (qs.to_rotation_matrix @ g) \
    + np.random.normal(acc_bias, scale=acc_noise, size=(n, 3))
```


```python
plt.plot(a[:,0])
plt.plot(a[:,1])
plt.plot(a[:,2])
plt.grid()
```

### Magnetometer


```python
bs_0 = np.array([19355.97, 0, 4495.36]) # values from https://www.geomatrix.co.uk/tools/magnetic-field-calculator/
bs_0 = bs_0 / np.linalg.norm(bs_0)

ms = qs.to_rotation_matrix @ bs_0  \
    + np.random.normal(mag_bias, scale=mag_noise, size=(n,3))
```


```python
plt.plot(ms[:,0], label='0')
plt.plot(ms[:,1], label='1')
plt.plot(ms[:,2], label='2')

plt.legend();
```

## Attitude Propagation


```python
def propagate_attitude(q, ω, Δt):
    q0 = q.to_scalar_part
    q1, q2, q3 = q.to_vector_part
    G = np.array([
        [-q1, q0, q3,-q2],
        [-q2,-q3, q0, q1],
        [-q3, q2,-q1, q0]
    ])
    
    return q + quat.array(0.5 * G.T @ (ω * Δt))
```


```python
import sympy

qw, qx, qy, qz = sympy.symbols("q_w, q_x, q_y, q_z", real=True)
ωx, ωy, ωz, Δt = sympy.symbols(r"omega_x, omega_y, omega_z, dt", real=True)

G = sympy.Matrix([
        [-qx, qw, qz,-qy],
        [-qy,-qz, qw, qx],
        [-qz, qy,-qx, qw]
])

Q = sympy.Matrix([qw, qx, qy, qz])

sol = (Q + 0.5 * G.T * sympy.Matrix([ωx, ωy, ωz]) * Δt)

sol.simplify()

sol
```




$\displaystyle \left[\begin{matrix}- 0.5 dt \left(\omega_{x} q_{x} + \omega_{y} q_{y} + \omega_{z} q_{z}\right) + q_{w}\\0.5 dt \left(\omega_{x} q_{w} - \omega_{y} q_{z} + \omega_{z} q_{y}\right) + q_{x}\\0.5 dt \left(\omega_{x} q_{z} + \omega_{y} q_{w} - \omega_{z} q_{x}\right) + q_{y}\\0.5 dt \left(- \omega_{x} q_{y} + \omega_{y} q_{x} + \omega_{z} q_{w}\right) + q_{z}\end{matrix}\right]$




```python
sympy.lambdify((qw, qx, qy, qz, ωx, ωy, ωz, Δt), sol)(1, 0, 0, 0, 1.0, 1.0, 0.0, 0.1)
```




    array([[1.  ],
           [0.05],
           [0.05],
           [0.  ]])




```python
sympy.ccode(sol)
```




    '// Not supported in C:\n// ImmutableDenseMatrix\nMatrix([\n[-0.5*dt*(omega_x*q_x + omega_y*q_y + omega_z*q_z) + q_w],\n[ 0.5*dt*(omega_x*q_w - omega_y*q_z + omega_z*q_y) + q_x],\n[ 0.5*dt*(omega_x*q_z + omega_y*q_w - omega_z*q_x) + q_y],\n[0.5*dt*(-omega_x*q_y + omega_y*q_x + omega_z*q_w) + q_z]])'




```python
import quaternionic as quat
import numpy as np
q_test = quat.array((1.0, 0.0, 0.0, 0.0))
a = propagate_attitude(q_test, np.array((0.1, 0.0, 0.0)), 0.1)
a / a.norm
```




    quaternionic.array([0.999975  , 0.00499988, 0.        , 0.        ])




```python
q_gyro = quat.array(np.zeros((n, 4)))
q_gyro[0, :] = (1.0, 0, 0, 0)

for i, ω in enumerate(ωs_[1:, :], start=1):
    q_gyro[i, :] = propagate_attitude(q_gyro[i-1,:], ω, Δt)
```


```python
plt.plot(q_gyro.to_axis_angle)
```

## Attitude Measurement (Acc, Mag)

pitch $\phi$ and roll $\theta$ from accelerometer

$
\begin{array}{rcl}
\theta &=& \mathrm{arctan2}(a_y, a_z) \\
\phi &=& \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big)
\end{array}
$   

yaw $\psi$ from mag

$
\begin{array}{rcl}
\mathbf{b} &=&
\begin{bmatrix}
    \cos\theta & \sin\theta\sin\phi & \sin\theta\cos\phi \\
    0 & \cos\phi & -\sin\phi \\
    -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi
\end{bmatrix}
\begin{bmatrix}m_x \\ m_y \\ m_z\end{bmatrix} \\
\begin{bmatrix}b_x \\ b_y \\ b_z\end{bmatrix} &=&
\begin{bmatrix}
    m_x\cos\theta + m_y\sin\theta\sin\phi + m_z\sin\theta\cos\phi \\
    m_y\cos\phi - m_z\sin\phi \\
    -m_x\sin\theta + m_y\cos\theta\sin\phi + m_z\cos\theta\cos\phi
\end{bmatrix}
\end{array}
$

$
\begin{array}{rcl}
\psi &=& \mathrm{arctan2}(-b_y, b_x) \\
&=& \mathrm{arctan2}\big(m_z\sin\phi - m_y\cos\phi, \; m_x\cos\theta + \sin\theta(m_y\sin\phi + m_z\cos\phi)\big)
\end{array}
$

Combine into quaternion

$
\mathbf{q}_{am} =
\begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} =
\begin{pmatrix}
    \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) - \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) - \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big)
\end{pmatrix}
$



```python
m_θ = np.arctan2(a[:, 1], a[:, 2]) - np.pi
m_ϕ = np.arctan2(-a[:, 0], np.sqrt(a[:, 1]**2 + a[:, 2]**2))
m_ψ = np.zeros(n) #np.arctan2(ms[:, 2] * sin(m_ϕ) - ms[:, 1] * cos(m_ϕ), ms[:, 0] * cos(m_θ) + (ms[:, 1] * sin(m_ϕ) + ms[:, 2] * cos(m_ϕ)) * sin(m_θ))
```


```python
measured_angles = np.vstack((m_θ, m_ϕ, m_ψ)).T
q_am = quat.array.from_axis_angle(measured_angles)
```


```python
plt.plot(q_am.to_axis_angle)
```

## Filtering

$
\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}, \quad \alpha \in (0, 1)
$



```python
α = 0.4

q_filter = (1 - α) * q_gyro + α * q_am
```


```python
plt.plot(q_filter.to_axis_angle)
plt.ylim(-0.1, 0.4)
```


```python
α = 0.7

q_filter = (1 - α) * q_gyro + α * q_am
```


```python
plt.plot(q_filter.to_axis_angle)
plt.ylim(-0.1, 0.4)
```


```python

```

21a152ef1a666387806793cd09ab703a878b7569
ipynb
Jupyter Notebook
prototyping/01_dump_complementary/ComplementaryFilterQuaternion.ipynb
abishekvupputur/zephly
e3067076b124419c2e7f1faff777a1a02fd36061
Apache-2.0
2022-01-24T14:26:04.000Z
2022-02-07T05:04:13.000Z
prototyping/01_dump_complementary/ComplementaryFilterQuaternion.ipynb
abishekvupputur/zephly
e3067076b124419c2e7f1faff777a1a02fd36061
Apache-2.0
2022-02-19T09:57:06.000Z
2022-02-19T09:57:06.000Z
prototyping/01_dump_complementary/ComplementaryFilterQuaternion.ipynb
abishekvupputur/zephly
e3067076b124419c2e7f1faff777a1a02fd36061
Apache-2.0
2022-02-01T21:38:45.000Z
2022-03-11T19:44:02.000Z
Qwen/Qwen-72B
1. YES
2. YES