Accelerometer and Gyroscope sensor fusion using Extended Kalman Filter

Setup

Arduino Nano
MPU6050

Concept

With an accelerometer and gyroscope, we obtain 6 degrees of freedom. We can only find roll and pitch with them. For yaw, we use a magnetometer. But in this project we only cover for roll and pitch. We use NED coordinate as reference frame. To find roll and pitch we use accelerometer.

$$roll=arctan2(a_y, a_z)$$ $$pitch=-arctan2(a_x, \sqrt{a_y^2+a_z^2})$$

When the accelerometer detects acceleration due to movement or vibration, the stability of roll and pitch can be compromised. We utilize an Extended Kalman Filter to fuse angular velocity data from the gyroscope, enabling us to detect if there is actual angular displacement. Relying solely on angular velocity can lead to drift in roll and pitch, even though it reacts faster than roll and pitch data from the accelerometer.

$$\hat{x}_n = \begin{bmatrix} \hat{\phi}_n \\\ \hat{\theta}_n \end{bmatrix}, u_{n} = \begin{bmatrix} p_{n} \\\ q_{n} \\\ r_{n} \end{bmatrix}$$

where $\hat{\phi}_n$ is roll
$\hat{\theta}_n$ is pitch
$p_{n}$ is roll rate in body frame
$q_{n}$ is pitch rate in body frame
$r_{n}$ is yaw rate in body frame

Prediction step

$$\bar{x}_n = \hat{x}_{n-1} + \Delta{t} \times f(\hat{x}_{n-1}, u_{n})$$ $$f(\hat{x}_{n-1}, u_{n}) = \begin{bmatrix} \dot{\phi}_n \\\ \dot{\theta}_n \end{bmatrix} = \begin{bmatrix} 1 & sin(\hat{\phi}_{n-1})tan(\hat{\theta}_{n-1}) & cos(\hat{\phi}_{n-1})tan(\hat{\theta}_{n-1}) \\\ 0 & cos(\hat{\phi}_{n-1}) & -sin(\hat{\phi}_{n-1}) \end{bmatrix}u_{n}$$ $$F=\frac{\partial f(\hat{x}_{n-1}, u_{n})}{\partial \hat{x}_{n-1}}= \begin{bmatrix} \frac{\partial \dot{\phi}_n}{\partial \hat{\phi}_{n-1}} & \frac{\partial \dot{\phi}_n}{\partial \hat{\theta}_{n-1}} \\\ \frac{\partial \dot{\theta}_n}{\partial \hat{\phi}_{n-1}} & \frac{\partial \dot{\theta}_n}{\partial \hat{\theta}_{n-1}} \end{bmatrix}= \begin{bmatrix} q \cdot cos(\hat{\phi}_{n-1})tan(\hat{\theta}_{n-1})-r \cdot sin(\hat{\phi}_{n-1})tan(\hat{\theta}_{n-1}) & q \cdot sin(\hat{\phi}_{n-1})sec^2(\hat{\theta}_{n-1})+r \cdot cos(\hat{\phi}_{n-1})sec^2(\hat{\theta}_{n-1}) \\\ -q \cdot sin(\hat{\phi}_{n-1})-r \cdot cos(\hat{\phi}_{n-1}) & 0 \end{bmatrix}$$ $$\bar{P}_{n}=FP_{n-1}F^T+Q$$

Update step

$$h(\bar{x}_{n}, u_{n}) = \begin{bmatrix} \bar{a}_{x} \\\ \bar{a}_{y} \\\ \bar{a}_{z} \end{bmatrix} = g \cdot \begin{bmatrix} sin(\bar{\theta}_{n}) \\\ -cos(\bar{\theta_{n}})sin(\bar{\phi}_{n}) \\\ -cos(\bar{\theta_{n}})cos(\bar{\phi}_{n}) \end{bmatrix}$$

where $\bar{a}_{x}$ is predict acceleration in x axis
$\bar{a}_{y}$ is predict acceleration in y axis
$\bar{a}_{z}$ is predict acceleration in z axis

$$H = \frac{\partial h(\bar{x}_{n}, u_{n})}{\partial \bar{x}_{n}} = g \cdot \begin{bmatrix} 0 & cos(\bar{\theta_{n}}) \\\ -cos(\bar{\theta_{n}})cos(\bar{\phi}_{n}) & sin(\bar{\theta_{n}})sin(\bar{\phi}_{n}) \\\ cos(\bar{\theta_{n}})sin(\bar{\phi}_{n}) & sin(\bar{\theta_{n}})cos(\bar{\phi}_{n}) \end{bmatrix}$$ $$K=\bar{P}_{n}H(H\bar{P}_{n}H^{T}+R)^{-1}$$ $$\hat{x}_{n}=\bar{x}_{n}+K(y-h(\bar{x}_{n}, u_{n}))$$ $$P_{n}=(I-KH)\bar{P}_{n}$$