## 2 System
### Attitude Estimation
#### 2.3.3 Correction of Attitude Estimates Using an Acceleration Sensor  
Using the values obtained from the acceleration sensor, the absolute angles of the _Pitch_ and _Roll_ axes with respect to the world coordinates can be obtained. This absolute angle is used to correct the attitude estimation. Rotating the acceleration sensor value based on the attitude calculated from the angular velocity sensor should always result in \((a_{x},a_{y},a_{z})=(0,0,g)\). For example, if the sensor is rotated 90 degrees around the \(x\)-axis in world coordinates, then the sensor valueis \((a_{x},a_{y},a_{z})=(0,g,0)\). If we know that the current attitude is rotated 90 degrees around the \(x\)-axis, then rotating the sensor value by \(-90\) degrees around the \(x\)-axis in the opposite direction will result in \((a_{x},a_{y},a_{z})=(0,0,g)\). However, in reality the estimated attitude by the angular velocity sensor deviates slightly from \((0,0,g)\) because of a small error in the attitude estimated by the angular rate sensor. Letting \(\mathbf{G}=(G_{x},G_{y},G_{z})\) be the vector on which the error exists, it can be calculated as in Equation (22):  
\[\mathbf{G}=\mathbf{R}(\mathbf{q_{t}})\mathbf{a}. \tag{22}\]  
Then, by finding the quaternion to rotate from \(\mathbf{G}\) to \((0,0,g)\) and multiplying it by \(\mathbf{q_{t}}\) and rotating it, we can calculate the attitude corrected by the acceleration sensor. The quaternions that rotate from \(\mathbf{G}\) to \((0,0,g)\) can be calculated using inner and outer products. We define the reference \((0,0,g)\) vector as \(\mathbf{a_{0}}=(0,0,1)\), the rotation axis unit vector representing the direction of rotation in the quaternion as \(\mathbf{a_{n}}=(an,x,an,y,an,z)\), and the scalar representing the rotation angle as \(\mathbf{\theta_{a}}\). Then, \(\mathbf{a_{n}}\) and \(\mathbf{\theta_{a}}\) are as follows:  
\[\mathbf{a_{n}}=\frac{\mathbf{G}\times\mathbf{a_{0}}}{\left|\mathbf{G}\times\mathbf{a_{0}}\right|}, \tag{23}\]  
\[\theta_{a}=\arccos(\mathbf{G}\cdot\mathbf{a_{0}}). \tag{24}\]  
The corrected quaternion \(\mathbf{q_{a}}\) due to the acceleration can be calculated as in Equation (25). \[ \begin{bmatrix}q_{a,0}\\ q_{a,1}\\ q_{a,2}\\ q_{a,3}\end{bmatrix}  = \begin{bmatrix}\cos\frac{\theta_{a}}{2}\\ a_{n,x}\sin\frac{\theta_{a}}{2}\\ a_{n,y}\sin\frac{\theta_{a}}{2}\\ a_{n,z}\sin\frac{\theta_{a}}{2}\end{bmatrix} \] \[= \begin{bmatrix}\sqrt{\frac{G_{x}+1}{2}}\\ \frac{G_{y}}{\sqrt{2(G_{x}+1)}}\\ -\frac{G_{y}}{\sqrt{2(G_{z}+1)}}\\ 0\end{bmatrix}  \tag{25}\]  
As shown in Equation (25), the \(z\)-axis component is 0, i.e., only the corrections to the _Pitch_ and _Roll_ axes are calculated. If the obtained correction quaternion \(\mathbf{q_{a}}\) is used as it is, it will be affected by all the effects of the noise from the acceleration sensor. Thus, it is necessary to use linear interpolation with the non-rotating quaternion \(\mathbf{q_{I}}=(1,0,0,0)\). We define \(\alpha\) as the value of the correction strength of the correction quaternion and set a new correction quaternion \(\mathbf{q_{a}^{\prime}}\). \[\mathbf{q_{a}^{\prime}}= \begin{bmatrix}\alpha q_{a,0}+(1-\alpha)\\ \alpha q_{a,1}\\ \alpha q_{a,2}\\ \alpha q_{a,3}\end{bmatrix}  \tag{26}\]  
Although \(\alpha\) can be a fixed value, in this study the correction intensity is adjusted according to the magnitude of the vectors obtained from the acceleration sensor. If the sensor is not accelerating or decelerating, the magnitude \(\sqrt{a_{x}^{2}+a_{y}^{2}+a_{z}^{2}}\) of the acceleration vector obtained from the sensor should be equal to \(g\). Therefore, we define the correction strength \(\alpha\) as  
\[\alpha=\alpha_{0}\cdot f(e_{g}), \tag{27}\]where \(\alpha_{0}\) is the fixed correction intensity and \(f(e_{g})\) is a function of the error with respect to the acceleration of gravity. We define \(e_{g}\) and \(f(x)\) as follows. \[e_{g}=\frac{||||\mathbf{a}||-g|}{g} \tag{28}\]  
\[f(x)=\left\{ \begin{array}{ll}1&(x\leq e_{1})\\ \frac{e_{2}-x}{e_{2}-e_{1}}&(e_{1}<x\leq e_{2})\\ 0&(x\geq e_{2})\end{array} \right.