# An INS-UWB Based Collision Avoidance System for AGV
## 3 Collision Avoidance System Design
### INS and UWB Integrated Positioning Method  
In the UWB distance estimation, the obstacle between the UWB transmitter and receiver has to avoid the line-of-sight propagation of the UWB signal. The receiver could only get the signal through diffraction, refraction and reflection, which will generate NLOS range estimation errors. The estimated distance in NLOS conditions is inaccurate, which makes the distance a biased estimate. The UWB distance estimating data shall be processed before being used in the positioning method. EKF (extend Kalman filter) which integrates INS and UWB is adopted in this paper to deal with the errors in the range estimation and positioning of the AGV.  
The Kalman filter adopted in this paper processes the UWB ranging distance data. Both the state and observation equations are as follows:  
\[X(k+1)=\Theta X(k)+\Gamma u(k) \tag{7}\]  
\[Z(k)=HX(k)+\
u(k) \tag{8}\]  
\[\Theta=\left[\begin{array}{ccc}1&T&0&0\\ 0&1&0&0\\ 0&0&1&T\\ 0&0&0&1\end{array}\right];\Gamma=\left[\begin{array}{ccc}\frac{T^{2}}{2}&0 \\ T&0\\ 0&\frac{T^{2}}{2}\\ 0&T\end{array}\right];H=\left[\begin{array}{ccc}\frac{1}{2}&0\\ 0&0\\ 0&0\\ 0&1\end{array}\right];X=\left[\begin{array}{c}x\\ x\\ y\\ y\end{array}\right];Z=\left[\begin{array}{c}x\\ y\end{array}\right], \tag{9}\]  
where \(u\),\(\
u\) are the observation and process noise, respectively, and \(T\) is the sampling period.  
Figure 4: Possible error conditions.  
The mean square error of the next step prediction is:  
\[\widehat{X}_{k|k-1}=\Theta_{k,k-1}\widehat{X}_{k-1} \tag{10}\]  
and the state estimation is:  
\[\widehat{X}_{k}=\widehat{X}_{k|k-1}+K_{k}(Z_{k}-H_{k}\widehat{X}_{k|k-1}), \tag{11}\]  
\[K_{k}=P_{k|k-1}H_{k}^{T}(H_{k}P_{k|k-1}H_{k}^{T}+R_{k})^{-1}. \tag{12}\]  
The mean square error of next step prediction is:  
\[P_{k|k-1}=\Theta_{k|k-1}P_{k-1}\Theta_{k|k-1}^{T}+\Gamma_{k-1}Q_{k}\Gamma_{k}^ {T} \tag{13}\]  
and the estimated mean square error is:  
\[P_{k}=(I-K_{k}H_{k})P_{k|k-1}. \tag{14}\]  
In the proof of Kalman filtering, given the initial system state error \(\widehat{X}_{0}\) and initial state error variance matrix \(\widehat{P}_{0}\), the predicted ranging data \(\widehat{X}_{k}\)\((k=1,2,3,...)\) of time \(k\).  
The gyroscope is one of the key devices in INS localization and the main error is due to the gyroscope drifting. In a short time, the white and quantizing noises cause an important effect on the angular error. The modeling of the gyroscope mainly includes: constant and random errors and the model of the gyroscope is:  
\[\varepsilon=\varepsilon_{e}+\varepsilon_{r}+\omega_{g}. \tag{15}\]  
In the design of the integrated localization method, the biased error \(\Delta_{i}\)\((i=x,y,z)\) is considered in the error model of the accelerometer:  
\[\Delta_{i}=\Delta_{bi}+\omega_{ai}\quad(i=x,y,z), \tag{16}\]  
where \(\Delta_{bi}\) is the constant error and \(\omega_{ai}\) is the white noise.  
The error of the INS localization includes the error of the device, which consist of the scale and the installation errors.  
The principle of the EKF is to linearize the system, transfer the system model to state equation, measure the equation error and predict the state error with the standard Kalman filter.  
Assume that the nonlinear observation model is:  
\[Z(t)=h(X(t))+V(t), \tag{17}\]  
where \(h(x)\) is the nonlinear function of the state vector.  
Then, the discrete form is:  
\[\delta Z_{k}=h(X_{k})-h(\widehat{X}_{k})=H_{k}\delta X_{k}+V_{k}, \tag{18}\]  
\[H_{k}=\frac{\partial h}{\partial X},\quad X=\widehat{X}_{k|k-1}. \tag{19}\]  
After linearization, standard Kalman filtering is used to calculate both the error state and the observation models.