# A Real-Time BLE/PDR Integrated System by Using an Improved Robust Filter for Indoor Position
## 3 Materials and Methods
### An Improved Mahony Complementary Filter Based on the Motion States  
The attitude of the device can be described by Euler angle, rotation matrix, and quaternion methods. The Euler angle is the angle between the three axes of the carrier coordinate system (CCS) and geographic coordinate system (GCS) which include pitch, roll, and yaw. The pitch, roll, and yaw represent rotation around x, y, and z axes as shown in Figure 4 and are denoted by the symbol \(\theta\), \(\gamma\) and \(\psi\), respectively. The Euler angle method  
Figure 3: The principle of PDR. only needs three elements to store attitude information, which is simple, intuitive, and easy to understand, but it is easy to generate a universal joint deadlock phenomenon. Different from the Euler angle, the Euler angle describes the attitude of the device at a certain moment, while the rotation matrix describes the motion process of the device rotation. In a GCS defined by the North-East-Up (NEU), the rotation matrix \(C_{n}^{b}\) between the two systems can be defined in Equation (7):  
\[C_{n}^{b}=C_{n}^{\gamma}C_{n}^{\theta}C_{n}^{\psi}=\left[ \begin{array}{ccc} cos\gamma&0&sin\gamma\\ 0&1&0\\ -sin\gamma&0&cos\gamma\end{array} \right]\left[ \begin{array}{ccc}1&0&0\\ 0&cos\theta&sin\theta\\ 0&-sin\theta&cos\theta\end{array} \right]\left[ \begin{array}{ccc}cos\psi& sin\eta&0\\ -sin\psi&cos\psi&0\\ 0&0&1\end{array} \right] \tag{7}\]  
where \(C_{n}^{b}\) represents the rotation matrix from GCS to CCS, \(C_{n}^{\psi}\), \(C_{n}^{\theta}\), \(C_{n}^{\gamma}\) represent the corresponding matrices for yaw, pitch, and roll in order. As we know, the matrix \(C_{n}^{b}\) is the orthogonal matrix, so the rotation matrix \(C_{b}^{n}\) is the inverse matrix of \(C_{n}^{b}\), which represents the rotation matrix from CCS to GCS. The matrix \(C_{b}^{n}\) can be defined as following Equation (8):  
\[C_{b}^{n}=\left(C_{n}^{b}\right)^{T}=\left(C_{n}^{b}\right)^{-1}=\left[ \begin{array} []{ccc}cos\psi cos\gamma-sin\psi sin\theta sin\gamma&sin\psi cos\theta&cos\psi sin \gamma+sin\psi sin\theta cos\gamma\\ -sin\psi cos\gamma-cos\psi sin\theta sin\gamma&cos\psi cos\theta&-sin\psi sin \gamma+cos\psi sin\theta cos\gamma\\ -cos\theta sin\gamma&-sin\theta&cos\theta cos\gamma\end{array} \right] \tag{8}\]  
Because the rotation matrix will suffer universal joint deadlock phenomenon and need 9 parameters to store attitude information, the quaternion is the most commonly used  
Figure 4: The attitude angle of the smartphone. method to calculate the attitude. Assuming that the quaternion vector is \(Q=\left[q_{0},q_{1},q_{2},q_{3}\right]^{T}\), the attitude angles expressed in quaternions are as follows in Equation (9):  
\[\left\{ \begin{array}{c}\psi=arctan\frac{2q_{1}q_{2}-2q_{0}q_{3}}{1-2q_{1}^{2} -2q_{3}^{2}},\psi\in(0,2\pi)\\ \theta=\arcsin(-2q_{2}q_{3}-2q_{0}q_{1}),\theta\in(-\frac{\pi}{2},\frac{\pi}{ 2})\\ \gamma=arctan\frac{-2q_{1}q_{2}+2q_{0}q_{2}}{1-2q_{1}^{2}-2q_{2}^{2}},\gamma \in(-\pi,\pi)\end{array} \right. \tag{9}\]  
The Mahony complementary filter (MCF) [39] utilizes the gyroscope to calculate the attitude angle of the device and the accelerometer and magnetometer are used to complement the accumulated error. When a gyroscope raw data \(\omega=\left(\omega_{x},\omega_{y},\omega_{z}\right)^{T}\) was obtained, we can use the first-order Runger-Kutta method to obtain the quaternion update as shown in Equations (10) and (11):  
\[Q[t+\Delta t]=Q[t]+\Delta t\cdot\Omega_{w}[t]\cdot Q[t] \tag{10}\]  
\[\left( \begin{array}{c}q_{0}\\ q_{1}\\ q_{2}\\ q_{3}\end{array} \right)_{t+\Delta t}=\left( \begin{array}{c}q_{0}\\ q_{1}\\ q_{2}\\ q_{3}\end{array} \right)_{t}+\frac{\Delta t}{2}\cdot\left( \begin{array}{cccc}0 &-\omega_{x}&-\omega_{x}&-\omega_{x}\\ \omega_{x}&0&\omega_{z}&-\omega_{y}\\ \omega_{y}&-\omega_{z}&0&\omega_{x}\\ \omega_{z}&\omega_{y}&-\omega_{x}&0\end{array} \right)\cdot\left( \begin{array} []{c}q_{0}\\ q_{1}\\ q_{2}\\ q_{3}\end{array}