## 2 Proposed Position Tracking System Based on Composite Sensors
### EKT-Based MEMS IMU and UWB Integrated Positioning Algorithm
\right] \tag{25}\]  
\((\vartheta_{xp},\vartheta_{yp})\) are \((\int_{0}^{t}a^{N}_{x}(\tau)d\tau,\int_{0}^{t}a^{N}_{y}(\tau)d\tau)\) and \((\hat{x}_{xp},\hat{x}_{yp})=(\int_{0}^{t}\hat{\sigma}_{xp}(\tau)d\tau,\int_{0 }^{t}\hat{\sigma}_{yp}(\tau)d\tau)\). \(\psi=\int_{0}^{t}\hat{\omega}_{z}(\tau)d\tau\). \(\Big{(}a^{N}_{x},a^{N}_{y}\Big{)}\) are derived from Equation (2). \[\left[ \begin{array}{c}a^{N}_{x}\\ a^{N}_{y}\end{array} \right]=\left[ \begin{array}{cc}\cos(\psi)&-\sin(\psi) \\ \sin(\psi)&\cos(\psi)\end{array} \right]\left[ \begin{array}{c}a^{B}_{x}- \hat{b}_{x}\\ a^{B}_{y}-\hat{b}_{y}\end{array} \right]-\left[ \begin{array}{c}\hat{v}_{xp}\\ -\hat{\omega}_{z}\hat{v}_{yp}\end{array} \right] \tag{26}\]  
\(\hat{\omega}_{z}\) is the error of the angle velocity to the bias on the z axis; \(\hat{\omega}_{z}=\omega_{z}-\hat{b}_{z}\). The range measurement \(\mathbf{z}_{z}\) and estimated rand measurement \(\hat{\mathbf{z}}_{z}\) for 3 anchors of UWB can be defined by the Euclidean distance of anchors and a target tag platform including an anchor height \(h\) and the measurement noises \(\mathbf{n}=[n_{1},n_{2},n_{3}]^{T}\) as follows:  
\[\mathbf{z}_{z}(t)=\mathbf{z}+\mathbf{n}\text{ for }\mathbf{z}=[z_{1},z_{2},z_{3}]^{T} \tag{27}\]  
\[\hat{\mathbf{z}}_{z}(t)=\hat{\mathbf{z}}+\mathbf{n}\text{ for }\hat{\mathbf{z}}=[ \hat{z}_{1},\hat{z}_{1},\hat{z}_{1}]^{T} \tag{28}\]  
where  
\[\left[ \begin{array}{c}z_{i}\\ \hat{z}_{i}\end{array} \right]=\left[ \begin{array}{c}\sqrt{\left(x_{i}-x_{p} (t)\right)^{2}+\left(y_{i}-y_{p}(t)\right)^{2}+h^{2}}\\ \sqrt{\left(x_{i}-\hat{x}_{p}(t)\right)^{2}+\left(y_{i}-\hat{y}_{p}(t)\right)^ {2}+h^{2}}\end{array} \right]+n_{i} \tag{29}\]  
The extended Kalman filter residual \(\boldsymbol{\mu}\), the error between two range measurements \(\mathbf{z}_{z}(t)\), \(\hat{\mathbf{z}}_{z}(t)\), can be approximated by the gradient matrix \(H_{z}=\frac{\partial\delta(\mathbf{x})}{\partial\mathbf{x}}\) and the state variables \(\mathbf{x}_{z}\) as follows:  
\[\boldsymbol{\mu}=\mathbf{z}_{z}(t)-\hat{\mathbf{z}}_{z}(t)\approx[H_{z}] \mathbf{x}_{z}+n \tag{30}\]  
The position and velocity of platform are estimated from the platform and navigation state variable \(\mathbf{x}_{z}\) from IMU sensors and the range measurements \(\mathbf{z}_{z}\) of 3 anchors of UWB sensors, as shown in Figure 3.  
Let us take a closer look at the positioning process by EKT (Extended Kalman filter) that linearizes the state equation, as shown in Figure 4. EKT assumes the differentiability of a state transition function instead of the model linearity. There is a closed-form expression that the estimated state \(\hat{x}_{k+1}\) can be computed by a nonlinear function \(f\) of the previous state \(\hat{x}_{k}\), control \(u_{k}\), noise \(w_{k}\), and time \(t\). \[\hat{x}_{k+1}=f(\hat{x}_{k},u_{k},\ t)+w_{k} \tag{31}\]  
The Jacobians of the predicted state to the previous state and the noise take simple forms when the noise goes linearly into the state update equation. In EKT, the measurement \(\hat{\mathbf{z}}_{k}\) can be computed by a nonlinear function \(h\) of the state \(x_{k}\) and the measurement noise \(v_{k}\). \[\hat{z}_{k+1}=h(\hat{x}_{k})+v_{k} \tag{32}\]  
Here, \(f\), \(h\) are differentiable. In a sensor module, the position data \(z_{k}\) measured at \(t\) time contain noise components, which interfere with signal analysis. Therefore, to predict the estimated value \(\hat{x}_{k}\) for which noise components have been removed, the near-optimal Kalman gain \(K_{k}\) is computed to predict \(\hat{x}_{k}\) in the EKT. \[K_{k}=P_{k|k-1}H_{k}^{T}S_{k}^{-1} \tag{33}\]  
\(S_{k}\) is the residual covariance; \(S_{k}=H_{k}P_{k|k-1}H_{k}^{T}+R_{k}\). The updated state estimate \(\hat{x}_{k|k}\) and covariance estimate can be expressed as  
\[\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_{k}\widetilde{y}_{k}, \tag{34}\]  
\[P_{k|k}=(\mathrm{I}-K_{k}H_{k})P_{k|k-1}. \tag{35}\]  
where \(\widetilde{y}_{k}\) is the measurement residual; \(\widetilde{y}_{k}=z_{k}-h\left(\hat{x}_{k|k-1}\right)\).