# NASA problem notes

## Description
The flight demonstration dataset contains the IMU (`dlc.csv`), Lidar (`commercial_lidar.csv`) and truth (`truth.csv`) data. We attempts to estimate the vehicle position, velocity and attitude by using IMU and Lidar data. The Kalman filter is selected as a core algorithm to do that. The estimation data will be compared with the truth data to check the Kalman filter performance.

**This note only contains IMU's part only for now.**
## The IMU definition
Since the navigation and control algorithm was designed based on the Vehicle frame, in this note we called it Center Of Navigation frame(CON) but the data sensed by IMU are on the IMU frame. Therefore we need to know the IMU attitude or the direct cosine matrix (DCM) and IMU's location on CON frame. These data can be found in `README.txt`
$$DCM_{IMU}^{CON} = \begin{bmatrix} -0.2477 & -0.1673 & 0.9543 \\ -0.0478 & 0.9859 & 0.1604 \\ -0.9677 & -0.0059 & -0.2522 \end{bmatrix}$$$$P_{IMU}^{CON} = \begin{bmatrix}
-0.08035 & 0.28390 & -1.42333
\end{bmatrix}$$
## Vehicle dynamics
Kalman filter needs vehicle dynamics model to construct the state transition equations. We will write the dynamics equations on ECEF frame because the truth data is on ECEF frame.
### Acceleration
Since IMU is not mounted at the navigation frame's origin, any vehicle rotation will induce extra acceleration which is centrifugal and tangential term. We need to compensate for this lever arm effect to strip those term from the IMU raw data.
$$a_{centrifugal}^{ECEF} = \omega_{vehicle}^{ECEF} \times (\omega_{vehicle}^{ECEF} \times (DCM^{ECEF}_{CON} \cdot P_{IMU}^{CON}))$$
$$a_{tangential}^{ECEF} = \dot{\omega}_{vehicle}^{ECEF} \times (DCM^{ECEF}_{CON} \cdot P_{IMU}^{CON})$$
Because IMU also sense the centrifugal and tangential term induced by earth rotation, but the truth data from `truth.csv` is on ECEF frame and these terms are not exist on ECEF frame. Therefore we need to strip them from the IMU raw data too.

$$a_{earth\_centrifugal}^{ECEF} = \omega_{earth}^{ECEF} \times (\omega_{earth}^{ECEF} \times P_{vehicle}^{ECEF})$$
$$a_{earth\_tangential}^{ECEF} = \dot{\omega}_{earth}^{ECEF} \times P_{vehicle}^{ECEF}$$
$$a_{coriolis}^{ECEF} = 2\omega_{earth}^{ECEF} \times v_{vehicle}^{ECEF}$$
The final acceleration equation as the following

$$\begin{aligned}
a_{vehicle}^{ECEF} = DCM_{CON}^{ECEF} \cdot DCM_{IMU}^{CON} \cdot a_{vehicle}^{IMU} - a_{centrifugal}^{ECEF} \\ - a_{tagential}^{ECEF} - a_{earth\_centrifugal}^{ECEF} - a_{earth\_tangential}^{ECEF} - a_{coriolis}^{ECEF}
\end{aligned}$$

## State transition