# Attitude estimation


```python
%matplotlib inline 
%config InlineBackend.figure_format = 'retina'

import numpy as np 
import matplotlib.pyplot as plt

from helpers import IMU, plot_compare
```

In this exercise you will use measurements from the IMU of the drone's attitude and body rates. The state and measurements are given by:

$$
\begin{align}
x_t= \begin{bmatrix} \theta \\ \phi \end{bmatrix} && z_t =\begin{bmatrix} \theta \\ \phi \\ q \\ p  \end{bmatrix}
\end{align}
$$

For this exercise, we will assume that the drone is close to the hover mode, so we can assume rotational rates in the body frame are the same as in the global frame. 

$$
\dot{\phi} = p \\
\dot{\theta} = q
$$

For larger angles, you have to use the transformation matrix which was covered in previous lessons.

The equations which describe the attitude update are as follows:

$$
\hat{\theta}_t = \frac{\tau}{\tau + dt}\left( \hat{\theta}_{t-1} +  z_{t,\dot{\theta}} \times dt\right) + \frac{dt}{\tau + dt} z_{t,\theta} \\ 
\hat{\phi}_t = \frac{\tau}{\tau + dt}\left( \hat{\phi}_{t-1} +  z_{t,\dot{\phi}} \times dt\right) + \frac{dt}{\tau + dt} z_{t,\phi}
$$


```python
class ComplementaryFilter:
    def __init__(self,dt,tau): 
        self.dt = dt
        self.tau = tau
        self.estimated_theta = 0.0
        self.estimated_phi   = 0.0
        
    def update(self,z):
        
        # TODO: 
        #  implement the estimated pitch and roll 
        
        self.estimated_theta = self.tau / (self.tau + self.dt) * (self.estimated_theta + z[2] * self.dt) \
                             + self.dt / (self.tau + self.dt) * z[0]
        self.estimated_phi = self.tau / (self.tau + self.dt) * (self.estimated_phi + z[3] * self.dt) \
                           + self.dt / (self.tau + self.dt) * z[1]
            
    def make_estimates(self,measurements):
        self.estimated_theta = 0.0
        self.estimated_phi = 0.0
        estimates = np.zeros((2, measurements.shape[1]))
        for i in range(measurements.shape[1]):
            z = measurements[:,i]
            self.update(z)
            est = np.array([self.estimated_theta, self.estimated_phi])
            estimates[:,i] = est
        return estimates
```


```python
# drone hovers for all N measurements 
N = 2000 
true_values = np.zeros((4, N))

# simulate sensor measurements
imu = IMU()
measurements = imu.make_measurements(true_values)
```


```python
# Parameters for complementary filter
# 
# TODO - try modifying TAU. 
#   What value gives you the best estimate?
#   What do your plots look like when TAU = DT * 100?
#   What about when TAU = DT * 1? 

DT  = 0.01
TAU = DT * 1
cf  = ComplementaryFilter(DT, TAU)

# make estimates based on measurements
estimates = cf.make_estimates(measurements)

# integrating gyro directly
integrated_ests = np.zeros((2,N))
integrated_ests[0,:] = np.cumsum(measurements[2,:]) * DT
integrated_ests[1,:] = np.cumsum(measurements[3,:]) * DT
```


```python
# plot pitch
plot_compare(true_values,estimates,measurements,integrated_ests, DT, 0)
```


```python
# plot roll
plot_compare(true_values,estimates,measurements,integrated_ests, DT, 1)
```

[Solution](./Attitude%20estimation-Solution-Revised.ipynb)

ffe0c8de583e2450985e136926ac93a568fd01c0
ipynb
Jupyter Notebook
Course04/19-8_Attitude estimation/Attitude estimation.ipynb
thhuang/notes-fcnd
c5b0ec7d99df3cb60a850308d16ccc6c096c7931
MIT
2021-12-14T09:34:36.000Z
2021-12-14T09:34:36.000Z
Course04/19-8_Attitude estimation/Attitude estimation.ipynb
thhuang/notes-fcnd
c5b0ec7d99df3cb60a850308d16ccc6c096c7931
MIT
Course04/19-8_Attitude estimation/Attitude estimation.ipynb
thhuang/notes-fcnd
c5b0ec7d99df3cb60a850308d16ccc6c096c7931
MIT
2018-10-26T04:06:23.000Z
2018-10-26T04:06:23.000Z
Qwen/Qwen-72B
1. YES
2. YES